CN110471441A - 一种基于激光探测的四旋翼无人机避障方法 - Google Patents

Abstract

本发明公开了一种基于激光探测的四旋翼无人机避障方法。该方法为：首先使用hector slam方法，基于激光雷达建立飞行场地的静态栅格地图，使用A*算法规划出从起始点到目标点的最短路径，并利用pixhawk飞控板内部的惯性测量单元计算出四旋翼无人机的里程信息；然后使用自适应蒙特卡洛定位算法，获得四旋翼无人机相对于地图的坐标转换；利用激光雷达在无人机飞行过程中建立局部代价地图；最后针对飞行过程中出现的动态障碍物，依据建立的局部代价地图，使用动态窗口算法对其进行规避，并进行局部路径规划，直至无人机回归到由A*算法得到的全局最短航路中。本发明具有方法简单方便、定位速度快、自主能力强、实时性好的优点。

Description

一种基于激光探测的四旋翼无人机避障方法

技术领域

本发明涉及多旋翼无人机控制与导航技术，特别是一种基于激光探测的四旋翼无人机避障方法。

背景技术

无人飞行器以嵌入式程序控制或者无线遥控等方式取代人来执行各种任务。因其具有成本低、伤亡风险小、机动性能好、操控方便、生存能力强等优点，在军事和民用方面的应用逐渐增多。在军事方面，无人飞行器通过在几次局部战争中执行侦察、通信中继、攻击等任务的出色表现，逐渐得到各国的认可，同时因其仍有巨大的潜力需要开发，各国纷纷加强了对各类用途的无人飞行器的研究。在民用方面，无人飞行器在航拍、灾情监测等方面有着广泛的应用。

近年来，许多学者对室内环境下四旋翼飞行器的避障与路径规划展开了深入的研究，取得了不错的成果，但是仍然存在一些问题需要解决和完善。这主要是因为室内环境比较复杂，障碍物的分布未知、定位精度比较低等原因造成的。首先，由于在室内环境下很难接收到GPS信号，因此四旋翼飞行器很难通过GPS等卫星设备来获得自身的位置信息，且GPS的定位精度不高，对于室内这种相对于比较狭窄的地方，很容易碰到障碍物。虽然可以通过惯性测量单元提供的加速度信息迭代计算出四旋翼飞行器的位置信息，但是由于四旋翼飞行器上安装的惯性测量单元对于加速度的测量精度不高，会造成计算出的位置信息与实际的位置之间存在比较大的偏差，因此需要使用其他传感器来提供更准确的位置信息，来帮助四旋翼飞行器实现避障与路径规划。由于室内环境一般比较狭窄，障碍物分布不均，其形状、大小等特征也不明确，这些都给四旋翼无人机的室内避障与路径规划增加了难度。

四旋翼飞行器对周围环境缺乏认知，因此需要合适的传感器来协助四旋翼飞行器感知周围环境，而选择适合的传感器是实现四旋翼飞行器室内避障的关键。激光雷达具有测量精度高、受环境影响小、测量范围广以及对环境的测量速度快等优点，可以帮助四旋翼飞行器快速且准确的感知周围环境，从而帮助四旋翼飞行器进行避障与路径规划，而如何通过激光雷达构建未知环境信息并设计导航策略，具有重要的研究价值。

目前采用的路径规划方法主要分为局部路径规划方法和全局路径规划方法两种，其中全局路径规划方法主要有A*算法、随机搜索法和拓扑法，局部路径规划方法主要有人工势场法、遗传算法和动态窗口算法。

文献1(谭海燕.基于改进A*算法的无人机航迹规划[J/OL].飞行力学)针对经典A*算法规划的航迹贴近威胁区，容易导致无人机与障碍物产生碰撞的问题，提出在威胁区外设定环状影响区的方法，同时引入航迹平滑以及死区修正算法，得到了一条最优航迹。但该方法只适用于仅存在静态障碍物的场景，无法避开动态障碍物。文献2(章志诚.基于ADRC的四旋翼飞行器自主避障控制系统研究[D].浙江大学,2017.)通过改进人工势场法中的启发函数，实现了四旋翼无人机的自主避障。该方法能够实现对动态障碍物的规避，但航迹没有得到优化。

