1.本发明涉及无人机运动路径规划技术领域，具体涉及一种无人机的运动路径规划方法、无人机及存储介质。
背景技术：
2.多旋翼无人机在复杂环境中实现安全且快捷的自主导航离不开鲁棒和高效的运动规划，对于一般的机器人而言，高质量的运动规划需要精准地考虑动力学的微分约束、控制输入的光滑性和整个系统状态的安全，多旋翼无人机，作为一种轻量级的移动机器人，更需要满足实际导航需求中的swp限制，即尺寸(size)、重量(weight)和功率(power)限制，这些限制要求多旋翼无人机利用极其有限的机载计算资源来完成飞行轨迹实时高效地求解计算，目前学术界的大部分运动规划求解器，或基于采样类方法，或基于最优控制类方法，以支持对一般机器人的动力学-运动学规划，尽管如此，这些通用求解器缺乏在线求解复杂大规模规划问题的能力，为了能满足时间敏感的计算任务需求，一些专用的运动规划方法过度简化多旋翼无人机的动力学模型以及动力学约束，使在线轨迹规划求解成为可能，这些专用方法通常会给出较为保守的解，从而难以充分发挥出多旋翼无人机自身的高机动性。高效率高质量的室内无人机运动规划存着四个理论层面的挑战：第一，为保证机体安全，规划器需要频繁访问存取三维精细地图的大量离散数据；第二，动力学的高度非线性使无人机极限飞行时难以有效地保障全状态层面的安全；第三，运动规划的高质量求解在传统意义上需要对动态过程做精细化时间离散，不可避免地提高数学规划的求解复杂度；第四，传统的飞行轨迹参数化方法缺乏连续时间约束下在时间域进行直接调整优化的自由度。
3.现有的求解室内无人机避障及运动规划方法可分为四类：基于图搜索的算法、基于采样的方法、插值曲线规划器和数值优化方法；其中：dijkstra算法是一种典型的图搜索算法，它可以找到两点之间的单源最短路径，它在darpa挑战中表现突出；a*算法是dijkstra算法的扩展，由于启发式的实现，该算法能够实现快速的节点搜索，它已被应用于广泛的无人机和无人车场景中，其自身的亮点是算法的节点权重设置和定义的成本函数功能；尽管这两种算法是实用的，但技术路径是不连续的，并且没有考虑室内无人机的运动学与动力学问题。
4.为了克服上述缺点，现有文献已经提供了许多改进的j9九游会真人的解决方案，如：vougioukas等人利用rrt算法来探索可能的运动空间并计算可行的轨迹，这为规划无人机的起点到目标点运动提供了详细的路径，但基于采样的j9九游会真人的解决方案是次优的，并且不是曲率连续的，这不符合室内无人机的实际需求；santos等人考虑到基于迭代bezier曲线的方法，开发了一种碰撞避免方法，但对于插值曲线，需要更多的控制点来使规划路径更平滑、更连续，增加了计算负担；数值优化方法最小化或最大化受各种约束变量影响的函数，它们包括人工势场法、离散优化和最优控制算法，但是这些方法中的大多数仅适用于室外道路场景。
5.综上所述，目前缺少一种在室内无人机飞行中规划安全、平滑、无碰撞的路径的方
法。
技术实现要素：
6.本发明提供的方法可以在室内无人机飞行中规划出安全、平滑及无碰撞的路径。
7.第一方面，本发明一实施例提供一种无人机的运动路径规划方法,包括：根据无人机当前位置信息和障碍物信息，得到运动轨迹函数，所述运动轨迹函数用于分段表示初始的无人机运动轨迹；根据所述运动轨迹函数构造运动规划目标函数，所述运动规划目标函数包括多个随时间变化的函数；根据所述运动规划目标函数和预设的所述运动规划目标函数的约束条件，构建优化目标函数；对所述优化目标函数进行求解，以得到优化后的无人机运动轨迹。
8.一些实施例中，所述根据无人机当前位置信息和障碍物信息，得到运动轨迹函数，包括：根据无人机当前位置信息和障碍物信息，得到多项式轨迹函数；根据所述多项式轨迹函数，得到所述运动轨迹函数。
9.一些实施例中，所述多项式轨迹函数可通过以下方式表示：其中，a0,a1…an
为轨迹参数，t为时间，n为多项式的阶数。
10.一些实施例中，所述运动轨迹函数可通过以下方式表示：
11.其中，p(t)为运动轨迹函数，p1,p2…
pn为所述初始的无人机运动轨迹中不同时间段的轨迹曲线，k为初始避障点，t为时间，t0,t1,
…
tk为所述时间段的起点/终点。
12.一些实施例中，所述运动规划目标函数，包括：
13.v(t)＝p
(1)
(t)＝[0,1,2t,3t2,...,nt
(n-1)
]p，a(t)＝p
(2)
(t)＝[0,0,2,6t,...,n(n-1)t
n-2
]p，
[0014]
jerk(t)＝p
(3)
(t)＝[0,0,0,6,...,n(n-1)(n-2)t
n-3
]p，
[0015]
snap(t)＝p
(4
)(t)＝[0,0,0,0,...,n(n-1)(n-2)(n-3)t
n-4
]p，其中，t为时间，p为参数向量，p＝[p0,p1…
pn]
t
，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，n为多项式的阶数，p(t)为运动轨迹函数，v(t)为速度，a(t)为加速度，jerk(t)为加加速度，snap(t)为加加加速度。
[0016]
一些实施例中，所述运动规划目标函数的约束条件包括：等式约束条件，所述等式约束条件包括第一等式约束条件、第二等式约束条件和连续性约束条件；所述第一等式约束条件可通过以下方式表示：
[0017][0018]
[0019][0020][0021]
其中，[x0,y0]为无人机运动轨迹的起点坐标，v
0x
为无人机运动轨迹的起点速度，p为参数向量，p＝[p0,p1…
pn]
t
，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，n为多项式的阶数，t0为起点时间；
[0022]
所述第二等式约束条件可通过以下方式表示：
[0023][0024][0025][0026][0027]
其中，[xk,yk]为无人机运动轨迹的终点坐标，v
kx
为无人机运动轨迹的终点速度，p为参数向量，p＝[p0,p1…
pn]
t
，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，n为多项式的阶数，tk为终点时间；
[0028]
所述连续性约束条件可通过以下方式表示：
[0029][0030][0031][0032][0033]
其中，p为参数向量，p＝[p0,p1…
pn]
t
，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，n为多项式的阶数，ti为从无人机运动轨迹的起点到终点的不同时刻。
[0034]
一些实施例中，所述运动规划目标函数的约束条件还包括：不等式约束条件，所述不等式约束条件包括区域约束条件和曲率约束条件；
[0035]
所述区域约束条件可通过以下方式表示：
[0036]
n＝[[(x
i-x
i 1
)2 (y
i-y
i 1
)2]/r] 1
[0037]
其中，[xi,yi]和[x
i 1
,y
i 1
]分别是第i个航路点和第i 1个航路点在初始的无人机运动轨迹上对应的坐标，n为在每两个航路点之间填充的路径点的个数，r为优化区域的半径，根据上述公式优化得到的坐标[p
ix
(t),p
iy
(t)]须满足x
min
≤p
ix
(t)≤x
max
和y
min
≤p
iy
(t)≤y
max
，(x
min
,y
min
)为优化区域的左下坐标，(x
max
,y
max
)为优化区域的右上坐标，(x
min
,y
min
)和(x
max
,y
max
)构成了优化区域的坐标。
[0038]
所述曲率约束条件可通过以下方式表示：
[0039]
[0040]
其中，k
max
为路径的最大曲率，r
min
为预设的无人机最小转弯半径。
[0041]
一些实施例中，所述优化目标函数可通过以下方式表示：
[0042][0043]
其中，a
eq
与a
ineq
分别为凸优化问题中等式约束与不等式的系数矩阵，b
eq
与b
ineq
分别为等式约束与不等式约束向量，q是时间分布函数，p为参数向量；
[0044]
q和qi可以通过以下方式表示：
[0045][0046][0047]
其中，其中，c为时间分布函数的期望，r为时间分布函数的方差，k为最终时刻，n为从0到k的时刻的统计，ti为从无人机运动轨迹的起点到终点的不同的时刻。
[0048]
第二方面，本发明一实施例提供一种无人机，所述无人机包括：存储装置，被配置为存储计算机程序；机载传感器，被配置为实时采集所述无人机的各项测量传感数据；飞控装置，被配置为根据机载传感器采集的各项测量传感数据生成无人机的当前位置和障碍物信息及将其发送给运动规划装置，和，根据无人机的当前位置以及运动规划装置规划的路径控制无人机飞行；运动规划装置，被配置为执行所述计算机程序时实现如前述的无人机的运动路径规划方法。
[0049]
第三方面，本发明一实施例提供一种存储介质，所述存储介质上存储有程序，所述程序能够被处理器执行以实现如前述的无人机的运动路径规划方法。
[0050]
在上述实施例的方法中，根据无人机当前位置信息和障碍物信息得到运动轨迹函数，根据运动轨迹函数构造运动规划目标函数，根据运动规划目标函数和预设的运动规划目标函数的约束条件构建优化目标函数，对优化目标函数进行求解，得到优化后的无人机运动轨迹，由于将室内无人机的运动规划问题转化为多准则优化问题后对该问题进行求解，使得在室内无人机飞行中可以实时规划出安全、平滑及无碰撞的路径。
附图说明
[0051]
图1为本发明提供的基于无人机的运动路径规划方法流程图；
[0052]
图2为一种实施例中得到运动轨迹函数的流程图；
[0053]
图3为本发明提供的无人机的结构图；
[0054]
图4为本发明提供的存储介质的结构图。
具体实施方式
[0055]
下面通过具体实施方式结合附图对本发明作进一步详细说明。其中不同实施方式中类似元件采用了相关联的类似的元件标号。在以下的实施方式中，很多细节描述是为了
使得本技术能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其他元件、材料、方法所替代。在某些情况下，本技术相关的一些操作并没有在说明书中显示或者描述，这是为了避免本技术的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。
[0056]
另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。
[0057]
本文中为部件所编序号本身，例如“第一”、“第二”等，仅用于区分所描述的对象，不具有任何顺序或技术含义。
[0058]
请参考图1，本发明一实施例中提供一种无人机的运动路径规划方法,包括：
[0059]
s10：根据无人机当前位置信息和障碍物信息，得到运动轨迹函数，运动轨迹函数用于分段表示初始的无人机运动轨迹。
[0060]
一些实施例中，根据无人机当前位置信息和障碍物信息，得到运动轨迹函数，如图2所示，包括：
[0061]
s11：根据无人机当前位置信息和障碍物信息，得到多项式轨迹函数。
[0062]
一些实施例中，无人机当前位置信息和障碍物信息通过机载传感器实时采集，多项式轨迹函数可通过以下方式表示：
[0063][0064]
其中，a0,a1…an
为轨迹参数，t为时间，n为多项式的阶数(轨迹参数的个数)。
[0065]
s12：根据多项式轨迹函数，得到运动轨迹函数。
[0066]
一些实施例中，假设参数向量p＝[p0,p1…
pn]
t
，则初始的无人机运动轨迹的运动轨迹函数可通过以下方式表示：
[0067][0068]
其中，p(t)为运动轨迹函数，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，p1,p2…
pn对应于a1,a2…an
，k为初始避障点，t为时间，t0,t1,
…
tk为时间段的起点/终点，如：t
k-1
为一个轨迹曲线的起点，tk为该轨迹曲线的终点，通过将初始的无人机运动轨迹按时间进行分段，每个轨迹曲线用一条多项式曲线表示，可以更好地表达多项式轨迹函数。
[0069]
s20：根据运动轨迹函数构造运动规划目标函数，运动规划目标函数包括多个随时间变化的函数。
[0070]
一些实施例中，运动规划目标函数，包括：
[0071]
v(t)＝p
(1)
(t)＝[0,1,2t,3t2,...,nt
(n-1)
]p
[0072]
a(t)＝p
(2)
(t)＝[0,0,2,6t,...,n(n-1)t
n-2
]p
[0073]
jerk(t)＝p
(3)
(t)＝[0,0,0,6,...,n(n-1)(n-2)t
n-3
]p
[0074]
snap(t)＝p
(4)
(t)＝[0,0,0,0,...,n(n-1)(n-2)(n-3)t
n-4
]p
[0075]
其中，t为时间，p为参数向量，p＝[p0,p1…
pn]
t
，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，n为多项式的阶数，p(t)为运动轨迹函数，v(t)为速度，a(t)为加速度，jerk(t)为加加速度，snap(t)为加加加速度，对于任一时间点，可以根据轨迹参数计算出轨迹曲线的位置p、速度v、加速度a、三阶导数jerk和四阶导数snap，本发明提供的方法将可能限制编入路线的轨迹的障碍物约束纳入构建运动规划目标函数考虑范围内，可以在规划的路径中做到合理的避障；还将初始的无人机运动轨迹上的位置、速度、加速度、三阶导数jerk和四阶导数snap均纳入构建运动规划目标函数的约束内，不仅考虑了无人机的运动学一阶、二阶导数约束，还添加了三阶导数和四阶导数约束，可以使得无人机规划出来的轨迹更加平滑，效果更好。
[0076]
s30：根据运动规划目标函数和预设的运动规划目标函数的约束条件，构建优化目标函数。
[0077]
在构建无人机运动轨迹的优化目标函数时往往需要轨迹满足一系列约束，然而，满足约束的轨迹有无数条，实际问题中，只需要一条最优的轨迹，因此需要构建一个最优函数，将问题建模成一个多准则优化问题。
[0078]
一些实施例中，运动规划目标函数的约束条件包括：等式约束条件，等式约束条件包括第一等式约束条件、第二等式约束条件和连续性约束条件。
[0079]
由实际运动可知，在无人机的起点仅存在无人机的起点速度，加速度a、三阶导数jerk和四阶导数snap均为0，第一等式约束条件可通过以下方式表示：
[0080][0081][0082][0083][0084]
其中，[x0,y0]为无人机运动轨迹的起点坐标，起点坐标和起点速度为预设的，v
0x
为无人机运动轨迹的起点速度，p为参数向量，p＝[p0,p1…
pn]
t
，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，n为多项式的阶数，t0为起点时间。
[0085]
在实际运动轨迹中，应以速度v、加速度a、三阶导数jerk和四阶导数snap等于0结束，第二等式约束条件可通过以下方式表示：
[0086][0087][0088][0089][0090]
其中，[xk,yk]为无人机运动轨迹的终点坐标，v
kx
为无人机运动轨迹的终点速度，
终点坐标和终点速度为预设的，p为参数向量，p＝[p0,p1…
pn]
t
，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，n为多项式的阶数，tk为终点时间。
[0091]
由于实际的运动轨迹应该是连续的，所以对速度v、加速度a和更高导数施加连续性约束，连续性约束条件可通过以下方式表示：
[0092][0093][0094][0095][0096]
其中，p为参数向量，p＝[p0,p1…
pn]
t
，p1,p2…
pn为初始的无人机运动轨迹中不同时间段的轨迹曲线，n为多项式的阶数，ti为从无人机运动轨迹的起点到终点的不同时刻。
[0097]
由于只增加等式约束，并不能保证室内无人机在实际避障和运动规划的安全性，因此本发明还增加不等式约束条件来加强安全约束，例如避免碰撞和进行曲率约束，在本实施例中，运动规划目标函数的约束条件还包括：不等式约束条件，不等式约束条件包括区域约束条件和曲率约束条件。
[0098]
一些实施例中，增加了区域约束条件以限制避障路径和运动规划的最佳范围，考虑到计算的复杂性，本实施例采用优化区域构建区域约束条件，优化区域可以选择为正方形、矩形或菱形，此外，初始的无人机运动轨迹包括多个航路点，当各个航路点彼此相距过远将导致算法难以进行相应的优化时，在每两个初始航路点之间填充n个路径点，即可实现轨迹点的平滑，区域约束条件可通过以下方式表示：
[0099]
n＝[[(x
i-x
i 1
)2 (y
i-y
i 1
)2]/r] 1
[0100]
其中，[xi,yi]和[x
i 1
,y
i 1
]分别是第i个航路点和第i 1个航路点在初始的无人机运动轨迹上对应的坐标，n为在每两个航路点之间填充的路径点的个数，r为优化正方形的半径，根据上述公式优化得到的坐标[p
ix
(t),p
iy
(t)]须满足x
min
≤p
ix
(t)≤x
max
和y
min
≤p
iy
(t)≤y
max
，(x
min
,y
min
)为优化区域的左下坐标，(x
max
,y
max
)为优化区域的右上坐标，(x
min
,y
min
)和(x
max
,y
max
)构成了优化区域的坐标。
[0101]
一些实施例中，采用正方形区域进行优化，优化正方形的半径为0.5m。
[0102]
以往的许多研究都考虑缩短避障路径的长度或满足避障路径位置连续性、方向连续性、曲率连续性等，而没有考虑避障路径的最大曲率，室内无人机在具体避障时会存在转弯半径，当避障路径的曲率超过最大转弯半径的范围时，由于无人机的转向不足，跟踪过程会降低跟踪精度，从而影响操作的质量和安全，因此，本发明增加了曲率约束条件来加强安全约束，曲率约束条件可通过以下方式表示：
[0103][0104]
其中，k
max
为路径的最大曲率，r
min
为预设的无人机最小转弯半径。
[0105]
一些实施例中，优化目标函数可通过以下方式表示：
[0106][0107]
其中，a
eq
与a
ineq
分别为凸优化问题中等式约束与不等式的系数矩阵，b
eq
与b
ineq
分别为等式约束与不等式约束向量，q是时间分布函数，p为参数向量。
[0108]
q和qi可以通过以下方式表示：
[0109][0110][0111]
其中，c为时间分布函数的期望，r为时间分布函数的方差，k为最终时刻，n为从0到k的时刻的统计，ti为从无人机运动轨迹的起点到终点的不同的时刻。
[0112]
s40：对优化目标函数进行求解，以得到优化后的无人机运动轨迹。
[0113]
一些实施例中，采用qp求解器对优化目标函数进行求解得到参数向量p，根据参数向量p可以得到优化后的无人机运动轨迹。
[0114]
本发明提供的方法将室内无人机的运动规划问题转化为多准则优化问题后对该问题进行求解，使得在室内无人机飞行中可以实时规划出安全、平滑及无碰撞的路径。
[0115]
本发明另一实施例中提供一种无人机，如图3所示，无人机包括：存储装置100，被配置为存储计算机程序；机载传感器200，被配置为实时采集无人机的各项测量传感数据；飞控装置300，被配置为根据机载传感器200采集的各项测量传感数据生成无人机的当前位置和障碍物信息及将其发送给运动规划装置400，和，根据无人机的当前位置以及运动规划装置400规划的路径控制无人机飞行；运动规划装置400，被配置为执行计算机程序时实现如前述的无人机的运动路径规划方法。
[0116]
一些实施例中，机载传感器200包括超声波测距雷达、激光测距雷达、光流传感器、三轴陀螺仪、三轴加速度计、三轴磁力计、气压计等等，主要用于采集无人机的各项测量传感数据，然后将采集得到的信息发送给飞控装置300，从而可以使得飞控装置300根据各项测量传感数据得到无人机在室内的具体位置和障碍物信息，为后续在运动规划装置400进行室内无人机的运动规划算法计算提供先验知识；运动规划装置400根据飞控装置300传过来的无人机的当前位置和障碍物信息，采用前述的无人机的运动路径规划方法规划出无人机的具体运动轨迹，并将规划出的无人机运动轨迹发送给飞控装置300，飞控装置300根据无人机的当前位置和姿态，以及运动规划装置400规划的路径，采用ekf算法计算出无人机的每个旋翼pwm波形，最后通过电调将飞控装置300输出的pwm波形转化为电机的转速。
[0117]
本发明另一实施例中提供一种存储介质，如图4所示，存储介质500上存储有程序，程序能够被处理器600执行以实现如前述的无人机的运动路径规划方法。
[0118]
以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。