自动驾驶---打造自动驾驶系统之控制模块开发（九）-CSDN博客

本文链接：https://blog.csdn.net/Janeiskangs/article/details/147710592

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

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

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

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

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

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

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

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

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

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

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

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

1 系统环境

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

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

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

2 控制模块

对于控制模块，不论上游的planning是基于Frenet坐标系取求解，或者是基于笛卡尔坐标系下去求解，最终两者输出的结果都会统一到笛卡尔坐标系下，然后依据建立的模型（运动学模型或者动力学模型）求解。

控制层面一般分为两个子模块：

横向控制
纵向控制

2.1 开发

在planning发出轨迹信息后，control模块需要对轨迹进行准确跟踪。planning轨迹输出可以参考博客《自动驾驶---打造自动驾驶系统之轨迹生成模块开发（八）（上）》和《自动驾驶---打造自动驾驶系统之轨迹生成模块开发（八）（下）》。

在车辆横向控制中，常用的算法包括LQR和MPC，关于LQR笔者之前已经介绍过《自动驾驶---Control之LQR控制》，博客中详细讲解了建模求解的过程（公式推导等），MPC控制器的话还未来得及，后续尽量补上。通常情况下，横向控制可直接使用反馈控制，也可使用前馈+反馈的形式。

在车辆的纵向控制中，通常使用PID算法，一般做一层速度PID即可，但有些公司可能会增加一层位置PID。

主要代码如下所示：

// solve
Eigen::MatrixXd LatController::solveRiccati(const Eigen::Matrix4d& A,
                            const Eigen::MatrixXd& B,
                            const Eigen::MatrixXd& Q,
                            const Eigen::MatrixXd& R,
                            int max_iter,
                            double eps) {
    Eigen::MatrixXd P = Q;
    for(int i = 0; i < max_iter; ++i){
        Eigen::MatrixXd P_next = Q + A.transpose() * P * A -
            A.transpose() * P * B * (R + B.transpose() * P * B).inverse() * B.transpose() * P * A;

        if((P_next - P).cwiseAbs().maxCoeff() < eps) {
            break;
        }
        P = P_next;
    }

    return P;
}

最终呈现的效果如下（视频链接：城区自动驾驶）：