一种无人机局部路径规划方法与流程

1.本发明涉及无人机操作技术领域，具体是一种无人机局部路径规划方法。


背景技术：

2.路径规划方法广义地可以分为全局路径规划和局部路径规划算法。当存在精确的全局地图时，规划一条无人机安全有效的甚至最优的路径很容易；但当无人机仅能通过传感器实时感知周边环境生成局部地图时，输出一条优秀的路径存在很大挑战。
3.其中动态窗口法（dynamic window approach, dwa）便是一种应用广泛的局部路径规划算法。传统的dwa算法采用固定权重组合，能在差分机器人低速运动面对稀疏的小目标障碍场景下表现良好，但面对新的复杂场景，例如存在大面积横断障碍场景的情况下，容易出现路径规划不合理甚至产生碰撞的情形。另外，当无人机在高速运动状态下需要较大的转向角且稳定在该状态一段时间，才可绕过障碍，传统的dwa算法经常出现以下不足：无人机过度转向导致的螺旋转圈、不足转向导致的转向静止，以及平顺性差导致的速度突变等问题。


技术实现要素：

4.发明目的：本发明的目的是提供一种无人机局部路径规划方法，解决了无人机在大面积横断障碍场景下，存在的过度转向、不足转向、以及平顺性差导致的无法顺利飞行的问题。
5.技术方案：本发明提供了一种无人机局部路径规划方法，包括以下步骤：步骤s1、通过无人机上的传感器，对所处的局部场景进行全方位扫描，获得若干可行域，并结合可行域选择评价函数q（v，ω）获得相应的最优采样控制空间，其中，v表示为线速度， w表示为角速度；步骤s2、通过dwa算法在最优采样控制空间中采样多组线速度和角速度组合，并根据评价函数g（v，ω）获得一系列预选轨迹，其中，v表示为线速度， w表示为角速度。
6.步骤s3、从一系列预选轨迹中输出一条得分最低的轨迹，限制始末点状态，最终解析出一条加速度变化率积分最小的更平顺的光滑轨迹解析式；步骤s4、将获得的解析式输送给无人机控制器，从而实现无人机稳定平顺地飞行。
7.本发明增加了一个串行可行域评价函数q（v，ω），对所处的具有大面积横断障碍的飞行场景的控制空间采样范围进行限制，在前端把动态窗口绑定在有限可行域内，从而有效减少无人机在遇到大面积横断障碍时候，过度转向导致的螺旋转动，或者转向不足导致的静止问题，从而大大提高了无人机的飞行效率。另外，对通过评价函数g（v，ω）获得的预选轨迹进行后端轨迹平滑处理，对加速度空间平滑处理，实现对控制空间的进一步离散约束，最终输出一条更平顺的轨迹，从而通过局部路径的规划，保证了无人机可以在复杂场景下的高速稳定的飞行，有效提高了无人机飞行的稳定性以及平顺性。
8.进一步的，所述步骤s1包括以下子步骤：
步骤s1-1、以无人机所在位置为中心点，通过传感器扫描全周环境，获得n个障碍遮挡点（n=1,2，
……
），并以最接近目标点的障碍点为起点，顺时针进行标记o1、o2、o3、
……
、on；步骤s1-2、避开感知域内的障碍，获得若干个可行域，从而将采样控制空间限制在可行域中；步骤s1-3、结合可行域选择评价函数q（v，ω）获得相应的最优采样控制空间。
9.进一步的，所述步骤s1-3中可行域评价函数为：，i=1,2，
……
，n；其中：；，为可行域限制范围； 、分别表示以障碍遮挡点、为可行域边缘点时候的绝对角， 表示无人机飞行目标点的绝对角。
10.进一步的，所述步骤s3中评价函数g（v，ω）为：其中：head为方位角评价函数；dist为障碍物距离评价函数；vel为目标速度评价函数；σ、α、β、γ为各项评价子函数权重系数。
11.由于评价函数的权重系数需要预先设置好，通过预先选择靠近目标方向的可行域，且远离目标的可行域惩罚项指数增长，会有效优先选择靠近目标方向的可行域，再在该选定可行域进行控制空间采样，能有的放矢地限制采样范围，使得在遇到大面积墙体等障碍时，避免α设置的较大导致的持续偏转自旋，以及有效改善α设置的较小时无法计算出逃逸轨迹，导致障碍权重急剧升高而悬停的情况。
12.进一步的，所述步骤s3包括以下子步骤：步骤s3-1、根据步骤s2的求解，获得预测轨迹函数f（t）；步骤s3-2、对轨迹函数f（t）求导，分别获得速度、加速度、加加速度等参数随时间变化的函数v（t）、a（t）、jerk（t）；步骤s3-3、构建轨迹起点的位置、速度和加速度约束条件，以及终点的位置、速度和加速度约束条件；步骤s3-4、对预测轨迹函数f（t）进行优化，最终获得一条加速度变化累计损失最小的轨迹方程式。
13.进一步的，所述步骤s3-1中的预测轨迹函数f（t）为：其中，p为轨迹上的位置点。
14.进一步的，所述s3-2中的v（t）、a（t）、jerk（t）为：jerk（t）为：
ꢀ
。
15.进一步的，所述步骤s3-3中的初始状态的约束条件如下：3中的初始状态的约束条件如下：3中的初始状态的约束条件如下：其中，为起点位置点； 为起点速度；为起点加速度。
16.进一步的，所述步骤s3-3中的终止状态的约束条件如下：3中的终止状态的约束条件如下：3中的终止状态的约束条件如下：其中，为终点位置点；为终点速度； 为终点加速度。
17.上述技术方案可以看出，本发明具有如下有益效果：1）通过可行域选择评价函数q（v，ω）对无人机飞行时候所处的局部区域进行可行域的选定，并在该选定可行域内进行控制空间采样，从而有的放矢地限制采样范围，避免在遇到大面积墙体等障碍时无人机出现偏转自旋或者悬停的情况，大大提高了无人机的飞行效率；2）对通过dwa算法获得的轨迹函数进行后端轨迹平滑处理，最终获得一条更平顺的轨迹，实现无人机稳定平顺地飞行。
附图说明
18.图1为本发明的流程图；图2为实施例一中无人机飞行局部区域图；图3为实施例二中的仿真场景图；图4为实施例二中未设定最优采样控制空间情况下无人机局部轨迹图；图5为实施例二中设定最优采样控制空间情况下无人机局部轨迹图；图6为实施例三中无人机飞行局部区域图；图7为实施例三中轨迹导数的通项公式；图8为实施例三中最终优化后的无人机局部轨迹图。
具体实施方式
19.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
20.在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本
发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
21.此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上，除非另有明确的限定。
22.在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
23.在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。
24.实施例一如图1所示为本发明的流程图，包括以下步骤：步骤s1、通过无人机上的传感器，对所处的局部场景进行全方位扫描，获得若干可行域，并结合可行域选择评价函数q（v，ω）获得相应的最优采样控制空间；步骤s2、通过dwa算法在最优采样控制空间中采样多组线速度和角速度组合，并根据评价函数g（v，ω）获得一系列预选轨迹，其中，v表示为线速度， w表示为角速度；步骤s2、通过dwa算法在最优采样控制空间中采样多组线速度和角速度组合，并根据评价函数g（v，ω）获得一系列预选轨迹，其中， v表示为线速度， w表示为角速度；步骤s4、将获得的解析式输送给无人机控制器，从而实现无人机稳定平顺地飞行。
25.所述步骤s1包括以下子步骤：步骤s1-1、以无人机所在位置为中心点 ，通过传感器扫描全周环境，获得n个障碍遮挡点（n=1,2，
……
），并以最接近目标点 的障碍点为起点，顺时针进行标记、 、、
……
、；如图2所示为本实施例中无人机飞行局部区域图，其中表示无人机的位置，圆形表示无人机传感器所扫描的范围，阴影部分表示大面积障碍， 表示目标点；无人机在 点通过传感器扫描周围环境，获得障碍遮挡点。
26.步骤s1-2、避开大面积腔体等障碍，获得若干个可行域，从而将采样控制空间限制在可行域中；本实施例中避开阴影部分的障碍位置，无人机的可行域为 、 、；步骤s1-3、结合可行域选择评价函数q（v，ω）获得相应的最优采样控制空间；由于无人机路径规划过程中，不涉及高度方向的变化，所述步骤s1-1中中心点 的坐标点为（ ，）， 、 分别表示 在x轴和y轴方向的坐标点；所述步骤s1-1中障碍遮挡点 的坐标点为（ ，），、 分别表示 在x轴和y轴方向的坐标点；
所述步骤s1-1中目标点的坐标为（），表示在x轴和y轴方向的坐标点。
27.所述步骤s1-3中可行域评价函数为： ，i=1,2，
……
，n；其中：； ，为可行域限制范围；、分别表示以障碍遮挡点、为可行域边缘点时候的绝对角， ， ；表示无人机飞行目标点的绝对角， 。
28.本实施例中，在可行域中，i=1，=；在可行域中，i=3， =；在可行域中，i=5， =；由此可见，本实施例中最终选择可行域作为控制采样空间。
29.实施例二基于实施例一中的步骤，在如图3所示的仿真场景中，结合可行域选择评价函数q（v，ω）获得相应的最优采样控制空间。
30.所述步骤s3中评价函数g（v，ω）为：其中：head为方位角评价函数；dist为障碍物距离评价函数；vel为目标速度评价函数；σ、α、β、γ为各项评价子函数权重系数。
31.本实施例中，通过预先选择靠近目标方向的可行域，且远离目标的可行域惩罚项指数增长，有效优先选择可行域作为最优控制采样空间。通过评价函数g（v，ω）在最优控制采样空间范围内做轨迹预测。如图4和如图5所示分别为未设定最优采样控制空间情况下无人机局部轨迹图以及设定最优采样控制空间情况下无人机局部轨迹图。图4中无人机在遇到大面积障碍之后，出现螺旋转动，无法顺利飞过障碍；而图5中经过采样控制空间的限制，无人机在此区间内规划和飞行，直到从墙体边缘绕过去，从而解决过度转向以及不足转向的问题。
32.实施例三基于实施例一、实施例二中的步骤，无人机在如图6所示的仿真场景中飞行。
33.所述步骤s3包括以下子步骤：
步骤s3-1、根据步骤s2的求解，获得预测轨迹函数f（t）；步骤s3-2、对轨迹函数f（t）求导，分别获得速度、加速度、加加速度等参数随时间变化的函数v（t）、a（t）、jerk（t）；步骤s3-3、构建轨迹起点的位置、速度和加速度约束条件，以及终点的位置、速度和加速度约束条件；步骤s3-4、对预测轨迹函数f（t）进行优化，最终获得一条加速度变化累计损失最小的轨迹方程式。
34.所述步骤s3-1中的预测轨迹函数f（t）为：其中，p为轨迹上的位置点，p0,p1,p2,pn是轨迹参数。
35.化为向量形式：对轨迹函数进行求导，所述s3-2中的速度、加速度、jerk等参数随时间变化的函数v（t）、a（t）、jerk（t）为：jerk（t）为：jerk（t）为：其中，k表示k阶导数，轨迹导数的通项公式如图7所示。
36.对轨迹首尾的位置、速度和加速度进行约束，这就有6个等式约束了，所以优化参数必须提供6个以上的自由度，而5阶多项式有6个系数，所以符合要求的多项式的最小阶次n=5。
37.因此jerk可表示为：优化函数转化为:令，对求平方，得到：令，得到得到
t取单位时间：令：所述步骤s3-3中的初始状态的约束条件如下：3中的初始状态的约束条件如下：3中的初始状态的约束条件如下：其中，为起点位置点； 为起点速度；为起点加速度。
38.所述步骤s3-3中的终止状态的约束条件如下：3中的终止状态的约束条件如下：3中的终止状态的约束条件如下：其中， 为终点位置点； 为终点速度；为终点加速度。
39.即：从而获得一条加速度变化累计损失最小的轨迹f（t），将这个二次规划出来的路径传递给无人机的控制模块即可实现优化控制。
40.如图8所示为优化后的无人机飞行路线。
41.以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，这些改进也应视为本发明的保护范围。