底层运动控制算法及代码实现（学习笔记一）

最新推荐文章于 2025-12-10 10:00:47 发布

原创最新推荐文章于 2025-12-10 10:00:47 发布 · 693 阅读

10 ·

CC 4.0 BY-SA版权

文章标签：

#新人首发 #学习

运动控制同时被 2 个专栏收录

2 篇文章

订阅专栏

底层运动控制

2 篇文章

订阅专栏

经典控制算法：

基于传递函数，只考虑输入输出关系

主要方法：PID、根轨迹法、频率响应法（波特图、奈奎斯特图）等

一般适用于：单输入单输出（SISO），线性时不变，低阶系统

现代控制算法：

基于状态空间方程

$\dot{x}= Ax+Bu$

$y=Cx+Du$

主要方法：状态反馈、极点配置、最优控制（LQR/LQG）、现代估计（卡尔曼滤波）、鲁棒控制等

可以用于：多输入多输出（MIMO），时变，非线性（线性化/非线性方程）

一、PID控制算法 ✳✳✳✳✳

二、LQR（Linear Quadratic Regulator）：线性二次型最优状态反馈 ✳✳✳✳✳

三、模型预测控制（MPC） ✳✳✳✳✳

四、自适应控制 ✳✳✳✳✳

五、对比

一、PID控制算法（传统）——误差控制

1.基本内容

PID（比例-积分-微分）控制是最经典的控制算法之一，通过调节比例、积分和微分三个参数实现误差的最小化。其输出公式为：

$u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt}$

其中：

u(t) 为控制输出；
e(t) 为系统误差（设定值与实际值之差）；
K_p 、K_i 、K_d 分别为比例、积分、微分增益。

PID控制适用于线性系统，需通过调参（如Ziegler-Nichols方法）优化性能，不需要系统模型，是误差控制。

2.比例（P）

按当前误差大小成比例输出控制量
误差越大，作用越强，加快响应、提高调节速度
Kp 过大容易 超调、振荡甚至不稳定

3.积分（I）

对误差随时间累加
积分会不断“累积”，消除稳态误差，提高稳态精度
积分过强会导致振荡甚至不稳定

4.微分（D）

根据误差的变化率（趋势）来提前动作
改善瞬态过程，减小超调，提高系统稳定性和平滑度
对噪声敏感，会放大高频噪声

5.调参（需要自己调节的参数）：K_p 、K_i 、K_d

二、LQR（Linear Quadratic Regulator）：线性二次型最优状态反馈

1.要解决的问题：

对一个线性系统（不要求连续）

$\dot{x}= Ax+Bu$ （连续形式）

$x_{k+1} = A_d x_k + B_d u_k$ （离散形式）

找到一个控制律u（即求K）

$u=-Kx$

使得系统稳定，并使性能指标（能量、误差等）最优，性能指标是一个积分成本函数（目的是找到最小值）：

$[ J = \int_{0}^{\infty} (x^T Q x + u^T R u) dt ]$

( Q ) 通常为半正定矩阵，对角线元素对应状态变量的权重。增大 ( Q ) 会加快状态收敛，但可能导致控制量过大。
( R ) 为正定矩阵，对角线元素表示控制输入的惩罚。增大 ( R ) 会限制控制量，但可能降低响应速度。

2.Riccati 方程求解（借助计算机计算不用自己算）

K由代数Riccati 方程得到，即LQR 的核心是求解代数 Riccati 方程（ARE）：

$[ A^T P + P A - P B R^{-1} B^T P + Q = 0 ]$

解 ( P ) 是一个对称正定矩阵

计算得到最优反馈增益矩阵 ( K )：

$[ K = R^{-1} B^T P ]$

最优控制律为：

$[ u = -K x ]$

3.实现步骤

系统建模：将实际系统表示为线性状态空间形式
权重选择：根据控制目标调整 Q 和 R 的数值。
求解 Riccati 方程：使用数值工具（如 MATLAB 的 lqr 函数）计算 P 和 K
闭环仿真：验证闭环系统 $( \dot{x} = (A - B K) x )$ 的稳定性和性能。

4.调参（需要自己调节的参数）：权重矩阵 Q 和 R

5.注意事项

LQR 仅适用于线性系统。对于非线性系统，需先线性化（如在工作点附近）。
若系统不可控，需检查状态空间模型或引入输出反馈（如 LQG）。

三、模型预测控制（MPC）——离散系统

参考：https://www.bilibili.com/video/BV1cL411n7KV/?spm_id_from=333.788.recommend_more_video.0

1.基本内容

MPC是一类利用系统模型，在未来一段时间内预测系统行为，并通过在线优化计算控制输入 的先进控制方法。

