一种无人机曲率连续可调路径规划方法
【专利说明】一种无人机曲率连续可调路径规划方法
[0001]
技术领域
[0002]本发明属于无人机控制技术领域，尤其是涉及一种无人机曲率连续可调路径规划方法。
[0003]
【背景技术】
[0004]随着智能控制与无人技术的发展，无人机(Unmanned Aerial Vehicle)在物流运输、农作物监测、灾难救援以及目标侦查等民用领域和军事领域中发挥着越来越重要的应用价值。路径规划是无人机快速高效执行任务的基础。路径规划的目的是提供一条满足无人机自身运动学要求的优化路径。
[0005]路径规划分为逻辑路径规划与物理路径规划问题。逻辑路径规划是在给定的区间上找到代价最小的起始点与终止点连接方式。物理路径规划的目的是给出满足物理限制条件(无人机运动力学限制)的可行路径，并且保证路径最优:实时性、局部可调整性、精确性。
[0006]典型的逻辑路径规划算法有:人工势场法(Artificial Potential Field)、TSP (Traveling Salesman Problem)、MTSP (Multiple Traveling Salesman Problem)、Dijkstra、A*、遗传算法(Genetic Algorithm)、SOM (Self Organizing Mapping)等。但是这些算法没有考虑物理可行性。
[0007]主流的物理路径规划算法有:DC (Dubins Curve)曲线、Bezier曲线、B-Spline、PH (Pythagorean Hodograph)曲线等。这些算法没有同时考虑路径曲率连续、曲率大小限制以及经过所有路标点的要求。DC曲线利用直线与圆弧连接平面上两点，保证路径最短，但是连接点处曲率间断Aezier与B-Spline为近似曲线，曲线逼近控制点，但是不经过所有控制点；PH曲线曲率连续并且可以根据曲率极值反演曲线参数，但是该曲线仍属于近似曲线，并且计算复杂度高，不适合实时路径规划。
[0008]
【发明内容】
[0009]本发明的目的是提供一种曲率连续可调路径规划方法，以解决现有技术中存在的路径无法满足无人机运动学条件、路径缺乏精确度以及算法复杂度高的问题。
[0010]本发明所采用的技术方案是:一种无人机曲率连续可调路径规划方法，其特征在于，包括以下步骤:
步骤1:初始化无人机任务地图，标注各任务点的地理坐标，设定无人机的最小转弯半径；
步骤2:设定分段Catmull-Rom曲线的形状参数分别为1、0 ;采用几何算法连接各任务点，得到规划路径；曲率间断点只存在于分段Catmull-Rom曲线的连接点处，计算各连接点是否满足曲率连续条件，得到曲率间断点Node[i], i=l、2、3...i ;
步骤3:标注曲率间断点Node[i], i=l、2、3...1，在原任务点分布情况下，插入新的路标点或者移动关键路标点，完成所有连接点的曲率连续化，以达到路径曲率连续，得到新路径；
步骤4:计算步骤3中得到的新路径曲线曲率值，记录曲率值超过最大曲率限制的曲率线段OverCur [j],计算对应路径曲线起始点为Point_sThod[j]、Point_fThod[j]；
步骤5:采用最小曲率圆过渡方法，利用曲率单调变化的Bezier曲线连接各曲率线段OverCur [j]的起始点与终点，完成曲率路径曲线重规划。
[0011]作为优选，步骤3中所述的在原任务点分布情况下插入新的路标点或者移动关键路标点，采用的是最优插值法，其具体实现包括以下子步骤:
步骤3.1:原始路径点数目记为data_len，根据曲率突变大小，决定插入点的位置，记该位置在原任务点中的序号为ctlflag ；
步骤3.2:根据ctlflag的值，分析曲线长度受该插入点影响的曲线段；其具体分析过程包括以下子步骤:
步骤3.2.1:计算处于该点之前的长度受影响曲线段数seg_ahead ;若ctlflag > 5，则段数为3;否则段数为mod(ctlflag+l，3);
步骤3.2.2:计算处于该点之后的长度受影响曲线段数seg_after ;若len-ctrflag 多 7，其中 len-ctrflag=data_len_ctlflag，则段数为 3 ;否则段数为len-ctlflag-4；
步骤3.2.3:曲线长度受该插入点影响的曲线段总段数为seg_ahead+seg_after ；
步骤3.3:根据ctlflag的值，分析曲率值受该插入点影响的连接点个数；其具体分析过程包括以下子步骤:
步骤3.3.1:计算插入点前面曲率值受影响的连接点的个数Cur_ahead=seg_ahead_l ； 步骤3.3.2:计算插入点后面曲率值受影响的连接点的个数为:Cur_after=seg_after
-1 ;
步骤3.3.3:曲率值受该插入点影响的连接点个数为Cur_ahead+Cur_after+1 ；
步骤3.4:利用遗传算法，优化需要插入点的位置，该位置保证受影响的曲率值连续，并且优化受影响的曲线长度值；
步骤3.5:重复上述步骤，完成所有连接点的曲率连续化。
[0012]作为优选，步骤5中所述的最小曲率圆过渡方法，其具体实现过程包以下子步骤: 步骤5.1:分别计算Point_sThod[j]、Point_fThod[j]处曲率圆的圆心坐标Cen_s[j]、
Cen_f[j];其中曲率圆半径为无人机最小转弯半径；
步骤5.2:以Point_sThod[j]为原点，建立局部坐标系，将Cen_f [j]转化为局部坐标；步骤5.3:本方法提出的曲率圆过渡法包括C、S、C-C、C-S、S-S、S-C曲线，其中C、C-C、S-S曲线保证起始点与终点绕旋方向不变；S、C-S、S-C曲线使终点与起始点绕旋方向相反；根据以上结论，选择转移曲线类型，计算最小曲率圆过渡法两个输入参数:形状控制参数mCm彡I)、Bezier结构化参数theta，m由Cen_s [j]、Cen_f [j]的相对位置唯一决定，theta由m与曲率单调条件唯一决定，连接其起始点和终点；
步骤5.4:重复以上步骤，完成曲率路径曲线重规划。
[0013]本发明与现有技术相比，提出了一种经过所有路标点的平滑路径规划方法，采用参数化的Catmull-Rom曲线，确保路径经过所有路标点；通过优化插值与关键点转移算法，保证路径各点曲率连续；利用曲率单调平滑连接算法，控制路径曲率极值范围，使路径在满足无人机运动学条件前提下，具有精确性和可行性。
[0014]
【附图说明】
[0015]图1:为本发明实施例的方法流程图；
图2:为本发明实施例最优插值不意图；
图3:为本发明实施例最优插值方法流程图；
图4:为本发明实施例最小曲率过渡圆连接示意图；
图5:为本发明实施例最小曲率圆过渡方法流程图；
图6:为本发明实施例具体实施例示意图。
[0016]
【具体实施方式】
[0017]为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。
[0018]请见图1，本发明提供的一种无人机曲率连续可调路径规划方法，包括以下步骤: 步骤1:初始化无人机任务地图，标注各任务点的地理坐标，设定无人机的最小转弯半径；
初始化地图，设定无人机在该次任务中的目标点序列，根据实际地形，设定无人机飞行高度；在任务执行过程中，无人机的速度与飞行高度不变。
[0019]步骤2:设定分段Catmull-Rom曲线的形状参数分别为1、0 ;采用几何算法连接各任务点，得到规划路径；曲率间断点只存在于分段Catmull-Rom曲线的连接点处，计算各连接点是否满足曲率连续条件，得到曲率间断点Node[i], i=l、2、3...1 ；
步骤3:标注曲率间断点Node[i], i=l、2、3...1，在原任务点分布情况下，插入新的路标点或者移动关键路标点，完成所有连接点的曲率连续化，以达到路径曲率连续，得到新路径；
在原任务点分布情况下插入新的路标点或者移动关键路标点，采用的是最优插值法，请见图2和图3，其具体实现包括以下子步骤:
步骤3.1:原始路径点数目记为data_len，根据曲率突变大小，决定插入点的位置，记该位置在原任务点中的序号为ctlflag ；
步骤3.2:根据ctlflag的值，分析曲