[发明专利]一种无人机曲率连续可调路径规划方法

说明书

技术领域

本发明属于无人机控制技术领域，尤其是涉及一种无人机曲率连续可调路径规划方法。

背景技术

随着智能控制与无人技术的发展，无人机（Unmanned Aerial Vehicle）在物流运输、农作物监测、灾难救援以及目标侦查等民用领域和军事领域中发挥着越来越重要的应用价值。路径规划是无人机快速高效执行任务的基础。路径规划的目的是提供一条满足无人机自身运动学要求的优化路径。

路径规划分为逻辑路径规划与物理路径规划问题。逻辑路径规划是在给定的区间上找到代价最小的起始点与终止点连接方式。物理路径规划的目的是给出满足物理限制条件（无人机运动力学限制）的可行路径，并且保证路径最优：实时性、局部可调整性、精确性。

典型的逻辑路径规划算法有：人工势场法（Artificial Potential Field）、TSP（Traveling Salesman Problem）、MTSP（Multiple Traveling Salesman Problem）、Dijkstra、A^*、遗传算法（Genetic Algorithm）、SOM（Self Organizing Mapping）等。但是这些算法没有考虑物理可行性。

主流的物理路径规划算法有：DC（Dubins Curve）曲线、Bezier 曲线、B-Spline、PH（Pythagorean Hodograph）曲线等。这些算法没有同时考虑路径曲率连续、曲率大小限制以及经过所有路标点的要求。DC曲线利用直线与圆弧连接平面上两点，保证路径最短，但是连接点处曲率间断；Bezier与B-Spline为近似曲线，曲线逼近控制点，但是不经过所有控制点；PH曲线曲率连续并且可以根据曲率极值反演曲线参数，但是该曲线仍属于近似曲线，并且计算复杂度高，不适合实时路径规划。

发明内容

本发明的目的是提供一种曲率连续可调路径规划方法，以解决现有技术中存在的路径无法满足无人机运动学条件、路径缺乏精确度以及算法复杂度高的问题。

本发明所采用的技术方案是：一种无人机曲率连续可调路径规划方法，其特征在于，包括以下步骤：

步骤1：初始化无人机任务地图，标注各任务点的地理坐标，设定无人机的最小转弯半径；

步骤2：设定分段Catmull-Rom曲线的形状参数分别为1、0；采用几何算法连接各任务点，得到规划路径；曲率间断点只存在于分段Catmull-Rom曲线的连接点处，计算各连接点是否满足曲率连续条件，得到曲率间断点Node[i],i=1、2、3...i；

步骤3：标注曲率间断点Node[i],i=1、2、3...i，在原任务点分布情况下，插入新的路标点或者移动关键路标点，完成所有连接点的曲率连续化，以达到路径曲率连续，得到新路径；

步骤4：计算步骤3中得到的新路径曲线曲率值，记录曲率值超过最大曲率限制的曲率线段OverCur[j],计算对应路径曲线起始点为Point_sThod[j]、Point_fThod[j]；

步骤5：采用最小曲率圆过渡方法，利用曲率单调变化的Bezier曲线连接各曲率线段OverCur[j]的起始点与终点，完成曲率路径曲线重规划。

作为优选，步骤3中所述的在原任务点分布情况下插入新的路标点或者移动关键路标点，采用的是最优插值法，其具体实现包括以下子步骤：

步骤3.1：原始路径点数目记为data_len，根据曲率突变大小，决定插入点的位置，记该位置在原任务点中的序号为ctlflag；

步骤3.2：根据ctlflag的值，分析曲线长度受该插入点影响的曲线段；其具体分析过程包括以下子步骤：

步骤3.2.1：计算处于该点之前的长度受影响曲线段数seg_ahead；若ctlflag＞5，则段数为3；否则段数为mod(ctlflag+1，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的值，分析曲率值受该插入点影响的连接点个数；其具体分析过程包括以下子步骤：