自动驾驶---打造自动驾驶系统之预测模块开发（六）

         各位读者朋友，大家好。本次打造的自动驾驶仿真系统，涉及感知，预测，规控等多个模块（以规控算法为主，包括Polynomial预测，MCTS决策算法，通行走廊Corridor构建，QP/CILQR轨迹生成求解器，LQR+PID的控制器等），同时也支持其它相关规控算法的扩展（部署&开发自身感兴趣的算法），非常便捷。笔者在该系列中开发的规控算法主要依据专栏《自动驾驶Planning决策规划》中的章节逐步搭建，后续实践系列涉及的博客包括但不局限于以下内容：

        《自动驾驶---打造自动驾驶系统之环境准备（一）》---已更新

        《自动驾驶---打造自动驾驶系统之定位模块开发（二）》---已更新

        《自动驾驶---打造自动驾驶系统之导航模块开发（三）》---已更新

        《自动驾驶---打造自动驾驶系统之参考线平滑（四）》---已更新

        《自动驾驶---打造自动驾驶系统之感知环境开发（五）》---已更新

        《自动驾驶---打造自动驾驶系统之预测模块开发（六）》

        《自动驾驶---打造自动驾驶系统之决策模块开发（七）》

        《自动驾驶---打造自动驾驶系统之轨迹生成模块开发（八） 》       

        《自动驾驶---打造自动驾驶系统之控制模块开发（九）》

---

        最终呈现的静态效果（无法直接贴视频）如下：

1 系统环境

        笔者的环境是：Ubuntu 18.04 + ROS Melodic，当然18.04以上的环境也支持。

        其它依赖库的安装参考之前的博客《自动驾驶---打造自动驾驶系统之环境准备（一）》，后续如果有遗漏，继续补充。

        当然，如果各位读者朋友感兴趣或者在此过程中遇到相关问题，欢迎私信咨询！

2 Prediction模块

        预测模块在自动驾驶中处于非常重要的一环，因为其决定了决策模块的可靠性。在非模型化的预测模块中，如果有车道，会以车道为牵引进行多项式的拟合，如果没有车道信息，只能通过其历史轨迹进行拟合。

        当预测模块进入模型化之后，一切都变得“简单”了，其中包括单模预测和多模预测。

2.1 开发

        关于预测模块，笔者之前也写过博客，如下几篇：

        《自动驾驶---基于深度学习模型的轨迹预测》

        《自动驾驶---LSTM模型用于轨迹预测》

        《自动驾驶---Perception之多模预测》

        《自动驾驶---不依赖地图的大模型轨迹预测》

        总体来说，前几年预测模块还是以多项式拟合为主（包括Apollo模块中），但随着模型技术的发展和应用，近两年的预测模块基本模型化，并且逐渐预测，决策，规划一体化。本篇博客主要介绍多项式拟合的预测方法，并用于仿真。

        主要代码如下：

// predict traj of dynamic obstacles by curvefitting
// x,y: history pose
// degree: polynomial degree
Eigen::VectorXd Prediction::curveFitting(const Eigen::VectorXd& x, const Eigen::VectorXd& y, const int degree){
    size_t n = x.size();
    Eigen::MatrixXd A(n, degree + 1);

    for (int i = 0; i < n; ++i){
        for (int j = 0; j <= degree; ++j){
            A(i, j) = pow(x(i), j);
        }
    }

    Eigen::VectorXd result = A.householderQr().solve(y);
    return result;
}

2.2 效果

       结合上一节的障碍物模拟，最终预测的效果如下（动态障碍物有预测轨迹，静态障碍物是没有预测轨迹的），由于设置的速度偏低， 因此预测的轨迹也偏短一些。