CN107656545A - 一种面向无人机野外搜救的自主避障与导航方法 - Google Patents

Abstract

本发明提供的是一种面向无人机野外搜救的自主避障与导航方法，包括基于GPS信号的全局导航和基于双目视觉与超声波结合的自主避障与导航算法，无人机首先按照规划好的路径，在GPS信号的导航下进行搜索，发现目标后，采用基于双目视觉和超声波结合的自主避障与导航算法，其中双目相机获取含有深度信息的视差图，把视差图重投影到三维空间得到点云，根据点云建立代价地图，使用代价地图进行路径重规划以避障，在无GPS信号时，使用视觉惯性里程计(VIO)保证无人机按照规划的路径飞行，并采用超声波传感器以探测并躲避无人机下方的障碍，从而接近被救目标，实现了在野外搜救场景下的无人机自主避障与导航。

Description

一种面向无人机野外搜救的自主避障与导航方法

技术领域

本发明属于无人飞行器避障领域，尤其涉及一种面向无人机野外搜救的自主避障与导航方法。

背景技术

近年来，随着无人机控制技术的逐渐成熟，其应用也越来越广泛，包括农间协助作业、工业电力线巡检等，其中涉及到许多方面的技术，如目标检测和跟踪、飞行器运动控制、远距离信号传输等。自主避障与导航也是无人机技术中的重要部分，是指无人机通过传感器主动感知周围的障碍物，根据得到的障碍物信息进行新的路径规划，以躲避周围的障碍物，并运动到目标位置，是保证无人机安全飞行的基础，也是无人机顺利完成各种作业的前提。

在将无人机技术运用到野外搜救中时，首先让无人机在GPS信号的反馈下沿预设路径飞行，此时无人机离地面较高，能够避开地形的影响，当无人机发现待搜救目标后，需要接近目标已进行近距离观察或投放物资，在此过程中，由于野外环境复杂，树林、山崖等障碍物较多，要求无人机具有可靠的自主避障能力和导航能力。早前有使用多个超声波传感器进行避障的方案，但在野外环境下，障碍密集，仅采用超声波传感器不能得到完整的障碍物信息，不能探测树枝、电线等细长障碍物，且不能保证实时更新，从而不能探测移动的障碍物。目前，在野外环境中，还没有一种较好的能够得到丰富的障碍物信息，能够探测细长障碍物，并进行实时更新和路径规划的算法，来保证无人机顺利完成野外搜救任务。

发明内容

本发明的目的在于提供一种基于双目视觉和超声波结合的算法，该算法能够实现在野外环境下的无人机自主避障与导航，从而完成野外搜救的任务。

本发明提供的自主避障与导航算法包括以下具体步骤：

一种面向无人机野外搜救的自主避障与导航方法，其特征在于，包括：

步骤1:无人机在GPS信号的导航下，按照预设的路径进行搜索，具体包括：

步骤101：在无人机配套软件上根据搜救环境，设置全局的搜索路径；

步骤102：无人机按照预设的路径，在GPS信号的导航下进行高空搜索；

步骤2:无人机发现目标后，通过基于双目视觉和超声波的算法实现自主避障和导航，从而接近目标，具体包括：

步骤201：当无人机在步骤1中发现目标后，开始下降并接近目标；

步骤202:无人机通过机载的双目相机拍摄周围环境，并得到视差图，根据双目测距原理，从视差图得到周围物体的深度信息；

步骤203：将视差图重投影到三维空间，从而得到点云数据；

步骤204：根据点云数据表示的障碍信息建立代价地图；

步骤205：根据代价地图上不断更新的障碍信息，调整最佳路径，并产生各个移动方向上的速度命令以便无人机避开障碍物并接近目标；

步骤206:采用超声波传感器探测飞机下方的地面，从而使无人机与地面始终保持一定的安全距离。

在上述的一种面向无人机野外搜救的自主避障与导航方法，所述步骤201的具体实现方法包括：

当无人机发现待救援目标后，使用双目相机可以根据左右两个相机的视差计算出物体的距离信息；使用两个相机可以得到左右视图，两个视图必定产生差异，称之为“左右视差”，定义前方物体上的点p12对应于左右视图上的点p1和点p2；则p1,p2,p12构成一个三角形，通过求解这个三角形的各边长，顶点位置，就可以算出点p12的坐标，再进一步也就可以得到p12到p1p2的距离，也就是p12到p1p2的深度；

