精华内容
下载资源
问答
  • Apollo代码学习(一)—控制模块概述

    万次阅读 多人点赞 2018-09-19 20:18:31
    Apollo学习-控制模块概述控制纵向控制横向控制仿真仿真平台及工具 概述 控制 纵向控制 横向控制 仿真 仿真平台及工具 仿真平台Windows 仿真工具CarSim + Simulink 并未在Apollo团队预定义的Docker环境...

    Apollo(阿波罗)是一个开放的、完整的、安全的自动驾驶平台,以灵活和高性能的技术架构,为全自动驾驶提供支持。


    补充

    2018.11.08更新

    仿真平台的搭建请参考我的另一篇博文:
    Apollo代码学习(四)—Windows下编译Apollo并与Carsim和Simulink联调
    需要了解车辆运动学模型和动力学模型,可以参考我的另两篇博文:
    【1】 Apollo代码学习(二)—车辆运动学模型
    【2】 Apollo代码学习(三)—车辆动力学模型
    横纵向控制详解可看:
    Apollo代码学习(五)—横纵向控制

    2018.11.15更新

    标定表的生成进行了补充。

    2018.11.20更新

    MPC解析可看:
    Apollo代码学习(六)—模型预测控制(MPC)

    前言

    本人出于工作需要,对Apollo的Control模块及其相关的部分代码进行了解析。Apollo的控制逻辑主要源于此书:Vehicle Dynamics and Control,因此,研究Apollo控制代码前请先准备一下这本书,结合此书去理解Control模块的相关代码会事半功倍。此外,还需要对Frenet坐标系有一定的了解,可参考这篇文章:Optimal trajectory generation for dynamic street scenarios in a Frenét Frame

    提倡大家支持正版资源,本人提供文档仅限交流学习使用,侵删:
    【1】Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. | CSDN资源
    【2】Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame | CSDN资源

    控制

    研究控制前,先了解一下Apollo项目的整体结构,如下图所示,它包含了感知、定位、决策、控制、通信等几大模块。
    Apollo整体框架
    其中,Apollo Control模块提供了三种控制方法:纵向控制、横向控制和MPC控制。本文主要介绍纵向控制和横向控制,横纵向控制详解请见我的另一篇博文:Apollo代码学习(五)—横纵向控制

    纵向控制

    纵向控制主要通过控制汽车的刹车/油门来实现对汽车速度的控制。它主要由一个级联控制器和一个标定表构成。
    级联控制器位置PID闭环控制器速度PID闭环控制器构成;
    标定表是指速度-加速度-刹车/油门命令标定表
    计算纵向控制命令的接口:

    Status LonController::ComputeControlCommand(
        const localization::LocalizationEstimate *localization,
        const canbus::Chassis *chassis,
        const planning::ADCTrajectory *planning_published_trajectory,
        control::ControlCommand *cmd)
    

    输入为:定位信息(localization)、自车底盘信息(chassis)、规划信息(planning_published_trajectory)
    输出为:油门/刹车命令(cmd)
    Apollo纵向控制的工作原理框图如下所示:
    纵向控制
    位置PID闭环控制器

    模块变量
    输入期望位置+当前实际位置
    输出速度补偿量

    速度PID闭环控制器

    模块变量
    输入速度补偿+当前位置速度偏差
    输出加速度补偿量

    速度-加速度-刹车/油门命令标定表

    模块变量
    输入加速度补偿+规划加速度,车速
    输出油门/刹车控制量

    纵向控制中理解起来较为复杂的是纵向误差的计算:

    void LonController::ComputeLongitudinalErrors(
    	const TrajectoryAnalyzer *trajectory_analyzer, const double preview_time,
    	SimpleLongitudinalDebug *debug) 
    

    涉及到Frenet坐标系1

    Trajectory generation in a Frene ́t-frame

    标定表的生成

    标定表的更新请参考文档:

    Apollo/docs/howto/how_to_update_vehicle_calibration.md

    标定流程大致如下图所示:
    标定表更新流程
    其中,命令集可自定义。因为本人是在Windows下利用carsim和matlab进行仿真的,所以采集数据的过程和文档有出入,但处理和转化过程相同。
    由于我搭建的是基于泊车场景的模型,对车速要求较低,因此怠速下的车速完全够用,再适当加一点刹车即可,因此标定的时候没有标定油门值,标定流程大致如下:
    1.首先,搭建CarSim和Smulink模型,CarSim中让汽车在怠速下走直线,同时给出一定的刹车值,使车辆模型伴随一定的刹车值(0~1)进行怠速行驶,判断哪些刹车值状态下,车辆可以进行加速运动;然后让车加速到怠速状态下最大速度,给刹车值,测试多大刹车值下可使车辆在最高速情况下停止;
    对于我来说,我测定并需要标定的值如下(m文件):

    % 倒车刹车标定。车辆靠怠速行驶,通过控制刹车压力,控制车辆加/减速
    % 加速阶段刹车压力范围:0:0.05:0.6    acc 
    % 减速阶段刹车压力范围:0.6:0.05:1.6  dec 
    % 一个加速度值对应多个减速度值,共312组数据
    if(brakeflag==0)
        k=1;
        acc=0:0.05:0.6;
        dec=0.6:0.05:1.6;
        
        rows=size(acc,2);
        nols=size(dec,2);    
        des_brake=cell(1,rows*nols);
        for i=1:1:rows
            for j=1:1:nols
                des_brake{k}=[acc(i),dec(j)];
                k=k+1;
            end
        end   
    end
    

    2.确定标定序列后(采样间隔自定,因为后期是后标定表时候需要插值计算,间隔越小相对来说插值越准确),按照一定间隔(我设置的采样周期为20s,加速12s,减速8s)在Simulink中采集CarSim传输过来的数据(主要为时间、速度、加速度)并按照Apollo采集数据的格式存入.csv文件中;
    我的采样时间定义如下:

    % 0s,将trajectory、count_num、saveFileflag、speedflag重置
    % 0~2s,静止状态,刹车压力为2
    % 3~12s,加速阶段,刹车压力:0~0.6
    % 12~20s,减速阶段,刹车压力:0.6~1.6,当18s时存文件
    % >20s,开始下一个文件采样
    

    是按照Apollo的采样样式定义的,以标定工具中的文件t20b13.txt为例

    a: gear_location = chassis_pb2.Chassis.GEAR_DRIVE //档位状态
    a: brake = 40.0 //刹车百分比
    a: steering_target = 0.0 //方向盘角度
    a: throttle = 0.0 //油门百分比
    c: vehicle_speed == 0.0 //车速
    t: 2.0 //启动时间,静止2s后启动
    a: brake = 0.0 //刹车百分比
    a: throttle = 20.0 //油门百分比,开始加速
    c: vehicle_speed >= 10.0 //判断车速,若车速达到10km/h,停止加速
    a: throttle = 0.0 //油门百分比
    t: 1.0 //匀速行驶1s
    a: brake = 13.0 //刹车百分比,然后开始减速
    c: vehicle_speed == 0.0 //减速至车速为0,采样结束
    

    3.然后调用python文件进行数据处理与转换,需要针对自己的数据对部分python文件进行更改。

    横向控制

    横向控制主要通过调节方向盘转角来实现对航向的控制。它主要由一个前馈控制器反馈控制器组成。横向控制器的核心是车辆动力学模型LQR模型
    计算纵向控制命令的接口:

    Status LatController::ComputeControlCommand(
        const localization::LocalizationEstimate *localization,
        const canbus::Chassis *chassis,
        const planning::ADCTrajectory *planning_published_trajectory,
        control::ControlCommand *cmd)
    

    输入为:定位信息(localization)、自车底盘信息(chassis)、规划信息(planning_published_trajectory)
    输出为:方向盘控制量(cmd)
    Apollo横向控制的工作原理框图如下所示:
    横向控制
    前馈控制器

    模块变量
    输入道路曲率
    输出方向盘前馈控制量

    前馈控制量为了补偿道路曲率对稳态误差的影响,实现零稳态误差。主要依据参考书2中第3.2节的公式3.12。
    δ f f = L R + K V a y − k 3 [ ℓ r R − ℓ f 2 C α r m V x 2 R ℓ ] \delta_{ff}=\frac{L}{R}+K_Va_y-k_3[\frac{\ell_r}{R}-\frac{\ell_f}{2C_{\alpha r}}\frac{m{V_x}^2}{R\ell}] δff=RL+KVayk3[Rr2CαrfRmVx2]

    反馈控制器

    模块变量
    输入期望航向角
    输出方向盘反馈控制量

    闭环控制器的主要公式为,详见参考书2中第2章关于横向控制的描述,重点关注公式2.31、2.45、2.46:
    d d t [ e 1 e 1 ˙ e 2 e 2 ˙ ] = [ 0 1 0 0 0 − 2 C a f + 2 C a r m V x 2 C a f + 2 C a r m − 2 C a f ℓ f + 2 C a r ℓ r m V x 0 0 0 1 0 − − 2 C a f ℓ f − 2 C a r ℓ r I z V x 2 C a f ℓ f − 2 C a r ℓ r I z − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] [ e 1 e 1 ˙ e 2 e 2 ˙ ] + [ 0 2 C a f m 0 2 C a f ℓ f I z ] δ + [ 0 − 2 C a f ℓ f − 2 C a r ℓ r m V x − V x 0 − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] ψ ˙ d e s \frac{d}{dt} \begin{bmatrix} e_1 \\ \dot{e1} \\ e_2 \\ \dot{e2} \\ \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{af}+2C_{ar}}{mV_x} & \frac{2C_{af}+2C_{ar}}{m} & \frac{-2C_{af}\ell_f+2C_{ar}\ell_r}{mV_x} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{-2C_{af}\ell_f-2C_{ar}\ell_r}{I_zV_x} & \frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_z} & -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \\ \end{bmatrix} \begin{bmatrix} e_1 \\ \dot{e1} \\ e_2 \\ \dot{e2} \\ \end{bmatrix} \\ +\begin{bmatrix} 0 \\ \frac{2C_{af}}{m} \\ 0 \\ \frac{2C_{af}\ell_f}{I_z} \\ \end{bmatrix}\delta + \begin{bmatrix} 0 \\ -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{mV_x}-V_x \\ 0 \\ -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \\ \end{bmatrix}\dot{\psi}_{des} dtde1e1˙e2e2˙=00001mVx2Caf+2Car0IzVx2Caff2Carr0m2Caf+2Car0Iz2Caff2Carr0mVx2Caff+2Carr1IzVx2Caff2+2Carr2e1e1˙e2e2˙+0m2Caf0Iz2Caffδ+0mVx2Caff2CarrVx0IzVx2Caff2+2Carr2ψ˙des
    闭环控制主要结合车辆动力学模型得出状态矩阵,然后利用LQR模型获取K矩阵,进而计算最优控制解steer_angle_feedback,它的实现过程如下:
    a、依据阿克曼转角几何和车辆动力学原理,搭建车辆动力学模型,列写状态空间方程;
    b、根据已知车辆信息,计算横向偏差,更新状态矩阵、状态矩阵系数、控制矩阵系数等;
    更新状态矩阵,代码对应接口:

    void LatController::UpdateStateAnalyticalMatching(SimpleLateralDebug *debug)
    

    输入:debug,包含横向误差、航向误差等信息
    输出:更新状态矩阵

    计算横向误差,代码对应接口:

    double LatController::ComputeLateralErrors(
    	const double x, 
    	const double y, 
    	const double theta, 
    	const double linear_v, 
    	const double angular_v, 
    	const TrajectoryAnalyzer &trajectory_analyzer, 
    	SimpleLateralDebug *debug)
    

    输入:当前车辆位置坐标(x, y)、当前航向角theta、当前线速度linear_v、当前角速度angular_v、规划轨迹trajectory_analyzer
    输出:debug信息,主要为生成状态矩阵,状态矩阵包含:lateral_error、lateral_error_rate、heading_error、heading_error_rate四个元素

    更新状态矩阵系数matrix_a_和离散状态矩阵系数matrix_ad_,代码对应接口:

    void LatController::UpdateMatrix()
    

    更新混合状态矩阵系数matrix_adc_和混合控制矩阵系数matrix_bdc_,代码对应接口:

    void LatController::UpdateMatrixCompound()
    

    c、计算K矩阵;
    代码对应接口:

    void common::math::SolveLQRProblem(
    	const Matrix &A, 
    	const Matrix &B, 
    	const Matrix &Q, 
    	const Matrix &R, 
    	const double tolerance, 
    	const uint max_num_iteration, 
    	Matrix *ptr_K)
    

    输入:状态矩阵系数A、控制矩阵系数B、控制权重矩阵R、状态权重矩阵Q、迭代次数max_num_iteration、计算阈值tolerance
    输出:增益矩阵ptr_K
    d、计算最优控制解。

    const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 / 
                                          M_PI * steer_transmission_ratio_ /
                                          steer_single_direction_max_degree_ * 100;
    

    反馈量:steer_angle = steer_angle_feedback + steer_angle_feedforward,即为方向盘控制量。

    控制信号

    Apollo中Control模块计算完cmd后按周期发送给canbus模块,再由canbus模块发送给CANBUS,并定期从canbus模块接收相关信息。
    Apollo can信号采用DBC格式,
    DBC数据格式
    了解消息收发请查看如下相关代码:

    Apollo\modules\canbus\canbus.cc
    Apollo\modules\canbus\vehicle\lincoln\protocol
    Apollo\modules\canbus\vehicle\lincoln\lincoln_controller.cc
    Apollo\modules\drivers\canbus\can_comm\message_manager.h
    Apollo\modules\drivers\canbus\can_comm\can_receiver.h
    Apollo\modules\drivers\canbus\can_comm\protocol_data.h

    消息分为接收和发送,不同消息对应不同ID,对于同一类型消息,接收和发送使用不同的消息ID,具体如下:
    接收模块

    信号发送周期发送接收
    brake_6110mscan_client_Apollo_module
    throttle_6310mscan_client_Apollo_module
    steering_6510mscan_client_Apollo_module
    gear_6710mscan_client_Apollo_module
    misc_6910mscan_client_Apollo_module
    wheelspeed_6a10mscan_client_Apollo_module
    accel_6b10mscan_client_Apollo_module
    gyro_6c10mscan_client_Apollo_module
    gps_6d10mscan_client_Apollo_module
    gps_6e10mscan_client_Apollo_module
    gps_6f10mscan_client_Apollo_module
    tirepressure_7110mscan_client_Apollo_module
    fullevel_7210mscan_client_Apollo_module
    surround_7310mscan_client_Apollo_module
    brakeinfo_7410mscan_client_Apollo_module
    throttleinfo_7510mscan_client_Apollo_module
    version_7f10mscan_client_Apollo_module
    license_7e10mscan_client_Apollo_module

    发送模块

    信号发送周期发送接收
    brake_6020mscan_client_CANBUS
    throttle_6220mscan_client_CANBUS
    steering_6420mscan_client_CANBUS
    gear_6620mscan_client_CANBUS
    turnsignal_6850mscan_client_CANBUS

    仿真

    仿真平台及工具

    仿真平台Windows
    仿真工具CarSim + Simulink
    本人并未在Apollo团队预定义的Docker环境中搭建Apollo,而是在同事的协助下抽调了Apollo里的纵向控制代码及部分编译依赖库,在Windows下利用CarSim搭建车模型,Matlab Simulink提供规划路径和控制逻辑实现了自动泊车的仿真。
    CarSim仿真界面
    Simulink仿真界面


    1. Werling M, Ziegler J, Kammel S, et al. Optimal trajectory generation for dynamic street scenarios in a Frenét Frame[C]// IEEE International Conference on Robotics and Automation. IEEE, 2010:987-993. ↩︎

    2. Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. ↩︎ ↩︎

    展开全文
  • apollo代码学习1

    千次阅读 2018-01-23 17:33:27
    接触到百度无人驾驶开源代码... 因网上对apollo代码解析的博客很少,我个人对apollo理解又多少有些偏颇,因此将自己学习过程中的心得体会写出来,希望大家一起学习交流,并能够对其中的错误进行指正,我也会虚心讨教。

     接触到百度无人驾驶开源代码apollo是导师推荐学习的,在摸索了一个多月后,终于有了一些眉目,对于一个本科并非学习计算机的我来说,学习过程中补充了很多基础知识,包括编程语言C++,以及软件设计方法的种种思想。

     因网上对apollo代码解析的博客很少,我个人对apollo理解又多少有些偏颇,因此将自己学习过程中的心得体会写出来,希望大家一起学习交流,并能够对其中的错误进行指正,我也会虚心讨教。

     今天,我将围绕apollo代码进行简单的归纳。(主要从整个工程角度分析)

     可以说,对于我一个小白来说,接触到apollo直接懵了,它所使用的工具真是不少,主要包括google的很多开发工具以及ros,具体列举如下,倘若读者对其中某些还未了解,我也会推荐相关学习网站或者相关博客进行学习。

     1. ros(机器人操作系统,现在科研和实用都很常用的工具,官网:http://wiki.ros.org/ROS/Tutorials)

     2. bazel (google的编译工具,相当于cmake一样的工具,建议从官网学习:https://docs.bazel.build/versions/master/install.html)

     3. protobuf (google的一种结构化数据存储格式,推荐博客:https://www.ibm.com/developerworks/cn/linux/l-cn-gpb/index.html)

     看了代码你会发现上面三个工具用到的最多,几乎搭建了apollo软件框架,而除此之外还用到很多很实用的小工具比如:glog, gflags,signal以及各个模块单独用到的库工具等等。最后再罗嗦一句:其实我个人觉得最重要的还是 c++基础,这些小工具现学现用就OK了!

     好了,今天罗嗦了很多,献上一张apollo框架结构图给大家。今天也没啥干货,下一次开始解析模块代码!


    展开全文
  • Apollo代码学习(六)—模型预测控制(MPC)

    万次阅读 多人点赞 2018-11-20 15:39:48
    Apollo代码学习—模型预测控制前言模型预测控制预测模型滚动优化反馈矫正 前言 查看Apollo中关于MPC_controller的代码可以发现,它的主体集成了横纵向控制,在计算控制命令时,计算了横纵向误差: ...

    前言

    非专业选手,此篇博文内容基于书本和网络资源整理,可能理解的较为狭隘,起点较低,就事论事。如发现有纰漏,请指正,非常感谢!!!

    查看Apollo中关于MPC_controller的代码可以发现,它的主体集成了横纵向控制,在计算控制命令时,计算了横纵向误差:

    ComputeLongitudinalErrors(&trajectory_analyzer_, debug);
    
    ComputeLateralErrors(com.x(), com.y(),
                           VehicleStateProvider::instance()->heading(),
                           VehicleStateProvider::instance()->linear_velocity(),
                           VehicleStateProvider::instance()->angular_velocity(),
                           trajectory_analyzer_, debug);
    

    下文将从MPC原理入手,结合横纵向控制进行分析。

    模型预测控制

    可以先结合官方的教学视频对MPC进行一个简单的了解:模型预测控制

    根据维基百科,模型预测控制是一种先进的过程控制方法,在满足一定约束条件的前提下,被用来实现过程控制,它的实现依赖于过程的动态模型(通常为线性模型)。在控制时域(一段有限时间)内,它主要针对当前时刻进行优化,但也考虑未来时刻,求取当前时刻的最优控制解,然后反复优化,从而实现整个时域的优化求解。

    也就是说,模型预测控制实际上是一种时间相关的,利用系统当前状态和当前的控制量,来实现对系统未来状态的控制。而系统未来的状态是不定的,因此在控制过程中要不断根据系统状态对未来的控制量作出调整。而且相较于经典的的PID控制,它具有优化和预测的能力,也就是说,模型预测控制是一种致力于将更长时间跨度、甚至于无穷时间的最优化控制问题,分解为若干个更短时间跨度,或者有限时间跨度的最优化控制问题,并且在一定程度上仍然追求最优解1

    可以通过下图对模型预测控制进行一个简单的理解:

    图1 模型预测控制原理
    (图片来源:无人驾驶车辆模型预测控制)

    图1中,k轴为当前状态,左侧为过去状态,右侧为将来状态。可结合无人驾驶车辆模型预测控制2第3.1.2节来理解此图。

    模型预测控制在实现上有三个要素:

    1. 预测模型,是模型预测控制的基础,用来预测系统未来的输出;
    2. 滚动优化,一种在线优化,用于优化短时间内的控制输入,以尽可能减小预测模型输出与参考值的差距;
    3. 反馈矫正,在新的采样时刻,基于被控对象的实际输出,对预测模型的输出进行矫正,然后进行新的优化,以防模型失配或外界干扰导致的控制输出与期望差距过大。

    下面从三要素入手对模型预测控制进行分析。

    预测模型

    无论是运动学模型,还是动力学模型,所搭建的均为非线性系统,而线性模型预测控制较非线性模型预测控制有更好的实时性,且更易于分析和计算。对于无人车来说,实时性显然很重要,因此,需要将非线性系统转化为线性系统,而非线性系统的线性化的方法大体可分为精确线性化和近似线性化,多采用近似的线性化方法。

    线性化

    此部分内容主要参考《无人驾驶车辆模型预测控制》23.3.2节,本人对对线性化、离散化等问题也理解的不深,不理解的可以自行查找一些其他的文章。

    非线性系统线性化的方法有很多种,大致分为:

    图2 线性化方法

    下面以泰勒展开的方法为例,结合《无人驾驶车辆模型预测控制》23.3.2节非线性系统线性化方法,展示一种存在参考系统的线性化方法。存在参考系统的线性化方法假设参考系统已经在规划轨迹上完全跑通,得出每个时刻上相应的状态量 X r X_r Xr和控制量 U r U_r Ur,然后通过处理参考系统与当前系统的偏差,利用模型预测控制器来跟踪期望路径。

    假设车辆参考系统在任意时刻的状态和控制量满足如下方程:
    (1) X ˙ r = f ( X r , U r ) \dot{X}_r=f(X_r, U_r) \tag{1} X˙r=f(Xr,Ur)(1)
    在任意点 ( X r , U r ) (X_r, U_r) (Xr,Ur)处泰勒展开,只保留一阶项,忽略高阶项,有:
    (2) X ˙ = f ( X r , U r ) + ∂ f ∂ X ( X − X r ) + ∂ f ∂ U ( U − U r ) \dot{X}=f(X_r, U_r)+\frac{\partial f}{\partial X}(X - X_r)+\frac{\partial f}{\partial U}(U - U_r) \tag{2} X˙=f(Xr,Ur)+Xf(XXr)+Uf(UUr)(2)
    公式2与公式1相减可得:
    (3) X ~ ˙ = A ( t ) X ~ + B ( t ) U ~ \dot{\widetilde{X}}=A(t)\widetilde{X}+B(t)\widetilde{U} \tag{3} X ˙=A(t)X +B(t)U (3)
    其中, X ~ ˙ = X ˙ − X ˙ r \dot{\widetilde{X}}=\dot{X}-\dot{X}_r X ˙=X˙X˙r X ~ = X − X r \widetilde{X}=X-X_r X =XXr U ~ = U − U r \widetilde{U}=U-U_r U =UUr A ( t ) A(t) A(t) f ( X , U ) f(X, U) f(X,U) X X X的雅克比矩阵, B ( t ) B(t) B(t) f ( X , U ) f(X, U) f(X,U) U U U的雅克比矩阵。
    此时,通过雅克比矩阵,将非线性系统近似转化为一个连续的线性系统,但并不适于模型预测控制器的设计,然后对其离散化处理可得,
    (4) X ~ ˙ ( k ) = X ~ ( k + 1 ) − X ~ ( k ) T = A ( k ) X ~ ( k ) + B ( k ) U ~ ( k ) \dot{\widetilde{X}}(k)=\frac{\widetilde{X}(k+1)-\widetilde{X}(k)}{T}=A(k)\widetilde{X}(k)+B(k)\widetilde{U}(k) \tag{4} X ˙(k)=TX (k+1)X (k)=A(k)X (k)+B(k)U (k)(4)
    得到线性化时变模型:
    (5) X ~ ( k + 1 ) = A ~ ( k ) X ~ ( k ) + B ~ ( k ) U ~ ( k ) \widetilde{X}(k+1)=\widetilde{A}(k)\widetilde{X}(k)+\widetilde{B}(k)\widetilde{U}(k) \tag{5} X (k+1)=A (k)X (k)+B (k)U (k)(5)
    其中: A ~ ( t ) = I + T A ( t ) , B ~ ( t ) = T B ( t ) , I \widetilde{A}(t)=I+TA(t) ,\widetilde{B}(t)=TB(t),I A (t)=I+TA(t)B (t)=TB(t)I为单位矩阵。

    至此,得到一个非线性系统在任意一参考点线性化后的线性系统,该系统是设计模型预测控制算法的基础。

    运动学模型中的式1.15为例,低速情况下的车辆运动学模型可表达为:
    (6) [ x ˙ y ˙ ψ ˙ ] = [ cos ⁡ ψ sin ⁡ ψ tan ⁡ δ f / l ] v \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot\psi \end{bmatrix}= \begin{bmatrix} \cos\psi \\ \sin\psi \\ \tan{\delta_f}/l \end{bmatrix}v \tag{6} x˙y˙ψ˙=cosψsinψtanδf/lv(6)
    其状态变量和控制变量分别为:
    X = [ x y ψ ] , U = [ v δ f ] X= \begin{bmatrix} x \\ y \\ \psi \end{bmatrix}, U= \begin{bmatrix} v\\ \delta_f \end{bmatrix} X=xyψU=[vδf]
    对应的雅克比矩阵:
    (7) A = [ 0 0 − v sin ⁡ ψ 0 0 v cos ⁡ ψ 0 0 0 ] , B = [ cos ⁡ ψ 0 sin ⁡ ψ 0 tan ⁡ δ f l v l cos ⁡ 2 δ f ] A= \begin{bmatrix} 0 & 0 & -v\sin{\psi} \\ 0 & 0 & v\cos{\psi} \\ 0 & 0 & 0 \end{bmatrix}, B= \begin{bmatrix} \cos{\psi} & 0\\ \sin{\psi} & 0\\ \frac{\tan{\delta_f}}{l} & \frac{v}{l\cos^2{\delta_f}} \end{bmatrix} \tag{7} A=000000vsinψvcosψ0B=cosψsinψltanδf00lcos2δfv(7)
    其线性时变模型为:
    X ~ ˙ = A ~ X ~ + B ~ U ~ \dot{\widetilde{X}}=\widetilde{A}\widetilde{X}+\widetilde{B}\widetilde{U} X ˙=A X +B U
    其中,
    (8) X ~ = [ x − x 0 y − y 0 ψ − ψ 0 ] , U ~ = [ v − v 0 δ f − δ f 0 ] , A ~ = I + T A = [ 1 0 − v sin ⁡ ψ T 0 1 v cos ⁡ ψ T 0 0 1 ] , B ~ = T B = [ cos ⁡ ψ T 0 sin ⁡ ψ T 0 tan ⁡ δ f T l v T l cos ⁡ 2 δ f ] \widetilde{X}= \begin{bmatrix} x-x_0 \\ y-y_0 \\ \psi-\psi_0 \end{bmatrix}, \widetilde{U}= \begin{bmatrix} v-v_0 \\ \delta_f-\delta_{f0} \end{bmatrix},\\ \widetilde{A}=I+TA= \begin{bmatrix} 1 & 0 & -v\sin{\psi}T \\ 0 & 1 & v\cos{\psi}T \\ 0 & 0 & 1 \end{bmatrix}, \widetilde{B}=TB= \begin{bmatrix} \cos{\psi}T & 0\\ \sin{\psi}T & 0\\ \frac{\tan{\delta_f}T}{l} & \frac{vT}{l\cos^2{\delta_f}} \end{bmatrix} \tag{8} X =xx0yy0ψψ0U =[vv0δfδf0]A =I+TA=100010vsinψTvcosψT1B =TB=cosψTsinψTltanδfT00lcos2δfvT(8)

    单车模型

    预测模型仍以单车模型为主,结合运动学模型和动力学模型对车辆运动状态进行分析。

    图3 单车模型

    根据化代码可知,Apollo的MPC模块中的状态变量共有6个:

      matrix_state_ = Matrix::Zero(basic_state_size_, 1);
      
      // State matrix update;
      matrix_state_(0, 0) = debug->lateral_error();
      matrix_state_(1, 0) = debug->lateral_error_rate();
      matrix_state_(2, 0) = debug->heading_error();
      matrix_state_(3, 0) = debug->heading_error_rate();
      matrix_state_(4, 0) = debug->station_error();
      matrix_state_(5, 0) = debug->speed_error();
    

    m a t r i x _ s t a t e _ = [ l a t e r a l _ e r r o r l a t e r a l _ e r r o r _ r a t e h e a d i n g _ e r r o r h e a d i n g _ e r r o r _ r a t e s t a t i o n _ e r r o r s p e e d _ e r r o r ] matrix\_state\_= \begin{bmatrix} lateral\_error\\ lateral\_error\_rate\\ heading\_error\\ heading\_error\_rate\\ station\_error\\ speed\_error\\ \end{bmatrix} matrix_state_=lateral_errorlateral_error_rateheading_errorheading_error_ratestation_errorspeed_error
    它们的计算请参考上一篇Apollo代码学习(五)—横纵向控制
    { m a t r i x _ s t a t e _ ( 0 , 0 ) = d y ∗ cos ⁡ φ − d x ∗ sin ⁡ φ m a t r i x _ s t a t e _ ( 1 , 0 ) = V x ∗ sin ⁡ Δ φ = V x ∗ sin ⁡ e 2 m a t r i x _ s t a t e _ ( 2 , 0 ) = φ − φ d e s m a t r i x _ s t a t e _ ( 3 , 0 ) = φ ˙ − φ ˙ d e s m a t r i x _ s t a t e _ ( 4 , 0 ) = − ( d x ∗ cos ⁡ φ d e s + d y ∗ sin ⁡ φ d e s ) m a t r i x _ s t a t e _ ( 5 , 0 ) = V d e s − V ∗ cos ⁡ Δ φ / k \begin{cases} matrix\_state\_(0, 0)=dy*\cos{\varphi}-dx*\sin{\varphi}\\ matrix\_state\_(1, 0)=V_x*\sin{\Delta\varphi} =V_x*\sin{e_2}\\ matrix\_state\_(2, 0)=\varphi-\varphi_{des}\\ matrix\_state\_(3, 0)=\dot{\varphi}-\dot{\varphi}_{des}\\ matrix\_state\_(4, 0)=-(dx*\cos{\varphi_{des}} + dy*\sin{\varphi_{des}})\\ matrix\_state\_(5, 0)=V_{des} - V*\cos{\Delta\varphi}/k\\ \end{cases} matrix_state_(0,0)=dycosφdxsinφmatrix_state_(1,0)=VxsinΔφ=Vxsine2matrix_state_(2,0)=φφdesmatrix_state_(3,0)=φ˙φ˙desmatrix_state_(4,0)=(dxcosφdes+dysinφdes)matrix_state_(5,0)=VdesVcosΔφ/k
    控制变量有2个:

      Eigen::MatrixXd control_matrix(controls_, 1);
      control_matrix << 0, 0;
    

    并结合代码去分析 c o n t r o l _ m a t r i x control\_matrix control_matrix内包含的变量:

    // 解mpc,输出一个vector,control内有10个control_matrix
    SolveLinearMPC(matrix_ad_, matrix_bd_, matrix_cd_, matrix_q_updated_,
              matrix_r_updated_, lower_bound, upper_bound, matrix_state_, reference,
              mpc_eps_, mpc_max_iteration_, &control)
    // 已知,mpc仅取第一组解为当前时刻最优控制解,即control[0]
    // control中第一个矩阵中的第一个值用于计算方向盘转角
    double steer_angle_feedback = control[0](0, 0) * 180 / M_PI *
                                    steer_transmission_ratio_ /
                                    steer_single_direction_max_degree_ * 100;
    double steer_angle = steer_angle_feedback + steer_angle_feedforwardterm_updated_;
    // control中第一个矩阵中的第二个值用于计算加速度
    debug->set_acceleration_cmd_closeloop(control[0](1, 0));
    double acceleration_cmd = control[0](1, 0) + debug->acceleration_reference();
    

    可得
    c o n t r o l _ m a t r i x = [ δ f a ] control\_matrix= \begin{bmatrix} \delta_f \\ a \end{bmatrix} control_matrix=[δfa]
    其中, δ f \delta_f δf为前轮转角, a a a为加速度补偿。
    结合方向盘控制的动力学模型:
    (9) d d t [ e 1 e ˙ 1 e 2 e ˙ 2 ] = [ 0 1 0 0 0 − 2 C a f + 2 C a r m V x 2 C a f + 2 C a r m − 2 C a f ℓ f + 2 C a r ℓ r m V x 0 0 0 1 0 − 2 C a f ℓ f − 2 C a r ℓ r I z V x 2 C a f ℓ f − 2 C a r ℓ r I z − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] [ e 1 e ˙ 1 e 2 e ˙ 2 ] + [ 0 2 C α f m 0 2 C α f ℓ f I z ] δ f + [ 0 − 2 C a f ℓ f − 2 C a r ℓ r m V x − V x 0 − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] φ ˙ \frac{d}{dt} \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \end{bmatrix}= \begin{bmatrix} 0 &amp; 1 &amp; 0 &amp; 0 \\ 0 &amp; -\frac{2C_{af}+2C_{ar}}{mV_x} &amp; \frac{2C_{af}+2C_{ar}}{m} &amp; \frac{-2C_{af}\ell_f+2C_{ar}\ell_r}{mV_x}\\ 0 &amp; 0 &amp; 0 &amp; 1 \\ 0 &amp; -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_zV_x} &amp; \frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_z} &amp; -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \end{bmatrix} \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \end{bmatrix} \\ +\begin{bmatrix} 0 \\ \frac{2C_{\alpha f}}{m} \\ 0 \\ \frac{2C_{\alpha f}\ell_f}{I_z} \end{bmatrix}\delta_f+ \begin{bmatrix} 0 \\ -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{mV_x} -V_x\\ 0 \\ -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \end{bmatrix}\dot{\varphi} \tag{9} dtde1e˙1e2e˙2=00001mVx2Caf+2Car0IzVx2Caff2Carr0m2Caf+2Car0Iz2Caff2Carr0mVx2Caff+2Carr1IzVx2Caff2+2Carr2e1e˙1e2e˙2+0m2Cαf0Iz2Cαffδf+0mVx2Caff2CarrVx0IzVx2Caff2+2Carr2φ˙(9)
    mpc_controller.ccmpc_slover.cc中的代码注释,

    // 初始化矩阵
    Status MPCController::Init(const ControlConf *control_conf) {
      ...
      // Matrix init operations.
      matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
      matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
      ...
      // TODO(QiL): expand the model to accomendate more combined states.
    
      matrix_a_coeff_ = Matrix::Zero(basic_state_size_, basic_state_size_);
      ...
    
      matrix_b_ = Matrix::Zero(basic_state_size_, controls_);
      matrix_bd_ = Matrix::Zero(basic_state_size_, controls_);
      ...
      matrix_bd_ = matrix_b_ * ts_;
    
      matrix_c_ = Matrix::Zero(basic_state_size_, 1);
      ...
      matrix_cd_ = Matrix::Zero(basic_state_size_, 1);
      ...
      }
    
    // 更新矩阵
    void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {
      const double v = VehicleStateProvider::instance()->linear_velocity();
      matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
      matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
      matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;
      matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;
    
      Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());
      matrix_ad_ = (matrix_i + ts_ * 0.5 * matrix_a_) *
                   (matrix_i - ts_ * 0.5 * matrix_a_).inverse();
    
      matrix_c_(1, 0) = (lr_ * cr_ - lf_ * cf_) / mass_ / v - v;
      matrix_c_(3, 0) = -(lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_ / v;
      matrix_cd_ = matrix_c_ * debug->heading_error_rate() * ts_;
    }
    
    // 求解MPC
    // discrete linear predictive control solver, with control format
    // x(i + 1) = A * x(i) + B * u (i) + C
    bool SolveLinearMPC(const Matrix &matrix_a, const Matrix &matrix_b,
                        const Matrix &matrix_c, const Matrix &matrix_q,
                        const Matrix &matrix_r, const Matrix &matrix_lower,
                        const Matrix &matrix_upper,
                        const Matrix &matrix_initial_state,
                        const std::vector<Matrix> &reference, const double eps,
                        const int max_iter, std::vector<Matrix> *control) 
    
    

    可得MPC控制模型:
    (10) d d t [ l a t e r a l _ e r r o r l a t e r a l _ e r r o r _ r a t e h e a d i n g _ e r r o r h e a d i n g _ e r r o r _ r a t e s t a t i o n _ e r r o r s p e e d _ e r r o r ] = [ 0 1 0 0 0 0 0 − C f + C r m V C f + C r m C r ℓ r − C f ℓ f m V 0 0 0 0 0 1 0 0 0 C r ℓ r − C f ℓ f I z V C f ℓ f − C r ℓ r I z − C r ℓ r 2 + C f ℓ f 2 I z V 0 0 0 0 0 0 0 1 0 0 0 0 0 0 ] [ l a t e r a l _ e r r o r l a t e r a l _ e r r o r _ r a t e h e a d i n g _ e r r o r h e a d i n g _ e r r o r r a t e s t a t i o n _ e r r o r s p e e d _ e r r o r ] + [ 0 0 C f m 0 0 0 C f ℓ f I z 0 0 0 0 − 1 ] [ δ f a ] + [ 0 C r ℓ r − C f ℓ f m V − V 0 − C r ℓ r 2 + C f ℓ f 2 I z V 0 1 ] φ ˙ \frac{d}{dt} \begin{bmatrix} lateral\_error\\ lateral\_error\_rate\\ heading\_error\\ heading\_error\_rate\\ station\_error\\ speed\_error\\ \end{bmatrix}= \begin{bmatrix} 0 &amp; 1 &amp; 0 &amp; 0 &amp; 0 &amp; 0\\ 0 &amp; -\frac{C_f+C_r}{mV} &amp; \frac{C_f+C_r}{m} &amp; \frac{C_r\ell_r-C_f\ell_f}{mV} &amp; 0 &amp; 0\\ 0 &amp; 0 &amp; 0 &amp; 1 &amp; 0 &amp; 0\\ 0 &amp; \frac{C_r\ell_r-C_f\ell_f}{I_zV} &amp; \frac{C_f\ell_f-C_r\ell_r}{I_z} &amp; -\frac{C_r\ell_r^2+C_f\ell_f^2}{I_zV} &amp; 0 &amp; 0\\ 0 &amp; 0 &amp; 0 &amp; 0 &amp; 0 &amp; 1\\ 0 &amp; 0 &amp; 0 &amp; 0 &amp; 0 &amp; 0 \end{bmatrix} \begin{bmatrix} lateral\_error\\ lateral\_error\_rate\\ heading\_error\\ heading\_error_rate\\ station\_error\\ speed\_error\\ \end{bmatrix} \\ +\begin{bmatrix} 0 &amp; 0\\ \frac{C_f}{m} &amp; 0\\ 0 &amp; 0\\ \frac{C_f\ell_f}{I_z} &amp; 0\\ 0 &amp; 0\\ 0 &amp; -1 \end{bmatrix} \begin{bmatrix} \delta_f \\ a \end{bmatrix}+ \begin{bmatrix} 0\\ \frac{C_r\ell_r-C_f\ell_f}{mV}-V\\ 0\\ -\frac{C_r\ell_r^2+C_f\ell_f^2}{I_zV}\\ 0\\ 1 \end{bmatrix} \dot{\varphi} \tag{10} dtdlateral_errorlateral_error_rateheading_errorheading_error_ratestation_errorspeed_error=0000001mVCf+Cr0IzVCrrCff000mCf+Cr0IzCffCrr000mVCrrCff1IzVCrr2+Cff200000000000010lateral_errorlateral_error_rateheading_errorheading_errorratestation_errorspeed_error+0mCf0IzCff00000001[δfa]+0mVCrrCffV0IzVCrr2+Cff201φ˙(10)

    其中, C f , C r C_f,C_r CfCr分别为前/后轮转弯刚度, ℓ f , ℓ r \ell_f,\ell_r fr分别为前悬/后悬长度, m m m为车身质量, V V V为车速, I z I_z Iz为车辆绕 z z z轴转动的转动惯量, φ ˙ \dot{\varphi} φ˙为转向速度。
    对应的状态矩阵、控制矩阵、扰动矩阵等如下:
    (11) A = [ 0 1 0 0 0 0 0 − C f + C r m V C f + C r m C r ℓ r − C f ℓ f m V 0 0 0 0 0 1 0 0 0 C r ℓ r − C f ℓ f I z V C f ℓ f − C r ℓ r I z − C r ℓ r 2 + C f ℓ f 2 I z V 0 0 0 0 0 0 0 1 0 0 0 0 0 0 ] , B = [ 0 0 C f m 0 0 0 C f ℓ f I z 0 0 0 0 − 1 ] , C = [ 0 C r ℓ r − C f ℓ f m V − V 0 − C r ℓ r 2 + C f ℓ f 2 I z V 0 1 ] A=\begin{bmatrix} 0 &amp; 1 &amp; 0 &amp; 0 &amp; 0 &amp; 0\\ 0 &amp; -\frac{C_f+C_r}{mV} &amp; \frac{C_f+C_r}{m} &amp; \frac{C_r\ell_r-C_f\ell_f}{mV} &amp; 0 &amp; 0\\ 0 &amp; 0 &amp; 0 &amp; 1 &amp; 0 &amp; 0\\ 0 &amp; \frac{C_r\ell_r-C_f\ell_f}{I_zV} &amp; \frac{C_f\ell_f-C_r\ell_r}{I_z} &amp; -\frac{C_r\ell_r^2+C_f\ell_f^2}{I_zV} &amp; 0 &amp; 0\\ 0 &amp; 0 &amp; 0 &amp; 0 &amp; 0 &amp; 1\\ 0 &amp; 0 &amp; 0 &amp; 0 &amp; 0 &amp; 0 \end{bmatrix},\\ B=\begin{bmatrix} 0 &amp; 0\\ \frac{C_f}{m} &amp; 0\\ 0 &amp; 0\\ \frac{C_f\ell_f}{I_z} &amp; 0\\ 0 &amp; 0\\ 0 &amp; -1 \end{bmatrix}, C=\begin{bmatrix} 0\\ \frac{C_r\ell_r-C_f\ell_f}{mV}-V\\ 0\\ -\frac{C_r\ell_r^2+C_f\ell_f^2}{I_zV}\\ 0\\ 1 \end{bmatrix} \tag{11} A=0000001mVCf+Cr0IzVCrrCff000mCf+Cr0IzCffCrr000mVCrrCff1IzVCrr2+Cff200000000000010B=0mCf0IzCff00000001C=0mVCrrCffV0IzVCrr2+Cff201(11)
    对应的线性化系数应为(欧拉映射离散法):
    A ~ = I + T A , B ~ = T B , C ~ = T C \widetilde{A}=I+TA,\widetilde{B}=TB,\widetilde{C}=TC A =I+TAB =TBC =TC
    但代码中实际线性化系数为(双线性变换离散法):

      matrix_ad_ = (matrix_i + ts_ * 0.5 * matrix_a_) *
                   (matrix_i - ts_ * 0.5 * matrix_a_).inverse();
                   
      matrix_bd_ = matrix_b_ * ts_;
    
      matrix_cd_ = matrix_c_ * debug->heading_error_rate() * ts_;
    

    (12) A ~ = ( I + T A / 2 ) ( I − T A / 2 ) − 1 , B ~ = T B , C ~ = T C ∗ h e a d i n g _ e r r o r _ r a t e \widetilde{A}=(I+TA/2)(I-TA/2)^{-1},\widetilde{B}=TB,\widetilde{C}=TC*heading\_error\_rate \tag{12} A =(I+TA/2)(ITA/2)1B =TBC =TCheading_error_rate(12)
    对于 C ~ \widetilde{C} C 的形式不同,个人认为是由于 h e a d i n g _ e r r o r _ r a t e heading\_error\_rate heading_error_rate是计算横向误差之后才有的,无法直接获取,在矩阵更新的时候才将 h e a d i n g _ e r r o r _ r a t e heading\_error\_rate heading_error_rate作为系数更新到矩阵中,也就是公式10中的 φ ˙ \dot{\varphi} φ˙,其实对于 C ~ \widetilde{C} C 来说,仍是 C ~ = T C \widetilde{C}=TC C =TC的形式。 A ~ \widetilde{A} A 的形式不一样,是因为此处采用了双线性变换离散法。感谢博友LLCCCJJ的友情提示。 关于连续系统离散化的方法可参考:连续系统离散化方法

    最终得到形如公式13的离散线性模型:
    (13) x ( k + 1 ) = A x ( k ) + B u ( k ) + C x(k+1)=Ax(k)+Bu(k)+C\tag{13} x(k+1)=Ax(k)+Bu(k)+C(13)
    系统的输出方程为: y ( k ) = D x ( k ) y(k)=Dx(k) y(k)=Dx(k)
    则在预测时域内有状态变量方程和输出变量方程如下:
    (14) x ( k ) = A k x ( 0 ) + ∑ i = 0 k − 1 A i B u ( k − 1 − i ) + ∑ i = 0 k − 1 A i C x(k)=A^kx(0)+\sum_{i=0}^{k-1}A^iBu(k-1-i)+\sum_{i=0}^{k-1}A^iC \tag{14} x(k)=Akx(0)+i=0k1AiBu(k1i)+i=0k1AiC(14)
    (15) y ( k ) = D A k x ( 0 ) + ∑ i = 0 k − 1 D A i B u ( k − 1 − i ) + ∑ i = 0 k − 1 D A i C y(k)=DA^kx(0)+\sum_{i=0}^{k-1}DA^iBu(k-1-i)+\sum_{i=0}^{k-1}DA^iC \tag{15} y(k)=DAkx(0)+i=0k1DAiBu(k1i)+i=0k1DAiC(15)
    假设
    (16) ξ ( k ∣ t ) = [ x ( k ∣ t ) u ( k − 1 ∣ t ) ] \xi(k|t)= \begin{bmatrix} x(k|t)\\ u(k-1|t) \end{bmatrix} \tag{16} ξ(kt)=[x(kt)u(k1t)](16)
    可得到新的状态空间表达式:
    (17) ξ ( k + 1 ∣ t ) = A ~ ξ ( k ∣ t ) + B ~ Δ u ( k ∣ t ) + C ~ η ( k ∣ t ) = D ~ ξ ( k ∣ t ) \begin{matrix} \xi(k+1|t)=\widetilde{A}\xi(k|t)+\widetilde{B}\Delta u(k|t)+\widetilde{C} \\ \eta(k|t)=\widetilde{D}\xi(k|t) \end{matrix}\tag{17} ξ(k+1t)=A ξ(kt)+B Δu(kt)+C η(kt)=D ξ(kt)(17)
    其中,
    (18) A ~ = [ A B 0 I ] , B ~ = [ B I ] , C ~ = [ C 0 ] , D ~ = [ D     0 ] \begin{matrix} \widetilde{A}= \begin{bmatrix} A &amp; B \\ 0 &amp; I \end{bmatrix}, \widetilde{B}= \begin{bmatrix} B \\ I \end{bmatrix}, \widetilde{C}= \begin{bmatrix} C \\ 0 \end{bmatrix}, \widetilde{D}=[D\ \ \ 0] \end{matrix}\tag{18} A =[A0BI]B =[BI],C =[C0],D =[D   0](18)
    可将公式16、公式18代入公式17,验证公式18的由来。
    则在预测时域内的状态变量和输出变量可用如下算式计算:
    (19) ξ ( k + N p ∣ t ) = A ~ t N p ξ ( t ∣ t ) + A ~ t N p − 1 B ~ t Δ u ( t ∣ t ) + A ~ t N p − 1 C ~ t + ⋯ + B ~ t Δ u ( t + N p − 1 ∣ t ) + C ~ t η ( k + N p ∣ t ) = D ~ t A ~ t N p ξ ( t ∣ t ) + D ~ t A ~ t N p − 1 B ~ t Δ u ( t ∣ t ) + D ~ t A ~ t N p − 1 C ~ t + ⋯ + D ~ t B ~ t Δ u ( t + N p − 1 ∣ t ) + D ~ t C ~ t \begin{matrix} \xi(k+N_p|t)=\widetilde{A}_t^{N_p}\xi(t|t)+\widetilde{A}_t^{N_p-1}\widetilde{B}_t\Delta u(t|t)+\widetilde{A}_t^{N_p-1}\widetilde{C}_t+\cdots +\widetilde{B}_t\Delta u(t+N_p-1|t)+\widetilde{C}_t \\ \eta(k+N_p|t)=\widetilde{D}_t\widetilde{A}_t^{N_p}\xi(t|t)+\widetilde{D}_t\widetilde{A}_t^{N_p-1}\widetilde{B}_t\Delta u(t|t)+\widetilde{D}_t\widetilde{A}_t^{N_p-1}\widetilde{C}_t+\cdots +\widetilde{D}_t\widetilde{B}_t\Delta u(t+N_p-1|t)+\widetilde{D}_t\widetilde{C}_t \end{matrix} \tag{19} ξ(k+Npt)=A tNpξ(tt)+A tNp1B tΔu(tt)+A tNp1C t++B tΔu(t+Np1t)+C tη(k+Npt)=D tA tNpξ(tt)+D tA tNp1B tΔu(tt)+D tA tNp1C t++D tB tΔu(t+Np1t)+D tC t(19)
    将系统未来时刻的状态和输出以矩阵形式表达为:
    (20) X ( t ) = Ψ t ξ ( t ∣ t ) + Φ t Δ U ( t ∣ t ) + Υ t Y ( t ) = D ~ t X ( t ) \begin{matrix} X(t)=\Psi_t\xi(t|t)+\Phi_t\Delta U(t|t)+\Upsilon_t \\ Y(t)=\widetilde{D}_tX(t) \end{matrix} \tag{20} X(t)=Ψtξ(tt)+ΦtΔU(tt)+ΥtY(t)=D tX(t)(20)
    其中,
    (21) X ( t ) = [ ξ ( t + 1 ∣ t ) ξ ( t + 2 ∣ t ) ξ ( t + 3 ∣ t ) ⋮ ξ ( t + N p ∣ t ) ] , Y ( t ) = [ η ( t + 1 ∣ t ) η ( t + 2 ∣ t ) η ( t + 3 ∣ t ) ⋮ η ( t + N p ∣ t ) ] , Δ U = [ Δ u ( t ∣ t ) Δ u ( t + 1 ∣ t ) Δ u ( t + 2 ∣ t ) ⋮ Δ u ( t + N p − 1 ∣ t ) ] , Υ t = [ C ~ A ~ C ~ + C ~ A ~ 2 C ~ + A ~ C ~ + C ~ ⋮ ∑ i = 0 N p − 1 A ~ i C ~ ] , Ψ t = [ A ~ t A ~ t 2 A ~ t 3 ⋮ A ~ t N p ] , Φ t = [ B ~ t 0 0 ⋯ 0 A ~ t B ~ t B ~ t 0 ⋯ 0 ⋮ ⋮ ⋯ ⋯ ⋯ A ~ t N p − 1 B ~ t A ~ t N p − 2 B ~ t ⋯ ⋯ B ~ t ] X(t)=\begin{bmatrix} \xi(t+1|t)\\ \xi(t+2|t)\\ \xi(t+3|t)\\ \vdots\\ \xi(t+N_p|t)\\ \end{bmatrix}, Y(t)=\begin{bmatrix} \eta(t+1|t)\\ \eta(t+2|t)\\ \eta(t+3|t)\\ \vdots\\ \eta(t+N_p|t)\\ \end{bmatrix}, \Delta U=\begin{bmatrix} \Delta u(t|t)\\ \Delta u(t+1|t)\\ \Delta u(t+2|t)\\ \vdots\\ \Delta u(t+N_p-1|t)\\ \end{bmatrix}, \Upsilon_t=\begin{bmatrix} \widetilde{C}\\ \widetilde{A}\widetilde{C} + \widetilde{C}\\ \widetilde{A}^2\widetilde{C} + \widetilde{A}\widetilde{C} + \widetilde{C}\\ \vdots\\ \sum_{i=0}^{N_p-1}\widetilde{A}^i\widetilde{C} \end{bmatrix},\\ \Psi_t=\begin{bmatrix} \widetilde{A}_t\\ \widetilde{A}_t^2\\ \widetilde{A}_t^3\\ \vdots\\ \widetilde{A}_t^{N_p}\\ \end{bmatrix}, \Phi_t=\begin{bmatrix} \widetilde{B}_t &amp; 0 &amp; 0 &amp; \cdots &amp; 0\\ \widetilde{A}_t\widetilde{B}_t &amp; \widetilde{B}_t &amp; 0 &amp; \cdots &amp; 0\\ \vdots &amp; \vdots &amp; \cdots &amp; \cdots &amp; \cdots \\ \widetilde{A}_t^{N_p-1}\widetilde{B}_t &amp; \widetilde{A}_t^{N_p-2}\widetilde{B}_t &amp; \cdots &amp; \cdots &amp; \widetilde{B}_t\\ \end{bmatrix} \tag{21} X(t)=ξ(t+1t)ξ(t+2t)ξ(t+3t)ξ(t+Npt),Y(t)=η(t+1t)η(t+2t)η(t+3t)η(t+Npt),ΔU=Δu(tt)Δu(t+1t)Δu(t+2t)Δu(t+Np1t),Υt=C A C +C A 2C +A C +C i=0Np1A iC ,Ψt=A tA t2A t3A tNp,Φt=B tA tB tA tNp1B t0B tA tNp2B t0000B t(21)
    通过公式20我们可以看出,预测时域内的状态和输出量都可以通过从系统当前的状态量 ξ ( t ∣ t ) \xi(t|t) ξ(tt)和控制增量 Δ U ( t ∣ t ) \Delta U(t|t) ΔU(tt)求得,这也即是模型预测控制算法中“预测”功能的实现。
    结合mpc_slover.cc,对MPC的模型进行分析:

      unsigned int horizon = reference.size();  // horizon =10
    
      // Update augment reference matrix_t
      // matrix_t为60*1的矩阵,存储的参考轨迹信息
      Matrix matrix_t = Matrix::Zero(matrix_b.rows() * horizon, 1);
      ...
    
      // Update augment control matrix_v
      // matrix_v为20*1的矩阵,存储控制信息
      Matrix matrix_v = Matrix::Zero((*control)[0].rows() * horizon, 1);
      ...
    
      // matrix_a_power为含有10个矩阵的容器,每个矩阵为6*6,matrix_a为6*6矩阵
      std::vector<Matrix> matrix_a_power(horizon);
      ...
    
      // matrix_k为60*20的矩阵,matrix_b为6*2矩阵
      Matrix matrix_k =
          Matrix::Zero(matrix_b.rows() * horizon, matrix_b.cols() * horizon);
      ...  
    

    R e f = m a t r i x _ t , C t r = m a t r i x _ v , A = m a t r i x _ a , A P = m a t r i x _ a _ p o w e r , B = m a t r i x _ b , K = m a t r i x _ k Ref=matrix\_t, Ctr=matrix\_v, A=matrix\_a, AP=matrix\_a\_power, B=matrix\_b, K=matrix\_k Ref=matrix_t,Ctr=matrix_v,A=matrix_a,AP=matrix_a_power,B=matrix_b,K=matrix_k,根据代码有:
    (22) R e f = [ r e f 1 r e f 2 ⋮ r e f 10 ] , C t r = [ c t r 1 c t r 2 ⋮ c t r 10 ] , P A = [ A A 2 ⋮ A 10 ] , K = [ A B 0 ⋯ 0 A 2 B A B ⋯ 0 ⋮ ⋮ ⋯ ⋮ A 10 B A 9 B ⋯ A B ] Ref=\begin{bmatrix} ref_1\\ ref_2\\ \vdots\\ ref_{10} \end{bmatrix}, Ctr=\begin{bmatrix} ctr_1\\ ctr_2\\ \vdots\\ ctr_{10} \end{bmatrix}, PA=\begin{bmatrix} A\\ A^2\\ \vdots\\ A^{10} \end{bmatrix},\\ K=\begin{bmatrix} AB &amp; 0 &amp; \cdots &amp; 0 \\ A^2B &amp; AB &amp; \cdots &amp; 0 \\ \vdots &amp; \vdots &amp; \cdots &amp; \vdots \\ A^{10}B &amp; A^9B &amp; \cdots &amp; AB \end{bmatrix}\tag{22} Ref=ref1ref2ref10,Ctr=ctr1ctr2ctr10,PA=AA2A10,K=ABA2BA10B0ABA9B00AB(22)
    其中, r e f i ref_i refi为参考轨迹序列, c t r i ctr_i ctri为预测控制序列,预测时域为10个采样周期。

    滚动优化

    MPC的线性优化问题可以结合无人驾驶车辆模型预测控制第3.3.1节及以下文章:Model predictive control of a mobile robot using linearization进行学习。

    滚动优化的目的是为了求最优控制解,是一种在线优化,它每一时刻都会针对当前误差重新计算控制量,通过使某一或某些性能指标达到最优来实现控制作用。因此,滚动优化可能不会得到全局最优解,但是却能对每一时刻的状态进行最及时的响应,达到局部最优。

    因此,需要设计合适的优化目标,以获得尽可能最优的控制序列,目标函数的一般形式可表示为状态和控制输入的二次函数:
    (23) J ( k ) = ∑ j = 1 N ( x ~ ( k + j ∣ k ) T Q x ~ ( k + j ∣ k ) + u ~ ( k + j − 1 ∣ k ) T R u ~ ( k + j − 1 ∣ k ) ) J(k)=\sum_{j=1}^N(\widetilde{x}(k+j|k)^TQ\widetilde{x}(k+j|k)+\widetilde{u}(k+j-1|k)^TR\widetilde{u}(k+j-1|k)) \tag{23} J(k)=j=1N(x (k+jk)TQx (k+jk)+u (k+j1k)TRu (k+j1k))(23)

    其中, N N N为预测时域, Q , R Q, R Q,R为权重矩阵,且满足 Q ≥ 0 , R &gt; 0 Q\ge0, R&gt;0 Q0,R>0的正定矩阵。形如 a ( m ∣ n ) a(m|n) a(mn)表示,在 n n n时刻下预测的 m m m时刻的 a a a值。第一项反映了系统对参考轨迹的跟踪能力,第二项反映了对控制量变化的约束。此外,上式(公式23)在书写过程中,累加和函数 ∑ \sum 后几乎都不加"()",但我看起来实在难受,所以自行将 x , u x,u x,u一起括起来。
    优化求解的问题可以理解为在每一个采样点 k k k,寻找一组最优控制序列 u ~ t ∗ \widetilde{u}_t^* u t和最优开销 J ∗ ( k ) J^*(k) J(k),其中:
    u ~ t ∗ = arg ⁡ min ⁡ u ~ J ( k ) \widetilde{u}_t^*=\arg\min_{\widetilde{u}}{J(k)} u t=argu minJ(k)
    在MPC控制规律中,将控制序列中的第一个参数作为控制量,输入给被控系统。

    对于车辆控制来说,就是找到一组合适的控制量,如 u = [ δ f , a ] T u=[\delta_f, a]^T u=[δf,a]T,其中, δ f \delta_f δf为前轮偏角, a a a为刹车/油门系数,使车辆沿参考轨迹行驶,且误差最小、控制最平稳、舒适度最高等。因此在设计目标函数的时候可以考虑以下几点:

    1. 看过Apollo官方控制模块视频的人,可能知道在设计代价函数时候,一般设计为二次型的样式,为的是避免在预测时域内,误差忽正忽负,导致误差相互抵消;此外,对于实在不理解为什么代价函数要采用平方形式的同学,也可以参考损失函数为什么用平方形式(二)
    2. 可考虑的代价有:
      a. 距离误差(Cross Track Error, CTE),指实际轨迹点与参考轨迹点间的距离,误差项可表示为: ( z i − z r e f , i ) 2 (z_i-z_{ref,i})^2 (zizref,i)2
      b. 速度误差,指实际速度与期望速度的差,误差项可表示为: ( v i − v r e f , i ) 2 (v_i-v_{ref,i})^2 (vivref,i)2
      c. 刹车/油门调节量,目的是为了保证刹车/油门变化的平稳性,误差项可表示为: ( a i + 1 − a i ) 2 (a_{i+1}-a_i)^2 (ai+1ai)2
      d. 航向误差等…
    3. 约束条件
      a. 最大前轮转角
      b. 最大刹车/油门调节量
      c. 最大方向盘转角
      d. 最大车速等

    因此公式23中的第一项可表示为:
    (24) ∑ j = 1 N x ~ ( k + j ∣ k ) T Q x ~ ( k + j ∣ k ) = ∑ j = 1 N ( w 1 ( z i − z r e f , i ) 2 + w 2 ( v i − v r e f , i ) 2 + w 3 ( a i + 1 − a i ) 2 + . . . ) \sum_{j=1}^N\widetilde{x}(k+j|k)^TQ\widetilde{x}(k+j|k)=\sum_{j=1}^N(w_1(z_i-z_{ref,i})^2+w_2(v_i-v_{ref,i})^2+w_3(a_{i+1}-a_i)^2+...) \tag{24} j=1Nx (k+jk)TQx (k+jk)=j=1N(w1(zizref,i)2+w2(vivref,i)2+w3(ai+1ai)2+...)(24)
    其中, w i w_i wi为该项误差的权重,对应权重矩阵Q中的某一项。

    对于上述的目标函数(公式23),是有多个变量,且要服从于这些变量的线性约束的二次函数,因此可以通过适当处理将其转换为二次规划问题。

    基于公式20,将控制量变为控制增量,以满足系统对控制增量的要求,则目标函数可以改写为:
    (25) J ( ξ ( t ) , u ( t − 1 ) , Δ U ( t ) ) = ∑ i = 1 N ∣ ∣ η ( t + i ∣ t ) − η r e f ( t + i ∣ t ) ∣ ∣ Q 2 + ∑ i = 1 N − 1 ∣ ∣ Δ U ( t + i ∣ t ) ∣ ∣ R 2 J(\xi(t), u(t-1), \Delta U(t))=\sum_{i=1}^N||\eta(t+i|t)-\eta_{ref}(t+i|t)||_Q^2+\sum_{i=1}^{N-1}||\Delta U(t+i|t)||_R^2\tag{25} J(ξ(t),u(t1),ΔU(t))=i=1Nη(t+it)ηref(t+it)Q2+i=1N1ΔU(t+it)R2(25)

    在此基础上满足一些约束条件,一般如下:

    (26) { 控 制 量 约 束 : u m i n ( t + k ) ≤ u ( t + k ) ≤ u m a x ( t + k ) 控 制 增 量 约 束 : Δ u m i n ( t + k ) ≤ Δ u ( t + k ) ≤ Δ u m a x ( t + k ) 输 出 约 束 : y m i n ( t + k ) ≤ y ( t + k ) ≤ y m a x ( t + k ) \begin{cases} 控制量约束:u_{min}(t+k) \le u(t+k) \le u_{max}(t+k) \\ 控制增量约束:\Delta u_{min}(t+k) \le \Delta u(t+k) \le \Delta u_{max}(t+k)\\ 输出约束:y_{min}(t+k) \le y(t+k) \le y_{max}(t+k) \end{cases}\tag{26} :umin(t+k)u(t+k)umax(t+k):Δumin(t+k)Δu(t+k)Δumax(t+k):ymin(t+k)y(t+k)ymax(t+k)(26)
    其中, k = 0 , 1 , 2 , 3 , . . . , N − 1 k=0,1,2,3,...,N-1 k=0,1,2,3,...,N1

    将公式20代入公式23中,并将预测时域内输出量偏差表示为:
    (27) E ( t ) = Ψ t ξ ( t ∣ t ) − Y r e f ( t ) E(t)=\Psi_t\xi(t|t)-Y_{ref}(t) \tag{27} E(t)=Ψtξ(tt)Yref(t)(27)
    其中, Y r e f = [ η r e f ( t + 1 ∣ t ) , . . . , η r e f ( t + N ∣ t ) ] T Y_{ref}=[\eta_{ref}(t+1|t),...,\eta_{ref}(t+N|t)]^T Yref=[ηref(t+1t),...,ηref(t+Nt)]T
    经过一定的矩阵计算,可将目标函数调整为与控制增量相关的函数:
    (28) J (