【欠驱动机器人】【SLQ-MPC】摆锤和倒立摆的 SLQ 模型预测控制算法实现（Matlab代码实现）

最新推荐文章于 2025-04-26 03:16:05 发布

机器学习之心_T

最新推荐文章于 2025-04-26 03:16:05 发布

阅读量827

点赞数 29

文章标签：机器人 matlab 开发语言

本文链接：https://blog.csdn.net/2302_77323582/article/details/147133145

版权

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

⛳️赠与读者

💥1 概述

基于SLQ-MPC的欠驱动摆锤与倒立摆控制研究

一、欠驱动系统的核心特性与挑战

二、SLQ-MPC算法的基本原理与数学模型

三、倒立摆与摆锤的动力学建模方法

四、SLQ-MPC在欠驱动系统中的实现与案例

五、参数优化与稳定性增强策略

六、总结与展望

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、数据下载

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

基于SLQ-MPC的欠驱动摆锤与倒立摆控制研究

一、欠驱动系统的核心特性与挑战

欠驱动系统指控制输入维度小于系统自由度维度的非线性系统，其特点包括：

非完整约束性：存在速度或加速度层面的不可积分约束（如轮式机器人横向移动限制）。
动力学耦合：主动关节与被动关节间的非线性相互作用显著（如倒立摆的摆杆与小车运动耦合）。
能量效率与灵活性：通过减少驱动器数量实现轻量化，但控制复杂度大幅增加。

典型应用包括双足机器人、倒立摆、Acrobot等。其核心挑战在于全局可控性受限（仅局部可控）及控制策略需利用重力势能或外部约束辅助。

二、SLQ-MPC算法的基本原理与数学模型

SLQ-MPC（顺序线性二次模型预测控制）结合了动态规划与在线优化，适用于非线性系统的高效控制。其核心步骤如下：

动态方程与成本函数
系统动态：

成本函数（含状态跟踪与控制输入惩罚）：

其中Q、R为权重矩阵，Qf为终端成本。
系统线性化与迭代优化
- 局部线性化：在每个时间步，对非线性动态方程在当前状态附近进行泰勒展开，得到线性近似模型：
- LQR求解：基于线性化模型，通过差分动态规划（DDP）或LQR迭代求解最优控制序列。
预测时域与滚动优化
在每个控制周期内，预测未来时间窗口TT内的系统行为，优化控制输入序列，仅执行首步控制，并重复此过程。

三、倒立摆与摆锤的动力学建模方法

倒立摆建模
- 拉格朗日方程法：以小车位移xx、摆角θθ为广义坐标，通过动能TT和势能VV构建方程：

其中L=T−V，QiQi为非保守力广义力。

状态空间模型：典型状态变量为[x,x˙,θ,θ˙][x,x˙,θ,θ˙]，通过线性化平衡点（如θ=0θ=0）得到线性化方程。

摆锤建模
- 牛顿力学法：通过受力分析（如重力、铰链力矩）直接建立微分方程，适用于简单系统。
- 多体动力学工具：利用Modelica等工具构建多体模型，自动生成运动方程。

四、SLQ-MPC在欠驱动系统中的实现与案例

倒立摆控制案例
- 模型整合：将非线性倒立摆模型嵌入SLQ-MPC框架，通过局部线性化处理非线性动态。
- 约束处理：在成本函数中加入软约束项，限制小车位移与摆角范围（如∣x∣≤0.5 m,∣θ∣≤15∘）。
- 实验验证：文献中通过实体实验验证，SLQ-MPC在扰动下恢复平衡时间比传统LQR减少约30%。
摆锤摆动控制
- 能量最优策略：通过成本函数设计，最小化摆锤动能与控制输入能量消耗的加权和。
- 路径跟踪：在移动3D打印场景中，SLQ-MPC结合TCPP算法优化末端轨迹，实现摆锤精准定位。

五、参数优化与稳定性增强策略

权重矩阵调优
- Q矩阵：状态跟踪权重（如摆角误差权重高于小车位移）。
- R矩阵：控制输入惩罚权重，避免执行器饱和。
- 优化方法：使用灰色狼优化器（GWO）等算法自动搜索最优Q/RQ/R组合。
预测时域与控制时域选择
- 预测时域TT ：过长增加计算负载，过短降低鲁棒性。实验表明T=2 sT=2s在倒立摆控制中平衡性能与实时性。
- 控制时域步长：通常取10-20步，离散化间隔为0.1 s。
稳定性约束设计
- Lyapunov函数约束：在优化问题中强制要求Lyapunov函数递减，保证闭环稳定性。
- 输入饱和处理：通过约束∣u(t)∣≤umax，防止执行器过载。

六、总结与展望

SLQ-MPC在欠驱动系统中展现出处理非线性与约束的独特优势，但其计算效率仍需提升。未来研究方向包括：

并行化求解：利用GPU加速优化计算，满足更高实时性需求。
自适应参数调整：结合强化学习动态优化权重矩阵与预测时域。
多模态控制：在摆锤与倒立摆切换场景中，开发混合控制策略。

📚2 运行结果

部分代码：

% set parameters
% 1. load target trajectory generated from dircol
load('data_for_Q4.mat');
dt=0.025; 
x_des=[theta' theta_dot'];
u_des=2*optimal;
plot(x_des(:,1),x_des(:,2),'r.-');
hold all

n_steps=numel(optimal); %% total number of timesteps through which the system is tracked

% 2. Set MPC params
n_sim=3; % roll out current strategy through so many steps through time
x_start= x_des(1,:);
x_start=[0.5 0.0];%x_start=x0; % this is where we start from. Should be in the neighborhood of 
% start point of the known optimal path. So that the lqr can attract it to
% the start of the optimal trajectory
x_goal=[pi,0];
iter=0; % required to keep track of time horizon
Q_lqr= 1500.*eye(2);
Q_slq=1500.*eye(2);
R_lqr=1; 
R_slq=1;
% these 2 parameters will be used for the LRQ ricatti stuff

x_goal=[pi,0]; % always fixed for the goal state
% begin MPC loop
%  [u_slq]= SLQ_solve(x_des,u_des,x_start,Q_slq,R_slq,100.*eye(2),[1:1:200],[2 2],dt); % to see if slq works
%  hold all
%  pause
% intermediate_counter
while (1) % we will run this until finished
    [At,Bt] = lin_dynamics(x_start);% linear dynamics at start point of the known optimal path
    [K,~,~] = lqr(At,Bt,Q_lqr,R_lqr);
    t_left=n_steps-(iter+n_sim);
    iter=iter+1;
    if t_left>=n_sim
        t_vec=[iter:1:iter+n_sim];
    else
%         intermediate_counter=intermediate_counter+1;

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)

[1]赵明生.预测函数控制的算法实现[J].计算机工程与设计, 2005, 26(6):4.DOI:10.3969/j.issn.1000-7024.2005.06.060.

[2]廖娟娟.基于RBF-ARX模型的预测控制在倒立摆系统中的应用[D].中南大学[2025-04-10].

[3]尹亚光.基于非线性时间序列模型的倒立摆起摆预测控制研究[D].中南大学,2014.

🌈4 Matlab代码、数据下载

资料获取，更多粉丝福利，MATLAB|Simulink|Python资源获取

在这里插入图片描述