由于采用的双目相机具有较高的分辨率，能够清晰地得到周围物体的影像，包括树枝、电线等超声波不能探测的细长障碍，也能够以这种方法进行检测并得到它们的深度信息。

在上述的一种面向无人机野外搜救的自主避障与导航方法，所述步骤202的具体实现方法包括：使用OpenCV开源计算机视觉库，其中有基于块匹配算法的由双目视图计算差异图的实现；差异图重投影到三维空间就可以得到点云(PointCloud)，；而点云数据恰好是对障碍信息一个最通用的描述；采用的机器人操作系统ROS实现点云的获取，最后根据双目视觉模块生成的可视化三维点云图，使用ROS开源工具RVIZ可以看到，障碍信息已经被可视化，得到的三维点云可以让无人机了解前方一定距离内的障碍信息，为避障提供了条件。

在上述的一种面向无人机野外搜救的自主避障与导航方法，所述步骤203的具体实现方法包括：云数据得到后，就可以得到障碍信息，需要解决的问题是如何躲避这些障碍；本算法使用基于代价地图的路径重规划躲避这些障碍；首先，建立一个空的网格地图，以无人机当前位置为参考，根据点云信息，有点云出现的位置设为Occupied，没有出现的网格为Free；点云密度大于某个值的地方就认定为有固体障碍，从而实现代价地图的建立。

在上述的一种面向无人机野外搜救的自主避障与导航方法，所述步骤204的具体实现方法包括：

代价地图分为全局代价地图和局部代价地图；由于本算法的应用场景是局部区域内未知地图的避障飞行，因此只能使用局部代价地图，这个是根据不断收到的传感器数据动态更新的地图；实际的避障中，首先需要初始化一个全局的规划器；这个全局规划器负责生成一个从指定的“起点”到指定的“目标点”之间的最短路径，通常使用Dijkstra最短路径算法，这个生成的最短路径为最初状态的最佳路径；然后根据局部代价地图，局部规划器将根据最新的障碍信息不断调整更新最佳路径，并产生各个移动移动方向上的速度命令以便无人机避开障碍物；这些速度命令一般采用动态窗口方法来生成，步骤如下：

步骤204A、在机器人控制空间(dx,dy,dθ)中离散采样；

步骤204B、对于每个采样速度，从机器人的当前状态执行正向仿真，以预测如果使用采样速度行进一段时间的运动会发生什么；

步骤204C、使用包括诸如以下特征的度量来评估(打分)，如图4所示，从而正向模拟产生的每个轨迹：接近障碍物，接近目标，接近全局路径和速度；然后丢弃非法轨迹(与障碍物碰撞的轨迹以及得分较低的轨迹)；

步骤204D、选择最高得分的轨迹，并将相关速度发送到移动基底；

步骤204E、清除得分并重复。

在上述的一种面向无人机野外搜救的自主避障与导航方法，所述步骤205的具体实现方法包括：

建立视觉惯性里程计；考虑到野外环境复杂，地形多变，无人机在搜救过程中可能会进入山崖下方GPS信号微弱的区域，此时不能再通过GPS信号实现绝对定位，需采用相对定位的方法来确定无人机的位置，里程计是一种广泛使用的相对定位方法；常用的里程计有惯性里程计(IO)和视觉里程计(VO)，惯性里程计(IO)利用加速度计和陀螺仪进行定位，由于漂移误差和积分导致的累积误差，惯性里程计在运行一段时间后便会出现较大的定位偏差，需要进行校正；视觉里程计(VO)则是通过相机感知周围环境，采集图像，然后根据特征匹配和运动约束等方法来估计相机的姿态，比较准确，不会有漂移和累积误差；视觉里程计的实现主要包括以下4个步骤：

步骤205A1、特征检测；对于双目相机拍摄的图像，先进行特征点检测，常用的特征有角点，方向梯度直方图(HOG)，局部二值模式(LBP)等，本算法采用了尺度不变特征(SIFT)，能够保持尺度变换和旋转的不变性；

步骤205A2、立体匹配；对于双目相机两个摄像头拍摄的图像，根据双目测距原理，得到图像上对应部分的深度信息，从而得到各点的三维坐标；

步骤205A3、时域特征点跟踪；对同一摄像头在不同时刻拍摄的图像之间特征点进行匹配，并通过三角测量得到特征点在相机坐标系下的三维坐标；

步骤205A4、运动估计；在完成特征点时域跟踪的基础上，便可以通过无人机在相邻采样间隔时间内在同一坐标系下的位姿变化，对它进行运动估计，采用了随机抽样一致性方法(RANSAC)进行运动估计；

尽管视觉里程计较为准确，但会由于场景特征的缺乏而导致跟踪的失败，为此，本算法将视觉信息与惯性测量单元(IMU)的数据进行融合，得到视觉惯性里程计(VIO)，从而实现在GPS信号缺失情况下的对无人机的准确定位；本算法采用的融合方法为MSCKF，步骤如下：

步骤205B1、初始化；初始化相机和IMU的参数，并构造MSCKF状态向量；

步骤205B2、读取IMU数据，估计新的MSCKF状态向量和对应的协方差矩阵；

步骤205B3、开始对图像数据进行处理，在MSCKF状态向量中增加当前帧的摄像机位姿，若位姿数大于滑动窗口大小的范围，去除状态变量中最早的视图对应的摄像机位姿；

步骤205B4、提取图像特征并匹配，去除外点；

步骤205B5、处理所有提取的特征；判断当前特征是否是之前视图中已经观察到的特征，如果是，且当前帧还可以观测到该特征，则加入该特征的track列表，否则将该特征的track加入到剩余列表，用于更新MSCKF的状态变量,如果不是，

给该特征分配新的特征ID，并加入到当前视图可观测特征的集合；

步骤205C1、循环遍历剩余列表中的track，用于更新MSCKF的状态变量；

步骤205C2、计算新的MSCKF状态向量的协方差矩阵；

步骤205C3、管理状态变量，绘制运动轨迹；

通过以上方法便能融合视觉信息和IMU的数据，以实现在无GPS信号场景下对无人机的定位，从而保证导航任务的完成。

在上述的一种面向无人机野外搜救的自主避障与导航方法，所述步骤206的具体实现方法包括：

搜救系统在野外工作时，与地面保持一定的距离是十分必要的；因为野外地形无法预测，如果固定设置无人机飞行高度，遇到某些起伏地形时，无人机则无法跟随地形相应地提升飞行高度，也就无法越过该地形继续搜索；此外，野外搜救时，无人机很有可能有降低飞行高度的需求，如自动递送急救物资，或者靠近目标做进一步确认；此类情况下，需要躲避无人机下方的障碍；此时超声波是一个很好的避障工具；利用超声波传感器，机载设备能以20Hz的频率接收到下方的障碍信息；超声波的有效测距范围为0.02—20m，在飞行过程中，采用下面的避障策略来躲避下方的障碍：

系统初始化时，需要设定正常飞行的高度，以及安全距离；在飞行过程中可以根据任务的变化来设定正常飞行高度；由此策略，无人机系统将与下方障碍保持一定的距离，从而保证安全飞行。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1.通过双目视觉能够得到无人机周围完整的障碍物信息，包括树枝、电线等细长障碍物，保证了无人机对周围环境探测的准确性。

2.代价地图可以进行实时更新，从而能够处理移动障碍的场景，能够应对更复杂的情形。

3.避免了气压计不能在地势起伏区域确定相对高度的弊端，采用超声波传感器探测下方地形，使无人机能自主爬山。

附图说明

图1为视差到深度转换原理图。

图2为三维点云图。

图3为可视化三维点云图。

图4为局部规划器对正向模拟路径打分图。

图5为无人机地形跟随示意图。

图6是本发明的方法流程示意图。

具体实施方式

本发明提供了一种面向无人机野外搜救的自主避障与导航方法。为使本发明的目的，技术方案及效果更加清楚、明确，以下参考附图对本发明进一步详细说明。

本发明主要包含两个部分，基于GPS信号导航的全局搜索和接近目标过程中的自主避障和导航。所述面向无人机野外搜救的自主避障与导航方法，主要包含以下步骤：

步骤S100:无人机在GPS信号的导航下，按照预设的路径进行搜索。

步骤S200:无人机发现目标后，通过基于双目视觉和超声波的算法实现自主避障和导航，从而接近目标。