2.核心思想

预测（Predict）— 优化（Optimize）— 滚动执行（Receding Horizon）

每一步都需要最优化，对控制器求解能力要求高

3.具体步骤

步骤一：系统模型建立

$x_{k+1} = A_d x_k + B_d u_k$

步骤二：构建预测序列 —— 根据当前状态（k时刻），预测系统未来 N 步的轨迹

MPC 会使用模型 预测未来 N 步的状态序列：

$x_{k+1},x_{k+2},...,x_{k+N}$

步骤三：构建代价函数进行优化 —— 优化目标

基于 $u_{k+1},u_{k+2},...,u_{k+N}$ 进行优化

其中代价函数（找最小值）：

$[J= \sum_{k=0}^{N-1} \left( E_k^{T} Q E_k + U_k^{T} R U_k + E_N^{T} F E_N \right)]$

Q：状态权重（偏差越重要越大）
R：控制权重（希望控制越平滑，R 越大）
P：终端权重（通常是 LQR 的 P）

步骤四：滚动执行（Receding Horizon）

只施加 $u_k$

4.最优化如何进行

把模型转换成二次规划（Quadratic Programming）的形式：

$[ min \left ( \frac{1}{2} z^{T} Q z + c^{T} z \right ) ]$

QP求解的方法很成熟，不需要自己算，借助计算机即可。

难点在于如何把J转换成QP形式

5.约束条件

可以加入约束条件（优化问题本来就和约束适配）

输入约束： $[u_{\min} \;\le\; u \;\le\; u_{\max}]$
状态约束： $[x_{\min} \;\le\; x \;\le\; x_{\max}]$
输出约束： $[y_{\min} \;\le\; y \;\le\; y_{\max}]$
安全约束（例如机器人避障）

6.优势与挑战

优势：

显式处理多变量系统和约束条件；
适用于时滞、非最小相位系统；
通过滚动优化实现鲁棒性。

挑战：

计算复杂度高，依赖实时优化算法；
模型精度影响控制性能；
需要调整参数（如时域长度、权重矩阵）。

四、自适应控制

1.基本内容

自适应控制是一种能够根据系统动态变化或环境不确定性自动调整控制器参数的控制方法。其核心目标是确保系统在未知或时变条件下仍能保持稳定性和性能。

2.自适应控制的主要类型

（1）模型参考自适应控制（MRAC）
通过设计参考模型来定义期望的系统动态，控制器参数实时调整以使实际系统输出跟踪参考模型输出。典型结构包括参考模型、可调控制器和自适应律。

参考模型：定义期望的系统动态特性，通常为线性时不变系统，输出为理想响应。

$\dot{x}_m = A_m x_m + B_m r$

其中 ( x_m ) 为参考模型状态，( r ) 为参考输入，( A_m ) 和 ( B_m ) 为稳定矩阵。

可调系统：实际系统通过可调参数（如增益、反馈矩阵）与控制器耦合。

$\dot{x}_p = A_p x_p + B_p u$

其中 ( u ) 为控制输入，通常设计为状态反馈形式：

$u = K_x x_p + K_r r$

误差动态：定义跟踪误差

$e = x_p - x_m$

自适应率：通过自适应律调整 K_x 和 K_r

$[\dot{\hat{\theta}}= -\gamma \, \phi \, e]$

$[\phi = \begin{bmatrix} x \\ r \end{bmatrix}]$

（2）自校正控制（STC）
基于在线参数估计调整控制器。通过实时辨识系统模型参数（如最小二乘法），动态更新控制器设计（如极点配置或PID参数）。适用于参数缓慢变化的系统。

五、对比

方法	是否需系统模型	是否能处理非线性	是否能处理约束	是否能适应参数变化	工程复杂度	典型使用场景
PID	❌ 不需要	❌ 很弱	❌ 不行	❌ 不行	⭐ 最低	简单单变量系统、工业过程、舵机、电机
LQR	✔ 需要线性模型	❌ 只能线性或线性化	❌ 不行	❌ 不行	⭐ 中等	多变量线性系统、姿态控制、倒立摆、机器人局部控制
MPC	✔ 需要模型	✔（可扩展到非线性）	✔ 最强	❌（或 AMPC 可部分做到）	⭐⭐⭐ 最高	约束控制、无人车/无人机轨迹控制、化工过程控制
自适应控制（MRAC/STR）	❌ 不需准确模型	✔（结构已知即可）	❌ 一般版不可以	✔✔✔ 非常强	⭐⭐ 较高	参数不确定系统、负载变化、飞行器、机器人动力学控制