发明内容

本发明的目的在于提供一种简单方便、定位速度快、自主能力强、实时性好的基于激光探测的四旋翼无人机避障方法，能够得到较优的航迹，而且能够应对存在动态障碍物的场景。

实现本发明目的的技术解决方案为：一种基于激光探测的四旋翼无人机避障方法，包括以下步骤：

步骤1、使用hector slam方法，基于激光雷达建立飞行场地的静态栅格地图；

步骤2、根据获得的全局静态栅格地图，使用A*算法规划出从起始点到目标点的最短路径；

步骤3、利用pixhawk飞控板内部的惯性测量单元计算出四旋翼无人机的里程信息；

步骤4、基于计算出的里程信息，使用自适应蒙特卡洛定位算法，获得四旋翼无人机相对于地图的坐标转换，实现四旋翼无人机的定位；

步骤5、利用激光雷达在无人机飞行过程中建立局部代价地图，即局部障碍物层与膨胀层；

步骤6、针对飞行过程中出现的动态障碍物，依据建立的局部代价地图，使用动态窗口算法对所述动态障碍物进行规避，并进行局部路径规划，直至无人机回归到由A*算法得到的全局最短航路中。

进一步地，步骤2中所述的A*算法，是将Dijkstra算法和启发式搜索相结合，用于找到最短路径。

进一步地，步骤3中所述的惯性测量单元，包括加速度计、陀螺仪和磁力计；所述陀螺仪用于获得四旋翼无人机的角速度；所述加速度计用于获得四旋翼无人机的运动加速度，对运动加速度进行积分得到四旋翼无人机的线速度。

进一步地，步骤4中所述的自适应蒙特卡洛定位算法，是指在传统蒙特卡洛定位算法的基础上，通过在丢失标识所有位姿粒子的情况下随机注入粒子，实现对无人机的重新定位。

进一步地，步骤5中所述的障碍物层，用于更新步骤1获得的静态栅格地图，使四旋翼无人机能实时获取新出现的或已被移除的障碍物；所述的膨胀层，用于扩张四旋翼无人机周边的障碍物，防止无人机在避障时撞到障碍物。

进一步地，步骤6中所述的动态窗口算法，具体如下：

根据运动物体的实时速度和加速度以及速度与加速度阈值，将该物体的速度限制在一个动态窗口内，然后在此窗口范围内进行速度采样，并根据采样的速度进行路径估计，以时间间隔Δt采样线速度v和角速度ω，根据物体的运动模型预估每一个采样结果对应的目标物体的运动轨迹，然后根据评价函数对每一条轨迹进行评价，选择最优估计轨迹对应的参数作为目标物体下一时刻的运动控制参数；

速度采样窗口的产生方法如下：

1)无人机受本身的约束，拥有最大速度和最小速度的限制：

V_s＝{(v,ω)|v∈[v_min,v_max],ω∈[ω_min,ω_max]}

其中，v_min、v_max、ω_min、ω_max分别代表最小线速度，最大线速度，最小角速度，最大角速度；

2)无人机受电机性能的约束，拥有最大加速度和减速度的限制，无人机的实际速度已知，根据无人机能达到的最大加速度和减速度，计算出下一时刻所能达到的速度范围V_d：

其中，v_a、ω_a为实际的线速度与角速度，为最大线加速度和最大角加速度；

3)为了实现避障功能，无人机需要在碰到最近的障碍物时停下来，即刹车距离V_a小于与最近障碍物的距离：

其中，dist(v,ω)为当前轨迹与障碍物的最近距离，和分别代表无人机的最大线加速度和角加速度；

综上，速度采样窗口V_r为：