本发明所依靠的无人机系统包括旋翼无人机，机载双目相机，机载红外相机，超声波传感器，机载X86计算平台，地面控制端包括遥控器，配套控制软件。本发明为一种面向无人机野外搜救的自主避障与导航方法，首先通过配套软件设定全局搜索的路径，然后让无人机在GPS信号的导航下，按照预设的路径进行搜索，当无人机发现目标后，通过基于双目视觉的方法得到周围障碍物的信息，通过位于飞机下方的超声波传感器保持与地面的安全距离，从而实现此阶段的自主避障和导航，并最终接近目标。下面对各个步骤进行详细描述：

所述一种面向无人机野外搜救的自主避障与导航方法，其中，步骤S100具体包括：

步骤S101：根据搜救环境，在无人机配套软件上设置全局的搜索路径。

步骤S102：无人机按照预设的路径，在GPS信号的导航下进行搜索，此时无人机飞行高度较高，不受地面障碍物的影响，此阶段目标为发现被困人员。

所述一种面向无人机野外搜救的自主避障与导航方法，其中，步骤S200具体包括：

步骤S201:当无人机发现待救援目标后，使用双目相机可以根据左右两个相机的视差计算出物体的距离信息。使用两个相机可以得到左右视图，两个视图必定产生差异，称之为“左右视差”，从左右视差(disparity)到“深度”(depth)的转换原理如图1所示。

设前方物体上的点p₁₂对应于左右视图上的点p₁和点p₂。则p₁,p₂,p₁₂构成一个三角形，通过求解这个三角形的各边长，顶点位置，就可以算出点p₁₂的坐标，再进一步也就可以得到p₁₂到p₁p₂的距离(深度)。

由于采用的双目相机具有较高的分辨率，能够清晰地得到周围物体的影像，包括树枝、电线等超声波不能探测的细长障碍，也能够以这种方法进行检测并得到它们的深度信息。

步骤S202：在本算法实现中，使用了OpenCV开源计算机视觉库，其中有基于块匹配算法的由双目视图计算差异图的实现。差异图重投影到三维空间就可以得到点云(PointCloud)，如图2所示。而点云数据恰好是对障碍信息一个最通用的描述。本算法采用的机器人操作系统ROS可以实现点云的获取。

图3是本算法根据双目视觉模块生成的可视化三维点云图，使用ROS开源工具RVIZ可以看到，障碍信息已经被可视化，得到的三维点云可以让无人机了解前方一定距离内的障碍信息，为避障提供了条件。

步骤S203:点云数据得到后，就可以得到障碍信息，需要解决的问题是如何躲避这些障碍。本算法使用基于代价地图的路径重规划躲避这些障碍。首先，建立一个空的网格地图，以无人机当前位置为参考，根据点云信息，有点云出现的位置设为Occupied(以概率表示)，没有出现的网格为Free(障碍出现概率为0)；点云密度大于某个值的地方就认定为有固体障碍，从而实现代价地图的建立。

步骤S204：代价地图分为全局代价地图和局部代价地图。由于本算法的应用场景是局部区域内未知地图的避障飞行，因此只能使用局部代价地图，这个是根据不断收到的传感器数据动态更新的地图。实际的避障中，首先需要初始化一个全局的规划器。这个全局规划器负责生成一个从指定的“起点”到指定的“目标点”之间的最短路径，通常使用Dijkstra最短路径算法，这个生成的最短路径为最初状态的最佳路径。然后根据局部代价地图，局部规划器将根据最新的障碍信息不断调整更新最佳路径，并产生各个移动移动方向上的速度命令以便无人机避开障碍物。这些速度命令一般采用动态窗口方法来生成，步骤如下：

(1)在机器人控制空间(dx,dy,dθ)中离散采样。

(2)对于每个采样速度，从机器人的当前状态执行正向仿真，以预测如果使用采样速度行进一段时间的运动会发生什么。

(3)使用包括诸如以下特征的度量来评估(打分)，如图4所示，从而正向模拟产生的每个轨迹：接近障碍物，接近目标，接近全局路径和速度。然后丢弃非法轨迹(与障碍物碰撞的轨迹以及得分较低的轨迹)。

(4)选择最高得分的轨迹，并将相关速度发送到移动基底。

(5)清除得分并重复。

