模型预测控制本质上是一种基于优化控制算法,通过对车辆的运动状态进行预测,然后优化目标,求解有限时间内的开环优化问题,并将预测时间周期的前部分控制量作用于被控对象。

在上一篇文章中介绍了基于车辆动力学的模型预测控制,该算法的运算量大,考虑了车辆摩擦力打滑等动力学参数,因此较多适用于车辆的高速控制场景。对于低速控制场景,由于车辆的速度,加速度等参数不会太大,车辆的摩擦力没有饱和,此时可以考虑采用运动学模型进行车辆的预测控制。前面讲到MPC算法本质上是一种基于优化的控制算法核心是构建优化函数,通常由三个步骤组成:

预测模型:在优化的时间段内,根据车辆当前时刻状态预测未来时间段内的最优控制输出。

滚动优化:在控制周期到来之时根据车辆当前状态量反复优化未来时间段内的所有控制状态量,保证控制的平滑性。此时需要构建控制误差的优化目标函数,来获得最优的控制输出量。

反馈矫正:在每一个控制周期到来之时根据测量反馈对车辆当前的初始误差状态进行修正更新。

基于运动学的车辆控制运动状态误差模型

基于运动学的车辆控制的运动模型是基于车道线坐标系的运动学模型,可以通过前面讲到 的frenet坐标变换转换到将当前车辆的位姿转换到车道线的frenet坐标系中。假设车辆前轮转向后轮驱动,如下图所示,且车辆的车体坐标系在后轮的中心O上,前轮转向角用 [公式] 表示,后轮驱动速度用 [公式]表示。

假设车道线的切线方向为x轴方向,垂直方向为y轴方向,则车辆运动控制的误差状态示意图如下图所示:

则车辆在坐标系O下的位姿状态转移方程为:

式中 [公式] 为机器人t时刻在车体坐标系baselink下的位置和方向角。

其中,trajectory为车体坐标系下轨迹。

因此,轨迹相对于车体baselink坐标系的横向位移和方向角为:

横向位移: [公式]

方向角: [公式]

则t时刻轨迹的横向位移误差和方向角误差为:

模型优化函数构建

为了找到最佳控制量的 [公式]  [公式],将构建跟踪误差的成本代价函数 [公式] ,并且将使用Ipopt(MPC)来优化目标函数。目标函数的构建原则是,使得期望的最优状态和估计最优状态之间的误差最小。它主要由以下几个部分组成:

跟踪误差代价

轨迹跟踪横向位移误差代价:

轨迹跟踪方向角误差代价:

轨迹跟踪速度误差代价:

控制输出代价

转向代价:

加速代价:

转向变化率代价:

加速变化率代价:

则在预测时间周期N内的模型优化目标函数为:

其中,

另外,在实际的跟踪控制过程中 [公式] 

模型优化

上述模型是一个非线性最优化问题,因此该模型的优化,可以采用CppAD和Ipopt中的非线性规划。 优化问题构建如下所示: