[发明专利]一种多旋翼无人机室内环境中的轨迹规划方法

权利要求书

1.一种多旋翼无人机室内环境中的轨迹规划方法，其特征在于，包括以下步骤：

S1.将地图转化为占据栅格地图，根据地图中每个栅格离障碍物的距离信息建立欧式符号距离场；

S2.根据欧式符号距离场，在占据栅格地图中的非障碍物区域搜索一条从起点到终点的初始路径；

S3.依据初始路径和占据栅格地图生成连通起点和终点的飞行通道；

S4.根据动力学约束，生成多旋翼无人机可执行的光滑的飞行轨迹。

2.根据权利要求1所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，欧式符号距离场计算公式为：

ESDF(n)＝min{||p_n-p₁||，||p_n-p₂||，...，||p_n-p_Q||}

其中ESDF(n)为当前栅格在欧式符号距离场中的值，p_n为当前栅格中心点的位置，Q为障碍物栅格的总数量，p₁，p₂，...，p_Q为分别为第1，2，...，Q个障碍物栅格的中心点的位置。

3.根据权利要求1所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，所述通过FIESTA算法计算整个地图的欧式符号距离场。

4.根据权利要求1所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，通过Voxblox算法来计算整个地图的欧式符号距离场。

5.根据权利要求1所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，步骤S2中，通过改进A*算法搜索一条尽量远离障碍物的初始路径，

f(n)＝g(n)+h(n)+O(n)

其中g(n)为起点到当前路径点的累计代价，h(n)是启发式函数，为当前路径点到终点的欧式距离，O(n)为当前路径点的碰撞代价。

6.根据权利要求5所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，碰撞代价O(n)的计算公式如下：

其中，ESDF(n)为当前路径点在欧式符号距离场中的值，a为大于零的常数。

7.根据权利要求1所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，所述飞行通道包括一系列正方体，相邻两个正方体通道存在公共区域，并且任何一个正方体通道内部不存在障碍物，多旋翼无人机通过每个正方体飞行通道的时间由正方体的边长决定，计算公式为：

其中W_i为第i个正方体通道的边长，s_i为多旋翼无人机飞行每个正方体通道的时间。

8.根据权利要求1所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，步骤S3的飞行通道生成步骤如下：

S31.从起点开始，依次将初始路径上的点设为正方体飞行通道的几何中心点，如果当前正方体飞行通道的几何中心点在上一个正方体飞行通道内部，则舍弃该路径点，否则将该路径点作为下一个正方体飞行通道的几何中心点，正方体的大小取一个栅格的大小；

S32.对该正方体飞行通道进行膨胀，膨胀过程中几何中心不变，只改变大小，每次膨胀正方体都是将其六个面向外移动一个栅格的大小，当正方体飞行道通接触到障碍物时则停止膨胀，进行下一个正方体飞行通道的生成；

S33.当终点被正方体飞行通道包含时，完成从起点到终点的飞行通道的生成。

9.根据权利要求1所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，步骤S4中，飞行轨迹为三维空间中的位置轨迹，包括X轴Y轴和Z轴，每一维的位置轨迹都是以多旋翼无人机能量最小为性能指标，以飞行通道为安全性约束，以最大速度、最大加速度和最大加加速度为动力学约束而生成的。

10.根据权利要求1～9任一项所述的多旋翼无人机室内环境中的轨迹规划方法，其特征在于，步骤S4中三维空间中的轨迹是采用分段贝塞尔曲线进行表示，如下所示：

其中，p(t)为分段贝塞尔曲线表示的轨迹曲线，n表示轨迹曲线的阶次，m表示轨迹的总段数，为第i段轨迹曲线的第j个参数，s_i为通过第i段轨迹的时间，T₀为第一段轨迹的起始时刻，T_i，i＝1，2，...，m为第i段轨迹的终点时刻，为第i段轨迹的伯恩斯坦基函数；

把轨迹生成转化为带有等式约束和不等式约束的二次规划问题，性能指标为多旋翼无人机能量最小，如下所示：

其中J为多旋翼无人机所消耗的能量，p⁽⁴⁾(t)为轨迹曲线的4次微分；

等式约束如下所示：

其中，k表示轨迹微分的次数，k取{0，1，2，3}，p^(k)(T₀)为T₀时刻轨迹的k次微分，为已知的起点位置的k次微分，p^(k)(T_m)为T_m时刻轨迹的k次微分，为已知的终点位置的k次微分，p^(k)(t)为轨迹曲线的k次微分，T_i-为T_i时刻的左极限，T_i+为T_i时刻的右极限；

不等式约束如下所示：

p_i(t)∈C_i (4)

-vel_max≤p⁽¹⁾(t)≤vel_max (5)

-acc_max≤p⁽²⁾(t)≤acc_max (6)

-jerk_max≤p⁽³⁾(t)≤jerk_max (7)

其中，p_i(t)为第i段轨迹曲线，p⁽¹⁾(t)为轨迹曲线的1次微分，p⁽²⁾(t)为轨迹曲线的2次微分，p⁽³⁾(t)为轨迹曲线的3次微分C_i为第i个飞行通道区域，vel_max多旋翼无人机的最大速度，acc_max为多旋翼无人机最大的加速度，jerk_max为多旋翼无人机最大的加加速度；

其中，等式(1)是起点的位置、速度、加速度和加加速度约束，等式(2)是终点的位置、速度、加速度和加加速度约束，等式(3)是相邻两端轨迹连接处的位置、速度、加速度和加加速度相等的约束，集合(4)表示飞行轨迹必须在飞行通道中，不等式(5)是对多旋翼无人机的飞行速度进行约束，不等式(6)是对多旋翼无人机的加速度进行约束，不等式(7)是对多旋翼无人机的加加速度进行约束。