步骤S205:建立视觉惯性里程计(VIO)。考虑到野外环境复杂，地形多变，无人机在搜救过程中可能会进入山崖下方等GPS信号微弱的区域，此时不能再通过GPS信号实现绝对定位，需采用相对定位的方法来确定无人机的位置，里程计是一种广泛使用的相对定位方法。常用的里程计有惯性里程计(IO)和视觉里程计(VO)，惯性里程计(IO)利用加速度计和陀螺仪进行定位，由于漂移误差和积分导致的累积误差，惯性里程计在运行一段时间后便会出现较大的定位偏差，需要进行校正。视觉里程计(VO)则是通过相机感知周围环境，采集图像，然后根据特征匹配和运动约束等方法来估计相机的姿态，比较准确，不会有漂移和累积误差。视觉里程计的实现主要包括以下4个步骤：

(1)特征检测。对于双目相机拍摄的图像，先进行特征点检测，常用的特征有角点，方向梯度直方图(HOG)，局部二值模式(LBP)等，本算法采用了尺度不变特征(SIFT)，能够保持尺度变换和旋转的不变性。

(2)立体匹配。对于双目相机两个摄像头拍摄的图像，根据双目测距原理，得到图像上对应部分的深度信息，从而得到各点的三维坐标。

(3)时域特征点跟踪。对同一摄像头在不同时刻拍摄的图像之间特征点进行匹配，并通过三角测量得到特征点在相机坐标系下的三维坐标。

(4)运动估计。在完成特征点时域跟踪的基础上，便可以通过无人机在相邻采样间隔时间内在同一坐标系下的位姿变化，对它进行运动估计，本算法采用了随机抽样一致性方法(RANSAC)进行运动估计。

尽管视觉里程计较为准确，但会由于场景特征的缺乏而导致跟踪的失败，为此，本算法将视觉信息与惯性测量单元(IMU)的数据进行融合，得到视觉惯性里程计(VIO)，从而实现在GPS信号缺失情况下的对无人机的准确定位。本算法采用的融合方法为MSCKF，步骤如下：

(1)初始化。初始化相机和IMU的参数，并构造MSCKF状态向量。

(2)读取IMU数据，估计新的MSCKF状态向量和对应的协方差矩阵。

(3)开始对图像数据进行处理，在MSCKF状态向量中增加当前帧的摄像机位姿，若位姿数大于滑动窗口大小的范围，去除状态变量中最早的视图对应的摄像机位姿。

(4)提取图像特征并匹配，去除外点。

(5)处理所有提取的特征。判断当前特征是否是之前视图中已经观察到的特征，如果是，且当前帧还可以观测到该特征，则加入该特征的track列表，否则将该特征的track加入到剩余列表，用于更新MSCKF的状态变量,如果不是，给该特征分配新的特征ID，并加入到当前视图可观测特征的集合。

(6)循环遍历剩余列表中的track，用于更新MSCKF的状态变量。

(7)计算新的MSCKF状态向量的协方差矩阵。

(8)管理状态变量，绘制运动轨迹。

通过以上方法便能融合视觉信息和IMU的数据，以实现在无GPS信号场景下对无人机的定位，从而保证导航任务的完成。

步骤S206:搜救系统在野外工作时，与地面保持一定的距离是十分必要的。因为野外地形无法预测，如果固定设置无人机飞行高度，遇到某些起伏地形(如斜坡)时，无人机则无法跟随地形相应地提升飞行高度，也就无法越过该地形继续搜索。此外，野外搜救时，无人机很有可能有降低飞行高度的需求，如自动递送急救物资，或者靠近目标做进一步确认。此类情况下，需要躲避无人机下方的障碍。此时超声波是一个很好的避障工具。利用超声波传感器，机载设备能以20Hz的频率接收到下方的障碍信息。超声波的有效测距范围为0.02—20m，在飞行过程中，采用下面的避障策略来躲避下方的障碍，如图5所示。系统初始化时，需要设定正常飞行的高度，以及安全距离。在飞行过程中可以根据任务的变化来设定正常飞行高度(但安全距离是固定的与障碍的最小可容忍距离)。由此策略，无人机系统将与下方障碍保持一定的距离，从而保证安全飞行。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (7)

1.一种面向无人机野外搜救的自主避障与导航方法，其特征在于，包括：

步骤1:无人机在GPS信号的导航下，按照预设的路径进行搜索，具体包括：

步骤101：在无人机配套软件上根据搜救环境，设置全局的搜索路径；

步骤102：无人机按照预设的路径，在GPS信号的导航下进行高空搜索；

