一种快速无人机路径重构方法
阅读说明：本技术 一种快速无人机路径重构方法 (Rapid unmanned aerial vehicle path reconstruction method ) 是由 郑多 裴培 林德福 宋韬 李斌 范世鹏 于 2020-07-14 设计创作，主要内容包括：本发明公开了一种快速无人机路径重构方法,该方法在探测到路径中存在障碍物时,将障碍物模拟成椭圆形,利用凸优化在求解优化问题的全局优化和快速求解的特性,快速重构出可以规避椭圆形障碍物的新的规划路径,为了确保新的规划路径的准确与可靠性,再通过迭代的方式进一步修正该规划路径,直至基本达到稳态为止。(The invention discloses a rapid unmanned aerial vehicle path reconstruction method, which is characterized in that when an obstacle exists in a path, the obstacle is simulated into an ellipse, a new planned path capable of avoiding the elliptical obstacle is rapidly reconstructed by utilizing the characteristics of global optimization and rapid solution of convex optimization in solving an optimization problem, and the planned path is further modified in an iteration mode until a steady state is basically achieved in order to ensure the accuracy and reliability of the new planned path.)
技术领域
本发明涉及无人机的自动路径规划，具体涉及一种快速无人机路径重构方法。
背景技术
无人机在按照预先设定的航迹飞行过程中，由于环境变化，可能会突然出现障碍物，无人机需要在飞行过程中对预先设定的航迹进行重构，按照新的路径进行飞行作业。
但是现有技术中，重新规划路径需要的信息很多，解算过程复杂，解算耗时较长，难以实时提供新的规划路径；而且自动规划的路径受到障碍物外形尺寸的影响较大，往往需要绕更远的路，消耗更多的时间，在实际工作过程中，一般都是通过人工控制的方式来避开障碍物，自动化程度较低。
由于上述原因，本发明人对现有的无人机路径规划方法做了深入研究，以期待设计出一种能够解决上述问题的新的路径规划方法。
发明内容
为了克服上述问题，本发明人进行了锐意研究，设计出一种快速无人机路径重构方法，该方法在探测到路径中存在障碍物时，将障碍物模拟成椭圆形，利用凸优化在求解优化问题的全局优化和快速求解的特性，快速重构出可以规避椭圆形障碍物的新的规划路径，为了确保新的规划路径的准确与可靠性，再通过迭代的方式进一步修正该规划路径，直至基本达到稳态为止，从而完成本发明。
具体来说，本发明的目的在于提供以一种快速无人机路径重构方法，该方法包括如下步骤：
步骤1，通过安装在无人机上的探测器实时探测无人机原始规划路径中是否存在障碍物；
步骤2，在发现原始规划路径中存在障碍物时，探测获知障碍物的圆心位置、障碍物的长轴半径和短轴半径。
步骤3，通过重构获得新的规划路径。
其中，在所述步骤3中，待获得的新的规划路径(xv[i]，yv[i])满足下式(一)和式(二)中的约束：
且所述待获得的新的规划路径(xv[i]，yv[i])能够使得目标函数取最小值，所述取最小值的目标函数如下式(三)中所述：
其中，χc[i]表示cos(ψv)[i]，χs[i]表示sin(ψv)[i]，v表示无人机当前的速度，χa[i]表示amax表示无人机最大允许的加速度，ψvmax表示无人机最大允许的飞行方位角，
xv[i]表示新的规划路径中离散点i处的X轴坐标，yv[i]表示新的规划路径中离散点i处的Y轴坐标，
xv[i+1]表示新的规划路径中离散点i+1处的X轴坐标，yv[i+1]表示新的规划路径中离散点i+1处的Y轴坐标，χs[i+1]表示新的规划路径中离散点i+1处的速度方位角正弦值；
td表示离散的时间步长；
表示原始规划路径中第N个离散点处的Y轴坐标；
A1、A2和A3分别表示轨迹规划计算的中间变量。
其中，A1、A2和A3通过下式获得：
其中，表示原始规划路径中离散点i处的X轴坐标，表示原始规划路径中离散点i处的Y轴坐标；
xn表示第n个障碍物的圆心的X轴坐标，yn表示第n个障碍区圆心的Y轴坐标；
an表示第n个障碍物的长半轴，bn表示第n个障碍物的短半轴。
其中，rn通过下式获得：
其中，在步骤3中，在得到新的规划路径后，对得到的新的规划路径进行迭代，以便于进一步优化规划路径，得到最终的规划路径。
其中，所述迭代过程包括如下子步骤：
子步骤1，根据得到的新的规划路径解算出中间变量A1′、A2′和A3′；
子步骤2，将中间变量A1′、A2′和A3′代入到约束式(五)中，
通过使得待获得的新的规划路径(xv[i]，yv[i])满足约束式(一)和式(五)，且所述待获得的新的规划路径(xv[i]，yv[i])能够使得目标函数J取最小值，得到该待获得的新的规划路径；
所述取最小值的目标函数如下式(三)中所述：
子步骤3，重复子步骤1和子步骤2，实时解算相邻两条规划路径之间的偏差，以确定迭代是否终止。
其中，在子步骤3中，若相邻两条规划路径之间的偏差值不小于设定值ε，利用最后一次迭代得到的新的规划路径解算中间变量A1′、A2′和A3′，即继续重复子步骤1；若相邻两条规划路径之间的偏差值小于设定值ε，即下式(四)成立时，停止迭代，最后一次迭代得到的新的规划路径即为最终的规划路径；
||Yk-Yk-1||＜ε (四)
其中，k表示迭代次数，Yk表示第k次迭代得到的规划路径，
优选地，ε的取值为
其中，中间变量A1′、A2′和A3′通过下式获得：
中间变量rn′通过下式获得：
其中，表示上一次迭代得到的规划路径中离散点i处的X轴坐标，表示上一次迭代得到的规划路径中离散点i处的Y轴坐标。
本发明所具有的有益效果包括：
根据本发明提供的快速无人机路径重构方法中的计算过程简单，计算迅速，可以在1秒以内实现求解，得到新的规划路径，从而可以实现实时规划控制。
附图说明
图1示出根据本发明一种快速无人机路径重构方法整体逻辑流程图；
图2示出根据本发明实验例中的规划路径及障碍物示意图。
具体实施方式
下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
根据本发明提供的快速无人机路径重构方法，如图1中所示，该方法包括如下步骤：
步骤1，通过安装在无人机上的探测器实时探测无人机原始规划路径中是否存在障碍物；
步骤2，在发现原始规划路径中存在障碍物时，探测获知障碍物的圆心位置、障碍物的长轴半径和短轴半径，若未发现障碍物，控制飞行器继续沿着原始规划路径飞行，直至到达目标位置；
步骤3，通过重构获得新的规划路径。
优选地，所述探测器具体包括雷达探测器，红外感应探测器，超声波探测器，激光距离感应器，双目视觉探测器中的一种或多种。
在一个优选的实施方式中，在步骤1中，无人机在起飞之前其内部已经装载了地图和飞行路径，其在飞行过程中实时通过探测器对环境进行建模，在地图的绝对坐标系中观察建模的障碍物是否在飞行的路径上，如果障碍物在飞行路径上，则重新进行路径规划。
在一个优选的实施方式中，在步骤2中，通过雷达探测障碍的位置和方向，通过双摄像头组成双目视觉探系统来探测距离，或者通过激光距离传感器探测距离，通过摄像头拍摄障碍物的照片，并分析障碍物的轮廓，再按照最大轮廓将障碍物圆整为椭圆形，进而获得障碍物的圆心位置、长轴半径和短轴半径，在圆整的过程中，确保障碍物完全落入到椭圆内即可。
本申请中涉及到的路径都是指二维平面上的路径，对应的坐标点及障碍物也都是特指二维平面上的坐标点及障碍物。
本申请中，每条规划路径都被离散为N个点，所述N的取值与路径的长短有关，一般为100～500。
在一个优选的实施方式中，在所述步骤3中，待获得的新的规划路径由(xv[i]，yv[i])表示；其中，xv[i]表示新的规划路径中离散点i处的X轴坐，yv[i]表示新的规划路径中离散点i处的Y轴坐标；离散点i表示规划路径中的任意一个离散点。
所述待获得的新的规划路径(xv[i]，yv[i])满足下式(一)和式(二)中的约束：
且所述待获得的新的规划路径(xv[i]，yv[i])能够使得目标函数J取最小值，所述取最小值的目标函数如下式(三)中所述：
其中，χc[i]表示cos(ψv)[i]，即新的规划路径中离散点i的速度方位角ψv的余弦值；χs[i]表示sin(ψv)[i]，即新的规划路径中离散点i的速度方位角ψv的正弦值；v表示无人机当前的速度，通过安装在无人机上的GPS和无人机自身惯导获得；χa[i]表示amax表示无人机最大允许的加速度，是无人机控制系统的设定值；ψvmax表示无人机最大允许的飞行方位角，是无人机控制系统的设定值；xv表示新的规划路径中离散坐标位置的X轴坐标；yv表示新的规划路径中离散坐标位置的Y轴坐标；yv[1]表示新的规划路径中第一个离散点处的Y轴坐标，yv[2]表示新的规划路径中第二个离散点处的Y轴坐标，yv[N]表示新的规划路径中第N个离散点处的Y轴坐标；cosψv表示速度方位角的余弦值；表示速度方位角的角速度；χs[i]表示新的规划路径中离散点i处的速度方位角正弦值。
td表示离散的时间步长，通过预估的飞行时间tf除以离散点个数N获得；所述预估的飞行时间tf是由规划路径中剩余路程除以飞行器速度得到的。
xv[i+1]表示新的规划路径中离散点i+1处的X轴坐标，yv[i+1]表示新的规划路径中离散点i+1处的Y轴坐标，χs[i+1]表示新的规划路径中离散点i+1处的速度方位角正弦值。
表示原始规划路径中第一个离散点处的Y轴坐标，表示原始规划路径中第二个离散点处的Y轴坐标，表示原始规划路径中第N个离散点处的Y轴坐标。
A1、A2和A3分别表示轨迹规划计算的中间变量，通过下式获得：
其中，表示原始规划路径中离散点i处的X轴坐标，表示原始规划路径中离散点i处的Y轴坐标；
xn表示第n个障碍物的圆心的X轴坐标，通过探测器建模获得；yn表示第n个障碍区圆心的Y轴坐标，通过探测器建模获得。
an表示第n个障碍物的长半轴，bn表示第n个障碍物的短半轴，当具有多个障碍物时，an中的n表示第n个障碍物，每个障碍物都对应计算出一组A1、A2和A3，每组A1、A2和A3都对应一组式(一)和式(二)的约束，即n个障碍物对应有n组式(一)和式(二)的约束。
rn表示轨迹规划过程中的中间变量，通过下式获得：
在一个优选的实施方式中，在步骤3中，在得到新的规划路径后，对得到的新的规划路径进行迭代，以便于进一步优化规划路径，得到最终的规划路径；其中，每次迭代都可以得到一条新的规划路径，当相邻两次迭代得到的两条新的规划路径之间偏差小于设定值时，迭代停止，最后一条规划路径即为最终的规划路径。
所述迭代过程包括如下子步骤：
子步骤1，根据得到的新的规划路径解算出中间变量A1′、A2′和A3′，
其中，rn′表示轨迹规划过程中的中间变量，通过下式获得：
其中，表示上一次迭代得到的规划路径中离散点i处的X轴坐标，表示上一次迭代得到的规划路径中离散点i处的Y轴坐标。
xn表示第n个障碍物的圆心的X轴坐标，通过探测器建模获得；yn表示第n个障碍区圆心的Y轴坐标，通过探测器建模获得。
an表示第n个障碍物的长半轴，bn表示第n个障碍物的短半轴，当具有多个障碍物时，an中的n表示第n个障碍物，每个障碍物都对应计算出一组A1′、A2′和A3′，每组A1′、A2′和A3′都对应一组式(一)和式(二)的约束，即n个障碍物对应有n组式(一)和式(二)的约束；
子步骤2，将中间变量A1′、A2′和A3′代入到约束式(五)中，
通过使得待获得的新的规划路径(xv[i]，yv[i])满足约束式(一)和式(五)，且所述待获得的新的规划路径(xv[i]，yv[i])能够使得目标函数J取最小值，得到该待获得的新的规划路径；
所述取最小值的目标函数如下式(三)中所述：
子步骤3，实时解算相邻两条规划路径之间的偏差，若相邻两条规划路径之间的偏差值不小于设定值ε，利用最后一次迭代得到的新的规划路径解算中间变量A1′、A2′和A3′，即重复子步骤1；若相邻两条规划路径之间的偏差值小于设定值ε，即下式(四)成立时，停止迭代，最后一次迭代得到的新的规划路径即为最终的规划路径。
||Yk-Yk-1||＜ε (四)
其中，k表示迭代次数，Yk表示第k次迭代得到的规划路径，Yk-1表示第k-1次迭代得到的规划路径，
即举例说明：
第一次迭代求得的规划路径
第二次迭代求得的规划路径
第k-1次迭代求得的规划路径
第k次迭代求得的规划路径
本发明中优选地，ε的取值为单位为米，当该设定值ε取值为(10，10)时，即时；
上述式(四)可以等价改写为：
即这N项中的最大值小于10，且这N项中的最大值也小于10时，上述不等式成立。
通过设置上述迭代及迭代终止条件，即能够兼顾到路径规划的准确性，还能够极大缩短迭代时间，使得从发现障碍物到获得最终的规划路径所需时间控制在1秒内，进而使得无人机能够实时根据障碍物信息沿着相应的新的规划路径飞行。
实验例：
无人机原始规划路径的起点为(0,0)，规划路径的终点为(40km,30km)；原始规划路径的轨迹如图2中的细实线所示。
在规划路径中存在障碍物，如图2中所示，障碍物为AZ1、AZ2、AZ3、AZ4、AZ5，其具体参数如下：
编号
中心坐标(km)
长轴(km)
短轴(km)
1
(15，10)
5
9
2
(25，10)
3.5
3.5
3
(35，11.5)
5
1.5
4
(25，0)
6
5
5
(26.5，20)
8
5
无人机在从起点起飞后，实时探测障碍物，在探测发现原始规划路径中存在障碍物时，重构新的规划路径(xv[i]，yv[i])，使得(xv[i]，yv[i])满足约束式(一)和约束式(二)；
且规划路径(xv[i]，yv[i])能够使得目标函数取最小值，所述取最小值的目标函数如下式(三)中所述：
其中，中间变量A1、A2和A3通过原始规划路径解算；
在得到新的规划路径后，通过上述约束式(一)和约束式(五)进行约束，使得目标函数取最小值，逐步进行重构迭代，
其中的中间A1′、A2′和A3′通过上一次迭代得到的规划路径解算，当得到的相邻两条规划路径之间的偏差值小于设定值(9.85,8.33)时，迭代停止。
得到最终的规划路径所用时间为0.9秒，所述最终的规划路径如图2中的虚线所示。
对比例：
无人机原始规划路径的起点为(0,0)，规划路径的终点为(40km,30km)，规划路径中存在的障碍物与实验例中的相同，
采用GPOPS(通用非线性规划求解器)获得新的规划路径，用时1052.4秒，得到的规划路径如图2中的点划线所示。
对比图2中的点划线和虚线可知，本申请提出的方法能够达到与通用非线性规划求解器类似的路径规划效果；通过本申请提出的方法得到的新的规划路径完美避开了障碍物，可以实现障碍物规避的任务要求，且解算时间短，将解算时间由通用非线性规划求解器的1052.4秒缩短到0.9秒，解算效率提升逾千倍，从而使得本申请提出的方法能够应用在飞行器上做实时规划处理。
以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：弓形路径无缝衔接规划方法