LQR、MPC、WBC

### LQR、MPC 和 WBC 控制算法概述 #### 线性二次调节器 (LQR) 线性二次调节器是一种用于优化控制系统性能的方法，适用于线性系统。该方法通过最小化一个代价函数来找到最优控制输入[^1]。 - **概念**: LQR 是一种基于状态反馈的控制器设计技术，旨在使系统的输出尽可能接近期望轨迹的同时保持能量消耗最低。 - **原理**: 设计者定义了一个关于误差平方和控制努力的成本函数，并求解黎卡提方程得到最佳增益矩阵 K* 。这个过程可以形式化为： \[ J(u)=\int_{0}^{T}\left(x(t)^{T} Q x(t)+u(t)^{T} R u(t)\right) d t \] 其中 \(Q\) 表示状态偏差权重矩阵而\(R\)表示控制量权重矩阵. - **应用场景**: 主要应用于具有已知动态特性的简单机械臂或飞行器姿态调整等领域，在这些场合下被控对象通常能够近似看作线性时不变(LTI)系统. ```python import numpy as np from scipy.linalg import solve_continuous_are def lqr(A, B, Q, R): P = solve_continuous_are(A, B, Q, R) K = np.dot(np.linalg.inv(R), np.dot(B.T, P)) return -K ``` #### 模型预测控制 (MPC) 模型预测控制是一类先进的多变量约束优化控制策略，它利用未来一段时间内的预测模型来进行决策规划并实时滚动更新计划以应对环境变化的影响. - **概念**: MPC 使用数学模型对未来的行为做出预估，并在此基础上制定一系列动作序列使得整个操作周期内目标函数达到极小值. - **原理**: 通过对给定时间段上的预期响应建立成本评估体系，然后采用数值最优化手段寻找能使此期间累积损失降至最少的一组指令集. 这种方式允许引入物理边界条件和其他限制因素作为额外考量项加入到问题描述之中. - **应用场景**: 广泛存在于化工流程自动化、汽车自动驾驶辅助功能开发以及电力调度管理等多个方面; 特别适合处理含有不确定性和复杂交互作用的过程. ```matlab function mpc_control(plant_model, horizon_length, cost_function, constraints) % Define optimization variables and parameters here... end ``` #### 整体重定向控制 (WBC) 整体重定向控制是指针对具备多个自由度冗余结构特征的服务机器人所采取的一种综合协调机制，其目的是为了实现高效的任务执行与自我保护之间的平衡. - **概念**: WBC 方法试图在同一框架下解决关节空间中的速度分配难题——即如何合理安排各部分肢体的动作速率才能既满足外部作业需求又能规避潜在风险？ - **原理**: 利用优先级划分原则确立不同层次的目标重要程度顺序关系；再借助雅可比伪逆变换工具将笛卡尔坐标系下的任务要求映射回内部参数表达式里去完成具体实施步骤的设计工作. - **应用场景**: 多见于双足行走装置步态生成研究领域之内，也常用来指导仿人形机体的手眼协作抓取物体等活动. ```cpp Eigen::VectorXd computeWholeBodyControl(const Eigen::MatrixXd& jacobian, const Eigen::VectorXd& task_velocity, double damping_factor) { // Compute the pseudo-inverse of Jacobian matrix with regularization term auto inv_jac = (jacobian.transpose() * jacobian + damping_factor * Eigen::MatrixXd::Identity(jacobian.cols(), jacobian.cols())) .inverse() * jacobian.transpose(); return inv_jac * task_velocity; } ```