前言

 上篇中介绍了基于稀疏点云传感器的常用避障规划算法，由于实际工程中受限于不同产品的技术成熟度、成本的因素的原因，不得不采用低成本的单点测距传感器进行避障。然而，避障作为无人机自动飞行必不可少的功能，设计一个健壮的避障规划器显得尤为重要。

 本篇将针对单点测距传感器（如超声波、红外、部分毫米波雷达等）的避障规划算法进行详解。

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

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

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

• 感知

  由于我们的无人机机头前部安装了单点测距传感器，整个飞行过程中，飞行方向始终保持与机头方向一致，以保证测距传感器能够正常检测飞行方向上的障碍物，支撑无人机的避障规划。

  这里，我们令检测到的障碍物距离为 $d$，在得到障碍物距离后，我们首先需要将其坐标从机体系 $B$转换至大地坐标系（导航系） $E$，由于单点测距传感器与机头方向一致，这里可认为无人机相对障碍物的偏角等价于无人机的航向角 $\varphi$，由此可得，障碍物在大地坐标系 $E$下的坐标为：
$\begin{cases} x_{obs}^{E} = \frac{d tan(\varphi)}{\sqrt{1+tan(\varphi^{2})}}\\[2ex] y_{obs}^{E} = \frac{d}{\sqrt{1+tan(\varphi^{2})}}\\[2ex] \end{cases}$
  其次，在得到障碍物在大地坐标系 $E$下的坐标后，我们需要对其进行半径为 $R$的膨胀处理，从而使得运动规划问题的求解变得更为简单。障碍物膨胀半径 $R$包含了我们对于无人机尺寸以及刹车缓冲区的预估，理论上，无人机规划轨迹只要不经过膨胀后的障碍物区域，那么我们认为，该轨迹就是Collision-free的。

• 轨迹生成与重规划

  在得到业务层给定的目标航点（Waypoint）后，我们采用上文提到的JLT算法进行轨迹生成/优化。

  首先，我们假设无人机从当前飞行状态进行刹车，从而，通过JLT算法，我们能够得到第一条轨迹，记为 $Traj_{Predection}$，该轨迹表示的是若当前飞行器突然发现前方突然出现障碍物，则飞行器进行紧急刹车时所生成的预测轨迹。

  在得到 $Traj_{Predection}$后，我们需要结合该段轨迹信息与障碍物信息，对无人机进行碰撞检测：

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轨迹生成，从而得到第二条轨迹，记为 $Traj_{Planned}$，该轨迹表示的是对于当前的业务目标，无人机下一时刻的运动轨迹。

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

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

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

总结

  本篇针对单点测距传感器的无人机避障规划算法进行讨论，在不进行建图的前提下，设计了一种基于JerkLimitedTrajectory的避障规划算法，并针对无人机自动返航的应用案例进行了实验仿真。仿真实验结果表明，该算法能够较早地预判无人机的刹车轨迹，并针对高楼等障碍物进行探索性的Collision-free轨迹重规划。在下一篇中，我们将对其他稀疏点云传感器的避障规划算法进行详解。

作者简介： 一个被Coding耽误的无人机算法工程师，控制、导航略懂一二，热衷技术，喜欢乒乓、音乐、电影，欢迎交流。

知乎： @遥远的乌托邦

GitHub: https://github.com/DistantUtopia

微信公众号： @遥远的乌托邦