V_r＝V_s∩V_a∩V_d

所述动态窗口算法采用的轨迹评价函数G(v,ω)如下：

G(v,ω)＝σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))

其中：

heading(v,ω)为方位角评价函数，用来评价在当前速度的控制下，到达轨迹末端无人机的朝向与设定的目标之间的夹角；

dist(v,ω)为无人机与障碍物最小距离评价函数，用来评价当前轨迹与最近的障碍物之间的最小距离；如果在当前轨迹上没有障碍物，则将该评价函数设为常数；

velocity(v,ω)为速度评价函数，用来评价当前轨迹对应的无人机速度。

本发明与现有技术相比，其显著优点为：(1)通过hector slam方法建立飞行场地的静态栅格地图，获得静态障碍物信息，只需依赖激光雷达便可以建立全局地图，更加简单方便；(2)使用A*算法进行全局路径规划，加快了寻径速度；(3)采用自适应蒙特卡洛定位算法对无人机进行室内定位，解决了“机器人劫持”问题；(4)实时监测无人机周围的动态障碍物并通过动态窗口算法进行局部路径规划，可以针对突然出现的障碍物进行避障。

附图说明

图1为本发明基于激光探测的四旋翼无人机避障方法的流程示意图。

图2为本发明实施例中使用hector slam方法构建的静态栅格地图。

图3为本发明实施例四旋翼无人机实际飞行中静态地图与代价地图的叠加地图。

图4为本发明实施例中无人机飞行的实际航迹图。

具体实施方式

本发明基于激光探测的四旋翼无人机避障方法，包括以下步骤：

步骤1、使用hector slam方法，基于激光雷达建立飞行场地的静态栅格地图；

步骤2、根据获得的全局静态栅格地图，使用A*算法规划出从起始点到目标点的最短路径；

步骤3、利用pixhawk飞控板内部的惯性测量单元计算出四旋翼无人机的里程信息；

步骤4、基于计算出的里程信息，使用自适应蒙特卡洛定位算法，获得四旋翼无人机相对于地图的坐标转换，实现四旋翼无人机的定位；

步骤5、利用激光雷达在无人机飞行过程中建立局部代价地图，即局部障碍物层与膨胀层；

步骤6、针对飞行过程中出现的动态障碍物，依据建立的局部代价地图，使用动态窗口算法对所述动态障碍物进行规避，并进行局部路径规划，直至无人机回归到由A*算法得到的全局最短航路中。

进一步地，步骤2中所述的A*算法，是将Dijkstra算法和启发式搜索相结合，用于找到最短路径。

进一步地，步骤3中所述的惯性测量单元，包括加速度计、陀螺仪和磁力计；所述陀螺仪用于获得四旋翼无人机的角速度；所述加速度计用于获得四旋翼无人机的运动加速度，对运动加速度进行积分得到四旋翼无人机的线速度。

进一步地，步骤4中所述的自适应蒙特卡洛定位算法，是指在传统蒙特卡洛定位算法的基础上，通过在丢失标识所有位姿粒子的情况下随机注入粒子，实现对无人机的重新定位，具体如下：步骤3所述的惯性测量单元通过积分得到无人机的位置信息，针对积分带来的累计误差，采用粒子滤波的方式，根据步骤2得到的全局静态栅格地图以及激光雷达实时获得的地图信息，对惯性测量单元积分出来的位置信息进行修正，从而实现无人机的定位。

进一步地，步骤5中所述的障碍物层，用于更新步骤1获得的静态栅格地图，使四旋翼无人机能实时获取新出现的或已被移除的障碍物；所述的膨胀层，用于扩张四旋翼无人机周边的障碍物，防止无人机在避障时撞到障碍物。

进一步地，步骤6中所述的动态窗口算法，具体如下：