步骤2:无人机发现目标后，通过基于双目视觉和超声波的算法实现自主避障和导航，从而接近目标，具体包括：

步骤201：当无人机在步骤1中发现目标后，开始下降并接近目标；

步骤202:无人机通过机载的双目相机拍摄周围环境，并得到视差图，根据双目测距原理，从视差图得到周围物体的深度信息；

步骤203：将视差图重投影到三维空间，从而得到点云数据；

步骤204：根据点云数据表示的障碍信息建立代价地图；

步骤205：根据代价地图上不断更新的障碍信息，调整最佳路径，并产生各个移动方向上的速度命令以便无人机避开障碍物并接近目标；

步骤206:采用超声波传感器探测飞机下方的地面，从而使无人机与地面始终保持一定的安全距离。

2.根据权利要求1所述的一种面向无人机野外搜救的自主避障与导航方法，其特征在于，所述步骤201的具体实现方法包括：

当无人机发现待救援目标后，使用双目相机可以根据左右两个相机的视差计算出物体的距离信息；使用两个相机可以得到左右视图，两个视图必定产生差异，称之为“左右视差”，定义前方物体上的点p12对应于左右视图上的点p1和点p2；则p1,p2,p12构成一个三角形，通过求解这个三角形的各边长，顶点位置，就可以算出点p12的坐标，再进一步也就可以得到p12到p1p2的距离，也就是p12到p1p2的深度；

由于采用的双目相机具有较高的分辨率，能够清晰地得到周围物体的影像，包括树枝、电线等超声波不能探测的细长障碍，也能够以这种方法进行检测并得到它们的深度信息。

3.据权利要求1所述的一种面向无人机野外搜救的自主避障与导航方法，其特征在于，所述步骤202的具体实现方法包括：使用OpenCV开源计算机视觉库，其中有基于块匹配算法的由双目视图计算差异图的实现；差异图重投影到三维空间就可以得到点云(PointCloud)，；而点云数据恰好是对障碍信息一个最通用的描述；采用的机器人操作系统ROS实现点云的获取，最后根据双目视觉模块生成的可视化三维点云图，使用ROS开源工具RVIZ可以看到，障碍信息已经被可视化，得到的三维点云可以让无人机了解前方一定距离内的障碍信息，为避障提供了条件。

4.据权利要求1所述的一种面向无人机野外搜救的自主避障与导航方法，其特征在于，所述步骤203的具体实现方法包括：云数据得到后，就可以得到障碍信息，需要解决的问题是如何躲避这些障碍；本算法使用基于代价地图的路径重规划躲避这些障碍；首先，建立一个空的网格地图，以无人机当前位置为参考，根据点云信息，有点云出现的位置设为Occupied，没有出现的网格为Free；点云密度大于某个值的地方就认定为有固体障碍，从而实现代价地图的建立。

5.据权利要求1所述的一种面向无人机野外搜救的自主避障与导航方法，其特征在于，所述步骤204的具体实现方法包括：

代价地图分为全局代价地图和局部代价地图；由于本算法的应用场景是局部区域内未知地图的避障飞行，因此只能使用局部代价地图，这个是根据不断收到的传感器数据动态更新的地图；实际的避障中，首先需要初始化一个全局的规划器；这个全局规划器负责生成一个从指定的“起点”到指定的“目标点”之间的最短路径，通常使用Dijkstra最短路径算法，这个生成的最短路径为最初状态的最佳路径；然后根据局部代价地图，局部规划器将根据最新的障碍信息不断调整更新最佳路径，并产生各个移动移动方向上的速度命令以便无人机避开障碍物；这些速度命令一般采用动态窗口方法来生成，步骤如下：

步骤204A、在机器人控制空间(dx,dy,dθ)中离散采样；

步骤204B、对于每个采样速度，从机器人的当前状态执行正向仿真，以预测如果使用采样速度行进一段时间的运动会发生什么；

步骤204C、使用包括诸如以下特征的度量来评估打分，从而正向模拟产生的每个轨迹：接近障碍物，接近目标，接近全局路径和速度；然后丢弃非法轨迹；

步骤204D、选择最高得分的轨迹，并将相关速度发送到移动基底；

步骤204E、清除得分并重复。

6.据权利要求1所述的一种面向无人机野外搜救的自主避障与导航方法，其特征在于，所述步骤205的具体实现方法包括：

建立视觉惯性里程计；考虑到野外环境复杂，地形多变，无人机在搜救过程中可能会进入山崖下方GPS信号微弱的区域，此时不能再通过GPS信号实现绝对定位，需采用相对定位的方法来确定无人机的位置，里程计是一种广泛使用的相对定位方法；常用的里程计有惯性里程计(IO)和视觉里程计(VO)，惯性里程计(IO)利用加速度计和陀螺仪进行定位，由于漂移误差和积分导致的累积误差，惯性里程计在运行一段时间后便会出现较大的定位偏差，需要进行校正；视觉里程计(VO)则是通过相机感知周围环境，采集图像，然后根据特征匹配和运动约束等方法来估计相机的姿态，比较准确，不会有漂移和累积误差；视觉里程计的实现主要包括以下4个步骤：

步骤205A1、特征检测；对于双目相机拍摄的图像，先进行特征点检测，常用的特征有角点，方向梯度直方图(HOG)，局部二值模式(LBP)等，本算法采用了尺度不变特征(SIFT)，能够保持尺度变换和旋转的不变性；

步骤205 A2、立体匹配；对于双目相机两个摄像头拍摄的图像，根据双目测距原理，得到图像上对应部分的深度信息，从而得到各点的三维坐标；

步骤205 A3、时域特征点跟踪；对同一摄像头在不同时刻拍摄的图像之间特征点进行匹配，并通过三角测量得到特征点在相机坐标系下的三维坐标；

步骤205 A4、运动估计；在完成特征点时域跟踪的基础上，便可以通过无人机在相邻采样间隔时间内在同一坐标系下的位姿变化，对它进行运动估计，采用了随机抽样一致性方法(RANSAC)进行运动估计；

尽管视觉里程计较为准确，但会由于场景特征的缺乏而导致跟踪的失败，为此，本算法将视觉信息与惯性测量单元(IMU)的数据进行融合，得到视觉惯性里程计(VIO)，从而实现在GPS信号缺失情况下的对无人机的准确定位；本算法采用的融合方法为MSCKF，步骤如下：

步骤205B1、初始化；初始化相机和IMU的参数，并构造MSCKF状态向量；

步骤205B2、读取IMU数据，估计新的MSCKF状态向量和对应的协方差矩阵；

步骤205B3、开始对图像数据进行处理，在MSCKF状态向量中增加当前帧的摄像机位姿，若位姿数大于滑动窗口大小的范围，去除状态变量中最早的视图对应的摄像机位姿；

步骤205B4、提取图像特征并匹配，去除外点；

步骤205B5、处理所有提取的特征；判断当前特征是否是之前视图中已经观察到的特征，如果是，且当前帧还可以观测到该特征，则加入该特征的track列表，否则将该特征的track加入到剩余列表，用于更新MSCKF的状态变量,如果不是，

给该特征分配新的特征ID，并加入到当前视图可观测特征的集合；

步骤205C1、循环遍历剩余列表中的track，用于更新MSCKF的状态变量；

步骤205C2、计算新的MSCKF状态向量的协方差矩阵；

步骤205C3、管理状态变量，绘制运动轨迹；

通过以上方法便能融合视觉信息和IMU的数据，以实现在无GPS信号场景下对无人机的定位，从而保证导航任务的完成。

7.根据权利要求1所述的一种面向无人机野外搜救的自主避障与导航方法，其特征在于，所述步骤206的具体实现方法包括：

搜救系统在野外工作时，与地面保持一定的距离是十分必要的；因为野外地形无法预测，如果固定设置无人机飞行高度，遇到某些起伏地形时，无人机则无法跟随地形相应地提升飞行高度，也就无法越过该地形继续搜索；此外，野外搜救时，无人机很有可能有降低飞行高度的需求，如自动递送急救物资，或者靠近目标做进一步确认；此类情况下，需要躲避无人机下方的障碍；此时超声波是一个很好的避障工具；利用超声波传感器，机载设备能以20Hz的频率接收到下方的障碍信息；超声波的有效测距范围为0.02—20m，在飞行过程中，采用下面的避障策略来躲避下方的障碍：

系统初始化时，需要设定正常飞行的高度，以及安全距离；在飞行过程中可以根据任务的变化来设定正常飞行高度；由此策略，无人机系统将与下方障碍保持一定的距离，从而保证安全飞行。