一种四旋翼无人机的运动规划方法

本发明属于无人机运动规划，具体涉及一种四旋翼无人机的运动规划方法。

背景技术：

1、随着无人机技术的不断发展，其在各个领域的应用越来越广泛。因此，如何进行有效的无人机运动规划成为了目前研究的重点内容。例如，文献(a general,fast,androbust implementation of the time-optimal path parameterizationalgorithm.ieee transactions on robotics,2014,30(6):1533-1540.)中采用一种基于数值积分的方法来规划时间最优的运动轨迹。该方法实现形式容易，可以显示地通过积分为位置坐标分配最佳速度，从而达到运动轨迹时间最优的效果，并通过仿真实验对比了该方法在可靠性与运行时间上的优势。但是其并未考虑到无人机实际工作时机载传感器的探测范围有限，运动规划在假设全局环境信息精确已知的条件下进行，无法真正实现无人机凭借机载资源实时飞行。文献(time-optimal path tracking for robots:a convexoptimization approach”.ieee transactions on automatic control,2009,54(10):2318-2327.)中利用凸优化的思路解决时间最优轨迹规划问题。通过状态空间建模并引入约束条件，将运动规划问题转化为凸优化问题，从而在优化代价函数中加入对于时间的考量，获得具备时间最优性的轨迹规划结果。但是其对无人机的运动模型、成本函数进行了一定的假设来满足凸函数的要求，运动规划方法使用范围受限并且降低了该方法的鲁棒性。文献(多旋翼无人机时空轨迹规划方法研究.大连海事大学,2022.000801.)中采用时空解耦的方式为无人机规划轨迹。空间运动规划通过rrt*算法的路径搜索结果结合基于梯度下降方法得到满足约束的平滑轨迹，然后通过额外的时间成本优化过程，进一步缩短空间轨迹的飞行时间。但是其将轨迹飞行时间进行单独优化，虽然获得了较短的轨迹时间分配结果，但是算法运行时间的增加无疑不利于无人机完成快速飞行。

2、综上所述，现有的优化无人机飞行时间的轨迹规划方法存在需要全局环境信息、轨迹规划所需的时间长以及规划方法的鲁棒性差的问题，解决上述问题是十分必要的。

技术实现思路

1、本发明的目的是为解决现有的优化无人机飞行时间的轨迹规划方法存在需要全局环境信息、轨迹规划所需的时间长以及规划方法的鲁棒性差的问题，而提出的一种四旋翼无人机的运动规划方法。

2、本发明为解决上述技术问题所采取的技术方案是：一种四旋翼无人机的运动规划方法，所述方法具体包括以下步骤：

3、步骤一、以当前无人机的中心为圆心，以机载传感器的探测极限距离为半径，将得到的圆形区域作为无人机的局部规划域；

4、将全局目标终点投影于局部规划域，获得无人机的局部目标终点；

5、步骤二、判断无人机的中心与局部目标终点的连线是否经过障碍物；

6、若无人机的中心与局部目标终点的连线经过障碍物，则执行步骤三；

7、若无人机的中心与局部目标终点的连线不经过障碍物，则将无人机的中心与局部目标终点的连线作为无人机到局部目标终点的路径；并转至步骤四；

8、步骤三、分别为经过的每个障碍物寻找替换点，将寻找到的替换点作为路径经过的航点，在当前无人机位置点、航点以及局部目标终点中，分别对每两个相邻点之间的航行路线进行初步路径规划，再对初步路径规划结果进行路径优化；

9、再转至步骤四；

10、步骤四、判断是否已经到达全局目标终点；

11、若已经到达全局目标终点，则结束运动规划；

12、若未到达全局目标终点，则返回执行步骤一。

13、进一步地，所述寻找替换点的具体方法为：

14、对于经过的任意一个障碍物，作障碍物与局部目标终点连线的垂线，在作出的垂线上任意选取替换点；

15、同理，为经过的每个障碍物分别寻找替换点。

16、进一步地，所述初步路径规划的具体过程为：

17、步骤1、初始化迭代次数i＝1；

