控制问题-耦合控制

此处提供了一个针对“控制问题-组合控制任务”的Matlab上手示例，示例同时考虑横向路径跟踪控制和纵向车速的控制，采用LQR控制器进行控制，最终采用横向平均跟踪误差作为指标进行结果评价。完整项目的下载链接为：https://github.com/TOPSlearningcenter/COMBINE

以下给出了模型介绍、操作步骤与代码解读。

模型介绍

1. 模型概述：

LQR (Linear Quadratic Regulator)即线性二次型调节器，LQR可得到状态线性反馈的最优控制规律，易于构成闭环最优控制。LQR算法将线性系统的控制问题转换成二次优化问题，并无限时间的二次型调节器能通过理论计算得到优化问题的解析表达式，因此在线求解运算量较小。

轨迹跟踪问题接收上游规划决策的轨迹或路径数据，通常为带时间戳的坐标点。输出为车辆方向盘/前轮转角和油门开度。本案例主要解决横向轨迹跟踪制问题，纵向以速度形式直接输出。

2. 文件说明：

  • reference.mat 为参考轨迹数据，包含时间戳 t_ref ,x坐标 x_ref ,y坐标 y_ref 三个变量。参考轨迹通常由上游决策规划模块生成。
  • Sim_Demo.slx 为Simulink文件，用于图形化编程及仿真运行。主要包括整体控制框架和被控对象模型等。
  • LQR_sFunction.m`控制器s函数文件，LQR控制算法代码编写在该文件中。
  • evaluator.mlx 后处理部分代码在该文件中，包括轨迹、误差、车辆状态参数曲线的绘制，动画演示。

操作步骤

0. 准备工作：

  • 本项目可运行在MATLAB R2022a及以上版本，需安装完整的Simulink工具箱。
  • 下载项目并解压，将MATLAB的工作路径设置为该文件夹；

  • 或将文件夹中的文件复制到当前文件夹下。

1. 运行文件：

  • 双击 Sim_Demo.slx 文件，并等待启动Simulink。
  • 点击 运行 按钮开始仿真。
  • 运行结束后，后在工作目录下会生成 reference_processed.mat 和 Sim_result.mat 两个数据文件。reference_processed.mat 为仿真时控制器为减少重复计算，初始化计算产生的中间数据。Sim_result.mat 为仿真结果数据。

2. 后处理运行结果：

  • 仿真结束后，需要对结果进行可视化绘图。双击 evaluator.mlx 。
  • 在 实时编辑器 选项卡下，按顺序点击 运行节 ，查看每一节的运行结果。

代码解读

1. 仿真模型：

如图，仿真模型分成控制器和被控对象两部分。控制器由s-函数编写而成，被控车辆是MATLAB提供的三自由度车辆模型，模型框图如下:

  • 组合控制的设置参数如下图，红框中需要进行设置。
    • 设置车辆为双轨模型，即左右车轮可以给定不同的参数，在转向时需要考虑阿克曼转角关系。驱动上给四个车轮的纵向力，理论上可以实现纵向力前后及左右的分配策略
    • 设置车辆初始速度为8m/s，防止起步目标突变带来的问题。
  • 绿框中的参数可以读取记下，后续控制器设计时的参数。

2. LQR控制器：

  • LQR控制器用s函数编写，主要用到输出模块和初始化模块，其主函数为
Copy to Clipboard

LQR控制器需要基于一个线性的模型，然后在模型的基础上，对输入和误差设目标标函数，构造一个优化问题。本质上控制的输入是一个序列，输出是目标函数的指标，因此优化问题是一个泛函极值问题。得益于目标函数二次和积分的形式，该泛函问题通过变分法可以进行求解。最优解可以通过求解一个黎卡提微分方程得到。对于线性定常系统的无限时间调节器，黎卡提方程进一步可以退化为代数黎卡提方程。因此在控制器中，只需要在线求解代数黎卡提方程并计算输出就能得输出的解析表达式。该表达式为一些简单的矩阵运算就能得到控制量。

  • 首先建立控制模型，全局坐标系中，以后轮轴中心为参考点，车辆的运动学方程可表示为

式中为系统状态，为控制量
将LQR控制，顾名思义需要作用到线性模型上。因此需要将模型线性化，将非线性方程在参考点泰勒展开到一阶得到扰动量的线性化模型

  • 定义扰动量:

  • 参考量中\(X_r\)和\(Y_r\)直接可以通过时间索引到，而参考横摆角计算公式为

  • 参考车速计算公式为

  • 参考加速度进一步在车速基础上继续微分

  • 参考前轮转角通过运动学模型第三个方程得到

将转向的车辆瞬时看作匀速圆周运动，因此有

其中，为瞬时圆周运动的半径，即轨迹的曲率半径，式子代入化简得到

其中，\(W_q\)为轨迹的曲率，表达式为

以上参考值的求解可以放在初始化函数中

Copy to Clipboard

为保证控制跟踪准确，只需要\(X-X_r \rightarrow 0, Y-Y_r\rightarrow 0, v-v_r\rightarrow 0\)故输出方程可以定义为:

因此可以系统写成状态空间表达式

现在的控制目标为，在整个控制过程中，被控量的误差\(\tilde{\mathcal{Y}}\)要尽可能小，输入也不宜过大，因此目标函数可以写成

其中，\(Q\)为\(3\times3\)的对称正定矩阵，通常我们将权重放在对角线上，对角线的位置对应被控量，权重越大，对应被控量的误差在目标函数中占比就会越多，算法会优先减小改部分误差。\(R\)为\(2\times2\)的对称正定矩阵，通常也将权重放在对角线上，对应的是两个输入量。权重越大，算法优先减小该部分的输入。

至此，控制问题变成，需要得到一个合适的控制函数\(\tilde u\)作用在受约约束从系统下，最终让上述目标函数最小。这是一个有约束的泛函极值问题。为求解这个问题首先利用拉格朗日乘数法将有约束优化问题问题变成无约束的极值问题，再用变分法进行求解。同时将\(\tilde{\mathcal{Y}}=C\tilde{\mathcal{X}}\)代入后，增广后的目标函数为

式中，\(\lambda\)为引入的协状态。
整理一下，令哈密尔顿函数为:

则目标函数变成

利用变分法公式得到目标函数的变分为

为以示区别，其中\(\Delta\)为变分符号，当\(t_f \rightarrow \infty\)时，最终状态需要稳定到目标值，因此\(\Delta \tilde{\mathcal{X}}(t_f)=0\)截断方程自然满足。为满足极值时变分\(\Delta J_a=0\)，则需要满足

代入后有，将状态方程一并放入有

通过观察发现，这些方程，每个时刻都是线性的映射关系，因此设前置条件为

这里允许映射关系\(P\)也是可以是时变的

代入前面三个方程进行整理得到

加入上述前置条件后，整理的方程必须恒成立才能说明加入的条件是正确的，则有黎卡提微分方程为

更进一步，我们观察到系统的参数都是时不变的，且为无限时间调节器，因此设\(P\)也是时不变的\(\dot P=0\)，从而将黎卡提微分方程退化为代数方程

手动可以通过待定系数法求解黎卡提代数方程，在MATLAB中利用`care`函数可以直接求解黎卡提方程，调用格式为`[P,~,~]=care(A,B,Q1,R)`
其中\(Q_1=C^TQC\)

求解得到\(P\)后解得控制输入为

将上述LQR求解器的求解过程封装在函数中得到

Copy to Clipboard

3. 纵向力分布：

最后，将增量控制量还原成实际控制量。

对于纵向的加速度（即油门开度）可以进一步换算到车轮的纵向力或转矩上，具体看被控车辆模型的输入。本例中，输入为车轮的纵向力。因此需要纵向力进行分配。整体的纵向力可以计算为

其中，\(m\)为整车质量
前后轴纵向力的分配可以根据前轴的垂向力情况进行按比例分配，有利于充分发挥路面附着系数。前后这轴垂向力分为静态和动态两部分，静态垂向力由质心位置决定，动态垂向力由纵向加速度引起。动态加速引起的载荷转移量为

其中，\(h\)为质心高度，\(a\)和\(b\)为车辆质心到前轴和后轴的距离
因此前后轴的垂向力为

前后轴的纵向力为：

将上述过程放在s函数的输出函数中得到

Copy to Clipboard

4. 后处理结果：

  • 组合控制对横纵向均施加控制，其跟踪误差可以用对应时刻参考位置和实际位置的误差的平均距离来表示：

本示例中，位置平均跟踪误差为 \(\bar{dis}_{err}=0.0554\mathrm{m}\)

  • 路径跟踪的效果和误差曲线如下图：

  • 速度跟踪的效果和误差曲线如下图

  • 输入的前轮转角和前后轴的纵向力输入如下图

  • 轨迹跟踪效果的动图如下