根据运动物体的实时速度和加速度以及速度与加速度阈值，将该物体的速度限制在一个动态窗口内，然后在此窗口范围内进行速度采样，并根据采样的速度进行路径估计，以时间间隔Δt采样线速度v和角速度ω，根据物体的运动模型预估每一个采样结果对应的目标物体的运动轨迹，然后根据评价函数对每一条轨迹进行评价，选择最优估计轨迹对应的参数作为目标物体下一时刻的运动控制参数；

速度采样窗口的产生方法如下：

1)无人机受本身的约束，拥有最大速度和最小速度的限制：

V_s＝{(v,ω)|v∈[v_min,v_max],ω∈[ω_min,ω_max]}

其中，v_min、v_max、ω_min、ω_max分别代表最小线速度，最大线速度，最小角速度，最大角速度；

2)无人机受电机性能的约束，拥有最大加速度和减速度的限制，无人机的实际速度已知，根据无人机能达到的最大加速度和减速度，计算出下一时刻所能达到的速度范围V_d：

其中，v_a、ω_a为实际的线速度与角速度，为最大线加速度和最大角加速度；

3)为了实现避障功能，无人机需要在碰到最近的障碍物时停下来，即刹车距离V_a小于与最近障碍物的距离：

其中，dist(v,ω)为当前轨迹与障碍物的最近距离，和分别代表无人机的最大线加速度和角加速度；

综上，速度采样窗口V_r为：

V_r＝V_s∩V_a∩V_d

所述动态窗口算法采用的轨迹评价函数G(v,ω)如下：

G(v,ω)＝σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))

其中：

heading(v,ω)为方位角评价函数，用来评价在当前速度的控制下，到达轨迹末端无人机的朝向与设定的目标之间的夹角；

dist(v,ω)为无人机与障碍物最小距离评价函数，用来评价当前轨迹与最近的障碍物之间的最小距离；如果在当前轨迹上没有障碍物，则将该评价函数设为常数；

velocity(v,ω)为速度评价函数，用来评价当前轨迹对应的无人机速度。

下面结合附图和具体实施例对本发明技术方案做进一步的详细说明。

实施例

结合图1，本实施例基于激光探测的四旋翼无人机避障方法，包括以下步骤：

步骤1、使用hector slam方法，利用激光雷达建立飞行场地的静态栅格地图；

步骤2、基于获得的全局静态栅格地图，使用A*算法规划出从起始点到目标点的最短路径；

所述的A*算法，是将Dijkstra算法和启发式搜索相结合，在减少运行时间的同时确保找到最短路径；

步骤3、利用pixhawk飞控板内部的惯性测量单元(Inertial Measurement Unit，IMU)计算出四旋翼无人机里程信息；

所述的惯性测量单元，包括加速度计、陀螺仪和磁力计；所述陀螺仪用于获得四旋翼无人机的角速度；所述加速度计用于获得四旋翼无人机的运动加速度，对运动加速度进行积分得到四旋翼无人机的线速度；

步骤4、基于计算出的里程信息，使用自适应蒙特卡洛定位算法，获得四旋翼无人机相对于地图的坐标转换，实现四旋翼无人机的定位；

所述的自适应蒙特卡洛定位算法，是在传统蒙特卡洛定位算法的基础上进行改进，可以在丢失所有正确位姿附近粒子的情况下，通过随机注入粒子，实现重新定位，具体如下：步骤3所述的惯性测量单元通过积分得到无人机的位置信息，针对积分带来的累计误差，采用粒子滤波的方式，根据步骤2得到的全局静态栅格地图以及激光雷达实时获得的地图信息，对惯性测量单元积分出来的位置信息进行修正，从而实现无人机的定位。

步骤5、利用激光雷达在无人机飞行过程中建立局部代价地图，即局部障碍物层与膨胀层；

所述的障碍物层，用于更新步骤1获得的静态栅格地图，使四旋翼无人机能实时获取新出现的或已被移除的障碍物；所述的膨胀层，用于扩张四旋翼无人机周边的障碍物，防止无人机在避障时撞到障碍物；