18、步骤2、采用rrt*算法对点xstart与点xgoal之间的路径进行规划，将规划出的路径长度记为li；其中，xstart和xgoal为两个相邻点；

19、步骤3、判断是否满足路径规划的停止条件；

20、若未达到路径规划的停止条件，则令i＝i+1，并继续执行步骤4；

21、若达到路径规划的停止条件，则将最后一次迭代获得的规划结果作为初步路径规划结果；

22、步骤4、将第i-1次迭代规划出的路径长度li-1作为椭圆长轴的长度，将作为椭圆短轴的长度，其中cmin代表点xstart与点xgoal之间的直线距离；将点xstart与点xgoal作为椭圆长轴上的点，且点xstart与点xgoal关于椭圆的中心对称；

23、将椭圆区域的内部作为采样区域，根据确定的采样区域，采用rrt*算法重新进行路径规划，将规划出的路径长度记为li；

24、并返回执行步骤3。

25、进一步地，所述rrt*算法中添加了成本优化环节，成本优化方式为(1)或(2)：

26、(1)在设定范围内，为新延伸的节点选择路径时间成本最小的父节点；

27、(2)在设定范围内，将新延伸的节点作为树中现有节点的父节点，进行树结构的重布线。

28、进一步地，所述路径规划的停止条件为：前后两次迭代规划出的路径长度差小于设定的阈值或达到了设置的最大迭代次数。

29、进一步地，所述对初步路径规划结果进行路径优化；具体为：

30、步骤(1)、根据设置的时间步长，在位置-速度平面上从初始状态以最大加速度前向积分直至触碰不违反约束时的最大速度曲线；

31、步骤(2)、从违反约束的状态开始，沿着不违反约束时的最大速度曲线寻找积分切换点；

32、步骤(3)、在积分切换点处，先以积分启动加速度向后积分再以最小加速度向后积分，直到与步骤(1)中得到的积分曲线相交；

33、步骤(4)、将积分切换点处的状态重新作为初始状态，返回执行步骤(1)。

34、进一步地，所述积分切换点的寻找方法为：满足下列条件之一的点即为积分切换点；

35、条件(1)：不违反约束时最大速度曲线上的不连续点；

36、条件(2)：不违反约束时最大速度曲线的切向量方向与加速度极限值所对应积分曲线的切向量方向相同的点；

37、条件(3)：确定零惯性点s*，在点s*处存在约束变量ak(s*)＝0，若零惯性点s*处约束变量bk(s*)≤0，则该点s*不是积分切换点，若零惯性点s*处约束变量bk(s*)＞0，则计算零惯性点s*处的速度

38、

39、其中，ck(s*)是约束变量；

40、比较速度与零惯性点s*在不违反约束时最大速度曲线上对应速度的大小，若速度小于零惯性点s*在不违反约束时最大速度曲线上对应的速度，则零惯性点s*是积分切换点，否则零惯性点s*不是积分切换点。

41、更进一步地，所述积分启动加速度的计算方法为：

42、将积分切换点s*处的速度表示为则计算出实数λ：

43、

44、其中，αk是积分切换点处的最小加速度，ε为任意小的正数；

45、则第一个衔接段的终点速度为根据和时间步长计算出第一个衔接段的积分启动加速度；

46、再采用第一个衔接段积分启动加速度的计算方法，根据计算第二个衔接段的积分启动加速度，以此类推，直至计算出第n个衔接段的积分启动加速度。

47、本发明的有益效果是：

48、本发明方法能够保证无人机利用机载资源在未知环境中进行实时规划，而非建立在先验已知的全局环境信息上进行，实现了真正的自主飞行。而且，本发明的路径规划算法可以在保证路径质量的同时进一步提升了算法的运算效率，为四旋翼无人机的快速飞行建立基础。本发明采用了实现形式简便、计算成本低的数值积分算法，并针对积分切换的处理进行设计，而非采用枚举方式寻找切换点，在保证轨迹时间最优的同时，提升了算法的可靠性和鲁棒性。