环境感知与规划专题（八）——基于稀疏点云的避障规划器（二）

前言

  上篇中介绍了基于稀疏点云传感器的常用避障规划算法，由于实际工程中受限于不同产品的技术成熟度、成本的因素的原因，不得不采用低成本的单点测距传感器进行避障。然而，避障作为无人机自动飞行必不可少的功能，设计一个健壮的避障规划器显得尤为重要。   本篇将针对单点测距传感器（如「超声波、红外、部分毫米波雷达等」）的避障规划算法进行详解。  

基于单点测距传感器的避障规划算法

  由于单点测距传感器的测量范围有限，且其信息量较小，我们设计了一种特殊的基于「JLT」（该算法已在前几篇中详解，本篇不再赘述）轨迹优化算法的避障规划算法，其核心算法流程如下图所示：  

  核心算法主要分为以下两部分：  

• 「感知」

  由于我们的无人机机头前部安装了单点测距传感器，整个飞行过程中，飞行方向始终保持与机头方向一致，以保证测距传感器能够正常检测飞行方向上的障碍物，支撑无人机的避障规划。   这里，我们令检测到的障碍物距离为
，在得到障碍物距离后，我们首先需要将其坐标从「机体系」
转换至大地坐标系（「导航系」）
，由于单点测距传感器与机头方向一致，这里可认为无人机相对障碍物的偏角等价于无人机的航向角
，由此可得，障碍物在大地坐标系
下的坐标为：  
  其次，在得到障碍物在大地坐标系
下的坐标后，我们需要对其进行半径为
的膨胀处理，从而使得运动规划问题的求解变得更为简单。障碍物膨胀半径
包含了我们对于无人机尺寸以及刹车缓冲区的预估，理论上，无人机规划轨迹只要不经过膨胀后的障碍物区域，那么我们认为，该轨迹就是「Collision-free」的。  

• 「轨迹生成与重规划」

  在得到业务层给定的目标航点（「Waypoint」）后，我们采用上文提到的「JLT」算法进行「轨迹生成/优化」。   首先，我们假设无人机从当前飞行状态进行刹车，从而，通过「JLT」算法，我们能够得到第一条轨迹，记为
，该轨迹表示的是「若当前飞行器突然发现前方突然出现障碍物，则飞行器进行紧急刹车时所生成的预测轨迹」。   在得到
后，我们需要结合该段轨迹信息与障碍物信息，对无人机进行碰撞检测：  

function [ ret ] = collisionCheck( x, y, z, x_obs, y_obs, z_obs, r)
    if((x - x_obs) * (x - x_obs) + (y - y_obs) * (y - y_obs) + (z - z_obs) * (z - z_obs) <= r * r)
        ret = 1;
    else
        ret = 0;
    end
end

其次，我们需要根据当前无人机状态，进行常规的「JLT」轨迹生成，从而得到第二条轨迹，记为
，该轨迹表示的是对于当前的业务目标，无人机下一时刻的运动轨迹。

当碰撞检测成功（即「无人机的刹车预测轨迹进入膨胀后的障碍物区域」），我们将对无人机的
方向进行「刹车」；同时，对于
方向而言，我们输入一个「微小」的上升速度指令，使其在水平刹车的同时，在垂直方向缓慢爬升，以避开障碍物。直到无人机在前方无法检测到障碍物，无人机将根据当前业务目标所生成的轨迹执行任务。

如下图所示，为本文所设计的避障规划算法的仿真飞行实验，该实验模拟了无人机在自动返航中途遇到障碍物的情况。

 

其中，绿色轨迹为
，红色轨迹为
，蓝色为环境中膨胀后的障碍物区域。从实验结果来看，无人机在执行返航任务的过程中，在其检测到障碍物后，能够及时地进行减速（「刹车」），同时，缓慢上升以寻找「Collision-free」的返航路径。