步骤6、针对飞行过程中出现的动态障碍物，依据建立的局部代价地图，使用动态窗口算法对其进行规避，并进行局部路径规划，直至无人机回归到由A*算法得到的全局最短航路中；

所述的动态窗口算法，具体如下：

根据运动物体的实时速度和加速度以及速度与加速度阈值，将该物体的速度限制在一个动态窗口内，然后在此窗口范围内进行速度采样，并根据采样的速度进行路径估计，以时间间隔Δt采样线速度v和角速度ω，根据物体的运动模型预估每一个采样结果对应的目标物体的运动轨迹，然后根据评价函数对每一条轨迹进行评价，选择最优估计轨迹对应的参数作为目标物体下一时刻的运动控制参数；

速度采样窗口的产生方法如下：

1)无人机受本身的约束，拥有最大速度和最小速度的限制：

V_s＝{(v,ω)|v∈[v_min,v_max],ω∈[ω_min,ω_max]}

其中，v_min、v_max、ω_min、ω_max分别代表最小线速度，最大线速度，最小角速度，最大角速度；

2)无人机受电机性能的约束，拥有最大加速度和减速度的限制，无人机的实际速度已知，根据无人机能达到的最大加速度和减速度，计算出下一时刻所能达到的速度范围V_d：

其中，v_a、ω_a为实际的线速度与角速度，为最大线加速度和最大角加速度；

3)为了实现避障功能，无人机需要在碰到最近的障碍物时停下来，即刹车距离V_a小于与最近障碍物的距离：

其中，dist(v,ω)为当前轨迹与障碍物的最近距离，和分别代表无人机的最大线加速度和角加速度；

综上，速度采样窗口V_r为：

V_r＝V_s∩V_a∩V_d

所述动态窗口算法采用的轨迹评价函数G(v,ω)如下：

G(v,ω)＝σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))

其中：

heading(v,ω)为方位角评价函数，用来评价在当前速度的控制下，到达轨迹末端无人机的朝向与设定的目标之间的夹角。角度越小，评分得分越高。

dist(v,ω)，即无人机与障碍物最小距离评价函数，用来评价当前轨迹与最近的障碍物之间的最小距离。如果在当前轨迹上没有障碍物，则将其设为常数。距离越大，评分得分越高。

velocity(v,ω),速度评价函数，当前轨迹对应的无人机速度，速度越大，无人机更快到达目标，评价越高。

本实施例基于四旋翼无人机平台，机体搭载pixhawk飞控板，UTM30-LX-EW激光雷达，以及树莓派3B+。将本发明基于激光探测的四旋翼无人机避障方法运行于树莓派3B+平台上，pixhawk与树莓派通过串口进行信息传递，由树莓派发送速度量给pixhawk飞控板，飞控板控制四旋翼无人机的飞行状态。

本实施例设计让四旋翼无人机沿场地的对角线飞行，并在四旋翼无人机飞行过程中，在其航路上添加障碍物。对比图2、图3可以看出，在四旋翼无人机飞行过程中，成功检测到了新出现的三个障碍物。四旋翼无人机实际飞行的航迹图如图4所示，从图中可以看出，四旋翼无人机成功避开了新设置的三个障碍物。从以上结果可以看出，本发明基于激光探测的四旋翼无人机的避障方法，实现了无人机的全局路径规划以及对动态障碍物的避障功能，具有方法简单方便、定位速度快、自主能力强、实时性好的优点。

Claims (6)

1.一种基于激光探测的四旋翼无人机避障方法，其特征在于，包括以下步骤：

步骤1、使用hector slam方法，基于激光雷达建立飞行场地的静态栅格地图；

步骤2、根据获得的全局静态栅格地图，使用A*算法规划出从起始点到目标点的最短路径；

步骤3、利用pixhawk飞控板内部的惯性测量单元计算出四旋翼无人机的里程信息；

