EGO-Planner学习笔记

前言

对于一般无人机设计，可以将无人机的飞行控制过程分为感知层，运动规划层以及控制层，框图如下

感知层对无人机的状态信息进行解析获取，结合状态信息以及任务需求对无人机i的期望运动状态进行规划，并生成对应的轨迹，最后通过控制层输出可由无人机直接执行的控制指令，由此实现闭环控制。

感知层

感知层一般由传感器作为硬件构成，如加速度计、磁力计、陀螺仪等外部传感器以及摄像头、激光、GPS等外部传感器，通过对各类传感器的数据进行获取并融合可以得到无人机的状态信息，如位置、速度、加速度以及姿态角信息。

硬件部分

在使用EGO_Planner算法时，需要使用外部视觉传感器对室内环境进行感知，使用IMU模块进行融合定位。

算法部分

该算法使用VINS-Fusion对各类定位数据进行融合，包括由视觉里程计计算得到位姿信息以及IMU积分得到的位姿信息。经融合后可以得到无人机准确的位姿信息及速度等状态量。同时根据感知到的环境信息实时构建地图，以完成对障碍物的探测等任务。

运动规划层

运动规划层将结合环境信息、无人机的状态信息以及飞行任务综合生成一条合理的路径，并交由控制层执行。

路径的表示方法

路径一般为曲线，表示方法由很多种，如样条曲线以及多项式曲线。考虑到样条曲线的控制点只对临近的曲线造成影响，该算法使用样条曲线作为最后的路径输出。

路径搜索

通过判断任务需求得到飞行任务的目标点，通过路径搜多算法生成一条从当前点到目标点的一条合理路径，该路径应避开当前可探测的障碍物。该算法使用A*算法对可行路径进行初步搜索。

轨迹优化

轨迹是有时间维度的路径，及可以在某个时间可以到达某个位置。
由于A*算法依据栅格地图的看空白处获得，所得到的路径为折线，考虑飞行的连贯性以及代价，路径应为光滑连续的曲线。故需要对该路径进行优化。
首先由初始路径生成一条由若干控制点组成的光滑的样条曲线，在优化过程中通过改变控制点在空间中的位置实现对路径的修改。
该算法采用基于梯度下降的优化算法，通过对当前对极构造代价函数，该代价函数应综合考虑总消耗，最终得到消耗最小的轨迹。其中总消耗包括飞行过程中的速度变化、加速变化以及角速度变化、力矩变化等，这里的状态量由得到期望路径后使用微分平坦算法得到，并作为期望值参与代价函数的计算。
优化过程中首先判断该路径是否会发生碰撞，若发生碰撞，应对路径进行搜索，得到一条无碰撞的路径。
在此基础上进行微分平坦得到控制量，并结合控制量以及其他惩罚项得到代价函数值，通过梯度下降对轨迹进行修改，最后得到最优的轨迹，并生成控制指令供控制层执行。

控制层

该算法使用pid控制，得到期望姿态角以及期望推力后传递至飞行控制板执行。
至此完成闭环。