环境感知与规划专题（八）——基于稀疏点云的避障规划器（二）
前言
上篇中介绍了基于稀疏点云传感器的常用避障规划算法，由于实际工程中受限于不同产品的技术成熟度、成本的因素的原因，不得不采用低成本的单点测距传感器进行避障。然而，避障作为无人机自动飞行必不可少的功能，设计一个健壮的避障规划器显得尤为重要。
本篇将针对单点测距传感器（如超声波、红外、部分毫米波雷达等）的避障规划算法进行详解。
基于单点测距传感器的避障规划算法
由于单点测距传感器的测量范围有限，且其信息量较小，我们设计了一种特殊的基于JLT（该算法已在前几篇中详解，本篇不再赘述）轨迹优化算法的避障规划算法，其核心算法流程如下图所示：
核心算法主要分为以下两部分：
- 感知
由于我们的无人机机头前部安装了单点测距传感器，整个飞行过程中，飞行方向始终保持与机头方向一致，以保证测距传感器能够正常检测飞行方向上的障碍物，支撑无人机的避障规划。
这里，我们令检测到的障碍物距离为 d d d，在得到障碍物距离后，我们首先需要将其坐标从机体系 B B B转换至大地坐标系（导航系） E E E，由于单点测距传感器与机头方向一致，这里可认为无人机相对障碍物的偏角等价于无人机的航向角 φ \varphi φ，由此可得，障碍物在大地坐标系 E E E下的坐标为：
{ x o b s E = d t a n ( φ ) 1 + t a n ( φ 2 ) y o b s E = d 1 + t a n ( φ 2 ) \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} ⎩⎪⎨⎪⎧xobsE=1+tan(φ2)dtan(φ)yobsE=1+tan(φ2)d
其次，在得到障碍物在大地坐标系 E E E下的坐标后，我们需要对其进行半径为 R R R的膨胀处理，从而使得运动规划问题的求解变得更为简单。障碍物膨胀半径 R R R包含了我们对于无人机尺寸以及刹车缓冲区的预估，理论上，无人机规划轨迹只要不经过膨胀后的障碍物区域，那么我们认为，该轨迹就是Collision-free的。
- 轨迹生成与重规划
在得到业务层给定的目标航点（Waypoint）后，我们采用上文提到的JLT算法进行轨迹生成/优化。
首先，我们假设无人机从当前飞行状态进行刹车，从而，通过JLT算法，我们能够得到第一条轨迹，记为 T r a j P r e d e c t i o n Traj_{Predection} TrajPredection，该轨迹表示的是若当前飞行器突然发现前方突然出现障碍物，则飞行器进行紧急刹车时所生成的预测轨迹。
在得到 T r a j P r e d e c t i o n Traj_{Predection} TrajPredection后，我们需要结合该段轨迹信息与障碍物信息，对无人机进行碰撞检测：
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轨迹生成，从而得到第二条轨迹，记为 T r a j P l a n n e d Traj_{Planned} TrajPlanned，该轨迹表示的是对于当前的业务目标，无人机下一时刻的运动轨迹。
当碰撞检测成功（即无人机的刹车预测轨迹进入膨胀后的障碍物区域），我们将对无人机的 x 、 y x、y x、y方向进行刹车；同时，对于 z z z方向而言，我们输入一个微小的上升速度指令，使其在水平刹车的同时，在垂直方向缓慢爬升，以避开障碍物。直到无人机在前方无法检测到障碍物，无人机将根据当前业务目标所生成的轨迹执行任务。
如下图所示，为本文所设计的避障规划算法的仿真飞行实验，该实验模拟了无人机在自动返航中途遇到障碍物的情况。
其中，绿色轨迹为 T r a j P r e d e c t i o n Traj_{Predection} TrajPredection，红色轨迹为 T r a j P l a n n e d Traj_{Planned} TrajPlanned，蓝色为环境中膨胀后的障碍物区域。从实验结果来看，无人机在执行返航任务的过程中，在其检测到障碍物后，能够及时地进行减速（刹车），同时，缓慢上升以寻找Collision-free的返航路径。
总结
本篇针对单点测距传感器的无人机避障规划算法进行讨论，在不进行建图的前提下，设计了一种基于JerkLimitedTrajectory的避障规划算法，并针对无人机自动返航的应用案例进行了实验仿真。仿真实验结果表明，该算法能够较早地预判无人机的刹车轨迹，并针对高楼等障碍物进行探索性的Collision-free轨迹重规划。在下一篇中，我们将对其他稀疏点云传感器的避障规划算法进行详解。
作者简介： 一个被Coding耽误的无人机算法工程师，控制、导航略懂一二，热衷技术，喜欢乒乓、音乐、电影，欢迎交流。
知乎： @遥远的乌托邦
GitHub: https://github.com/DistantUtopia
微信公众号： @遥远的乌托邦