步骤4、基于计算出的里程信息，使用自适应蒙特卡洛定位算法，获得四旋翼无人机相对于地图的坐标转换，实现四旋翼无人机的定位；

步骤5、利用激光雷达在无人机飞行过程中建立局部代价地图，即局部障碍物层与膨胀层；

步骤6、针对飞行过程中出现的动态障碍物，依据建立的局部代价地图，使用动态窗口算法对所述动态障碍物进行规避，并进行局部路径规划，直至无人机回归到由A*算法得到的全局最短航路中。

2.根据权利要求1所述的基于激光探测的四旋翼无人机避障方法，其特征在于，步骤2中所述的A*算法，是将Dijkstra算法和启发式搜索相结合，用于找到最短路径。

3.根据权利要求1所述的基于激光探测的四旋翼无人机避障方法，其特征在于，步骤3中所述的惯性测量单元，包括加速度计、陀螺仪和磁力计；所述陀螺仪用于获得四旋翼无人机的角速度；所述加速度计用于获得四旋翼无人机的运动加速度，对运动加速度进行积分得到四旋翼无人机的线速度。

4.根据权利要求1所述的基于激光探测的四旋翼无人机避障方法，其特征在于，步骤4中所述的自适应蒙特卡洛定位算法，是指在传统蒙特卡洛定位算法的基础上，通过在丢失标识所有位姿粒子的情况下随机注入粒子，实现对无人机的重新定位。

5.根据权利要求1所述的基于激光探测的四旋翼无人机避障方法，其特征在于，步骤5中所述的障碍物层，用于更新步骤1获得的静态栅格地图，使四旋翼无人机能实时获取新出现的或已被移除的障碍物；所述的膨胀层，用于扩张四旋翼无人机周边的障碍物，防止无人机在避障时撞到障碍物。

6.根据权利要求1所述的基于激光探测的四旋翼无人机避障方法，其特征在于，步骤6中所述的动态窗口算法，具体如下：

根据运动物体的实时速度和加速度以及速度与加速度阈值，将该物体的速度限制在一个动态窗口内，然后在此窗口范围内进行速度采样，并根据采样的速度进行路径估计，以时间间隔Δt采样线速度v和角速度ω，根据物体的运动模型预估每一个采样结果对应的目标物体的运动轨迹，然后根据评价函数对每一条轨迹进行评价，选择最优估计轨迹对应的参数作为目标物体下一时刻的运动控制参数；

速度采样窗口的产生方法如下：

1)无人机受本身的约束，拥有最大速度和最小速度的限制：

V_s＝{(v,ω)|v∈[v_min,v_max],ω∈[ω_min,ω_max]}

其中，v_min、v_max、ω_min、ω_max分别代表最小线速度，最大线速度，最小角速度，最大角速度；

2)无人机受电机性能的约束，拥有最大加速度和减速度的限制，无人机的实际速度已知，根据无人机能达到的最大加速度和减速度，计算出下一时刻所能达到的速度范围V_d：

其中，v_a、ω_a为实际的线速度与角速度，为最大线加速度和最大角加速度；

3)为了实现避障功能，无人机需要在碰到最近的障碍物时停下来，即刹车距离V_a小于与最近障碍物的距离：

其中，dist(v,ω)为当前轨迹与障碍物的最近距离，和分别代表无人机的最大线加速度和角加速度；

综上，速度采样窗口V_r为：

V_r＝V_s∩V_a∩V_d

所述动态窗口算法采用的轨迹评价函数G(v,ω)如下：

G(v,ω)＝σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))

其中：

heading(v,ω)为方位角评价函数，用来评价在当前速度的控制下，到达轨迹末端无人机的朝向与设定的目标之间的夹角；

dist(v,ω)为无人机与障碍物最小距离评价函数，用来评价当前轨迹与最近的障碍物之间的最小距离；如果在当前轨迹上没有障碍物，则将该评价函数设为常数；

velocity(v,ω)为速度评价函数，用来评价当前轨迹对应的无人机速度。