技术分享 | P450 ego-planner的干货分享

Posted 2021-05-16 阿木实验室

原理说明

传统的基于梯度的运动规划算法需要构建所需的ESDF地图，然而构建地图花费了整个规划算法70%的时间，从而限制了在有限资源情况下的运动规划方法的使用。

ESDF的构建方式有全局增量式和批量本地计算两种方式，但他们并不是专门用于运动规划而构建的，也就是说对于运动规划来说现有的两种方法构建出的ESDF地图是多余的、不必要的。从图1中可以看出轨迹仅仅覆盖小范围的ESDF地图，大部分都是没用的。简单地手动减小ESDF地图范围，缺乏理论依据，也包含不必要的计算。

EGO-Planer主要由基于梯度的样条曲线优化器和细化过程组成。

基于梯度的样条曲线优化器：使用平滑性、碰撞性和动力学可行性项优化轨迹。碰撞项的构成通过比较障碍物内的轨迹与无碰撞的引导路径，然后用梯度信息将碰撞到障碍物的轨迹拉出障碍物，从而算法只需要计算碰撞处的障碍物梯度即可。

细化过程：当某段轨迹动力学不可行时，激活细化过程，即增大该轨迹分配的时间。新生成的B样条曲线平衡了动力学可行性与拟合之前动力学不可行轨迹的准确性。在轴向和径向上拟合的准确性惩罚并不一样，以提高模型的鲁棒性。

结合Prometheus对无人机的底层控制接口实现EGO-planner在PX4飞控上的实现。

 

节点图

TF树

 

---

 

实机操作

每次飞行前需重新上电以重启飞控和板载计算机

传感器检查

首先在终端输入下面命令，查看是否T265和D435i都已经被系统识别到

rs-sensor-control

操作

在终端中输入下面命令启动

'/home/amov/EGO_ws/ego_px4.sh'

依次查看每个终端是否有报错（注意注意：每一个终端都需要检查的哦~）

正常启动的话，会自动弹出rviz界面，并可以看到建图效果，里面可以看到障碍物

确认参数和设置没有问题后，在如下终端输入1并敲回车

然后遥控器先切到定点模式，在此终端继续输入1并敲回车，解锁无人机

在此终端继续输入2并敲回车，代表给无人机发送起飞指令，但是此时无人机还没有切入offboard模式，所以不会立马执行起飞，此时用遥控器切入offboard模式后，无人机会立马起飞

等到无人机起飞到一个稳定高度（默认是0.8m），也即是我们设定的起飞高度后，回到rviz界面，点击选择2D Nav Goal

在rviz上点击一个点作为目标点，会自动生成一个绕开障碍物的路径

此时无人机会沿着这个生成的路径飞，绕开障碍物，且高度会保持不变。

等待到达目标设定点之后，可以继续给下一个目标点。

降落

无人机飞到目标点后，可以遥控器拨动offboard飞行模式开关切出offboard模式，并缓慢手动降落。

 

更多详细可以参考Wiki，链接如下：https://prometheus-wiki.readthedocs.io/zh_CN/latest/docs/p450/5-%E8%BF%9B%E9%98%B6%E5%8A%9F%E8%83%BD-fastplanner.html