简介面向机器人工程与仿生技术学习者这是一份基于模型预测控制MPC算法的轮式四足机器人动态运动控制仿真项目。内容聚焦轮腿复合机器人在复杂地形下的适应性运动围绕地面反作用力优化、关节力矩计算、雅可比矩阵变换与正向运动学解算等核心问题展开能够帮助读者系统理解MPC在机器人运动控制中的落地方法。压缩包共10个文件以MATLAB脚本.m和Simulink多体动力学模型.slx为主另含markdown/txt说明文档与docx附赠资料包体仅170KB便于快速下载与运行调试。目前已有59人学习适合有初步机器人学基础、正在开展课程设计或毕业设计的工程师与学生参考。通过Simscape多体模型和控制器脚本使用者可复现仿真环境观察不同坡度与地形下的力矩变化和运动响应为后续算法优化提供直接的研究样本。1. 轮式四足机器人的动态运动控制为什么要选模型预测控制轮式四足机器人也叫轮腿复合机器人的难点在于它同时具备腿部机构的离散步态和轮式的连续滚动两种运动模式切换时系统动力学呈现明显的混合特征——四轮着地时是典型移动基座抬起腿越障时又退化成浮动基座。对这种系统做动态运动控制传统PID很难同时管住“身体姿态”和“轮地接触力”两个目标而模型预测控制MPC恰好能把地面反作用力作为显式决策变量写进优化问题在预测未来若干步的同时满足接触力锥形约束、关节极限约束。本文从Simscape多体建模出发走完正运动学解算、雅可比矩阵变换到关节力矩计算的全链路给出一个可以在仿真里跑通的完整方案。适合正在做轮腿机器人控制、想从纯运动学仿真升级到动力学控制的工程师。2. 用Simscape搭建轮腿机器人多体模型正运动学与雅可比矩阵的仿真映射2.1 从DH参数到Simscape刚体树正运动学解算的建模起点轮腿复合机器人的正运动学解算理论上可以从经典的DH参数表出发。比如每条腿设计为“髋关节偏航—髋关节俯仰—膝关节俯仰—轮毂电机”的4自由度结构轮子本身不参与姿态解算但在速度层要计入它的滚动自由度。DH参数表里最关键的是髋关节到轮心的齐次变换矩阵它决定了后续雅可比矩阵的形态。在Simscape Multibody里搭建时我一般不用手写变换矩阵而是直接把它做成rigidBodyTree对象再用getTransform()验证正运动学% 构建刚体树注意基座坐标系定义在机身质心 rbt rigidBodyTree(DataFormat, column, MaxNumBodies, 20); % ... 逐段添加刚体与关节略去重复代码 ... % 给定一组关节角计算左前腿末端轮心位姿 q [0.1; -0.4; 0.8; 0]; % 髋偏航、髋俯仰、膝俯仰、轮转角 T getTransform(rbt, q, wheel_front_left); pos tform2trvec(T); % 轮心在基座坐标系下的位置这里getTransform返回的是4×4齐次矩阵tform2trvec提取平移分量。需要注意Simscape里关节角度的正方向必须与DH定义一致否则后续雅可比矩阵的符号会反——这个问题在仿真里隐蔽性强建议每搭一条腿就用一组已知构型验证一次。Simscape与原生的rigidBodyTree有个明显的差异Simscape里刚体质量、惯性张量直接影响动力学仿真而rigidBodyTree只做运动学。二者必须建立对应关系我通常先在Simscape里完成质量属性设置再用exportrobot把刚体树导出为URDF网格化模型后回灌到Simscape中保证两套模型的对象一致。2.2 轮地接触建模地面反作用力的仿真物理基础轮式四足机器人在地面滚动时真正“撑住”机器人的是轮地接触力而MPC的输出恰恰是期望的地面反作用力——模型和控制器之间必须用一个可量化的接触模型衔接。Simscape里实现轮地接触力常见做法是用Spatial Contact Force模块设置接触刚度和阻尼系数。轮子我建议简化为圆柱体不要做带胎纹的精细几何否则仿真步长会被接触检测拖慢一个量级。法向力采用Hertz接触模型F_normal max(0, k * penetration^1.5 c * penetration_dot)切向力用库仑摩擦模型加Stribeck速度修正% 切向摩擦力计算伪代码在Simscape内部通过自定义力实现 v_tangent norm(v_t); % 接触点切向滑移速度 mu mu_k (mu_s - mu_k) * exp(-abs(v_tangent) / vs); F_tangent -mu * abs(F_normal) * v_tangent / (v_tangent eps);mu_s是静摩擦系数mu_k是动摩擦系数vs是Stribeck速度。这四个参数直接影响机器人在斜坡上的表现mu_k设得偏小MPC计算出的期望反作用力超出摩擦锥机器人就会在仿真里原地打滑mu_s设得偏大平地启动时会出现高频抖动。一个初始参考值是mu_s 0.8、mu_k 0.6、vs 0.1 m/s接触刚度设为1e5 N/m量级。2.3 从Simscape读取正运动学与雅可比仿真里的实际做法Simscape本身不直接提供雅可比矩阵但可以通过两种途径获得一是用Simscape的Transform Sensor测出各关节相对基座的位姿配合数值求导二是直接调用rigidBodyTree的geometricJacobian函数把关节角从Simscape传出来计算。工程上第二条路更稳代码很短% 假设joint_pos是从Simscape里读出的8×1关节角向量四腿各2个主动关节4个轮角 % 基座到轮心的雅可比输出6×N矩阵N为关节数 J_left_front geometricJacobian(rbt, joint_pos, wheel_front_left);这里geometricJacobian返回6行前3行是线速度映射后3行是角速度映射。轮腿机器人的特殊之处在于轮子的滚动自由度让雅可比矩阵出现了“欠驱动”行——即使所有关节锁死轮子滚动依然会让机身移动。因此算地面反作用力到关节力矩的映射时要单独考虑轮子接触点处的约束不能简单对全矩阵求伪逆。3. 模型预测控制器的核心设计预测模型、约束与滚动优化3.1 预测模型选择为什么用单刚体动力学而非全阶多体模型轮腿机器人的全阶动力学模型自由度非常多机身6自由度腿部关节12个直接拿来做MPC的预测模型优化求解时间完全跟不上20~50 Hz的控制频率。仿真项目里通常采用单刚体动力学Single Rigid Body DynamicsSRBD模型作为预测模型。SRBD模型的思路是忽略腿的质量把机器人等效为一个刚体质心和四条腿末端轮子接触点构成的空间力系。动力学方程分两部分——质心平移和姿态旋转% 质心平移方程 m * (p_ddot - g) sum(f_i) % 姿态方程 I * omega_dot omega × (I * omega) sum(r_i × f_i) % 其中 r_i 是接触点到质心的向量f_i 是地面反作用力变量的约定p是质心位置omega是机体角速度I是转动惯量张量在机体坐标系下f_i是第i个接触点的地面反作用力世界坐标系。把这个连续方程用前向欧拉离散步长取MPC的控制周期dt 0.02s预测时域N 10每个步长里的地面反作用力即为优化变量维度是3×4×10 120QP求解规模很小。离散化后任意时刻的质心位姿都表达成地面反作用力的线性组合这正是MPC能把动力学约束转化为线性等式的关键。代码里可以用稀疏矩阵预计算每一时刻的系数N 10; nf 12; % 4个接触点每个3维力 % 预计算从力向量到质心位移的递推矩阵 A_pred (3N × nf*N) % 第k步质心位置 p_k p_0 k*dt*v_0 dt^2/m * A_pred(:,:,k) * f_seq A_pred build_centroid_map(N, dt, mass);3.2 MPC优化目标与约束地面反作用力优化怎么实现MPC每一拍要解的优化问题目标函数有三个层次跟踪误差质心位置、速度、姿态、地面反作用力变化率保证力的平滑性、松弛变量防止约束过死导致无解。min sum_k ( x_k - x_ref_k ) Q ( x_k - x_ref_k ) sum_k ( f_k - f_prev ) R ( f_k - f_prev ) rho * epsilon^2 s.t. 动力学等式约束SRBD fz_i fz_min 接触点不能有拉力 |fx_i| mu * fz_i 摩擦锥约束 |fy_i| mu * fz_i |tau_j| tau_max 关节力矩极限约束 |epsilon| 上限Q矩阵的权重分配我实测下来的经验是质心高度误差权重调大Q_pz 200水平位置误差权重适中Q_px 100姿态角误差权重取Q_rpy 150。速度误差权重可以设小20~50因为速度跟踪主要靠反作用力大小控制权重过大会引发高频振荡。R矩阵控制力的平滑度设R 1e-4 * I。松弛变量权重rho设到1e6它只在任务本身超出物理可行性时激活正常运动应该让松弛变量恒为0。求解器方面仿真项目里直接用quadprog或OSQP都行。OSQP的优势是支持热启动——上一帧的最优解作为下一帧的初值迭代次数能减少40%以上。代码如下% 构建QP矩阵稀疏形式 prob optimproblem(Objective, objective, Constraints, cons); % 或者直接调 osqp 的 MATLAB 接口 solver osqp; solver.setup(P, q, A_lin, l, u, warm_starting, true); results solver.solve();注意每帧的参考轨迹可能变化所以q向量目标函数一次项要逐帧更新而P矩阵如果系统模型不变可以只在初始化时构建一次。3.3 复杂地形适应性运动MPC如何“看到”前方路面轮式四足机器人要适应复杂地形关键在于MPC预测时域内的接触点和接触法向必须随地形变化。仿真中最直接的实现方式预先把地形高度图或数值高程函数传给控制器每个预测步长内的接触点高度根据当前机身预测位置插值获得。% 预测第k步的接触点位置简化处理默认接触点在轮子正下方 contact_pos_x(:, k) p_x_pred(:, k) r_offset_x; contact_pos_z(:, k) terrain_height(contact_pos_x(:, k)); % 更新动力学方程中的 r_i质心到接触点的向量 r_i(:, :, k) contact_pos(:, k) - p_pred(:, k);这就实现了“预瞄”——当机器人前方有台阶时MPC提前几个步长感知到接触点抬高于是调整机身姿态和轮力分配。实际测试中预测时域N10、步长dt0.02s时最远预瞄距离为前方0.5米左右对30cm高的台阶、15°斜坡能提前响应。地形突变时要注意摩擦锥的方向也要跟着旋转——接触面法向不再是竖直方向。这个修正体现在约束矩阵A_lin中让切向力约束沿着路面切平面重新投影。如果省略这一步在斜坡上MPC计算出的“竖直方向反作用力”会被地面分解导致实际法向力过小、侧滑失稳。4. 从任务空间到关节力矩雅可比矩阵变换与逆动力学解算4.1 力雅可比转置地面反作用力怎么变成关节力矩MPC给出的最优解是质心系下每个轮子处的地面反作用力向量f_i。要把这个力“分配”到各条腿的关节核心关系是力雅可比转置tau_leg J_leg * f_i这里的J_leg是腿的雅可比矩阵中线性速度对应的前3行维度3×2髋关节俯仰、膝关节俯仰两个主动关节偏航关节若忽略。轮子处的力通过腿的连杆结构转化为髋关节和膝关节的力矩。实际操作中必须考虑雅可比矩阵的奇异位形。当膝关节完全伸直时力雅可比矩阵条件数趋近无穷大地面反作用力映射到关节力矩会出现极端放大。仿真中的约束手段有两个一是MPC约束里限制质心高度让腿永远保持弯曲状态参考质心高度设为腿长总长的85%二是对tau_leg做饱和限幅超过关节极限就截断。更精细的做法是在MPC里加入关节极限的线性近似约束用当前关节角处的雅可比矩阵线性外推力矩范围% 关节力矩极限约束J * f tau_max J_lin geometricJacobian(rbt, q_current, wheel_front_left); A_tau J_lin(1:3, :); % 转置后作为约束矩阵 l_tau -tau_max * ones(3,1); u_tau tau_max * ones(3,1);注意这个约束是基于当前时刻的雅可比预测时域内腿的构型会变严格来说力矩约束的系数矩阵也随时间变化。好在轮腿机器人在“轮式行驶”模式下腿部构型变化不大固定雅可比近似在仿真中足够。腿抬起越障时建议减少MPC预测时域并在高优先级约束里保证足端可达性。4.2 关节力矩计算的完整通道重力补偿与摩擦补偿MPC直接输出的关节力矩只是“净力矩”——让机身产生预期加速度的那部分。实际发送给仿真模型的关节力矩还要叠加重力项和摩擦项tau_cmd tau_mpc tau_gravity tau_friction tau_coriolis重力补偿在Simscape里可以直接用externalForce和inverseDynamics函数计算% 给定当前关节角和零速度提取重力力矩 tau_g inverseDynamics(rbt, q_current, zeros(8,1), zeros(8,1), ... Gravity, [0 0 -9.81]); % 最终的关节指令力矩 tau_cmd tau_mpc tau_g tau_friction;摩擦力矩在Simscape接触模块里已经包含了轮地接触摩擦但关节内部的库仑摩擦和粘滞摩擦不会自动计算需要加一个前馈补偿% 关节摩擦补偿根据实测得到的摩擦参数 v_joint joint_velocities; tau_friction sign(v_joint) .* tau_coulomb v_joint .* tau_viscous;补偿系数tau_coulomb和tau_viscous可以通过Simscape里给每个关节加恒定驱动力矩、测量稳态速度来标定。如果疏忽了这一步MPC优化出的力在关节层面会打折扣实际仿真中机身会出现明显的“低头”——重力补偿没做干净。4.3 仿真中的力矩执行为何要在信号路径上加滤波器MPC以20~50 Hz频率输出期望轮力直接乘上雅可比转置后得到的关节力矩天然带有阶梯状跳变。Simscape中的电机模型如果带宽较高会把这个阶跃转化为机械振动。所以在关节力矩进入Simscape之前需要加一个二阶低通滤波% 二阶低通截止频率50Hz阻尼比0.7 flt designfilt(lowpassiir, FilterOrder, 2, ... HalfPowerFrequency, 50, SampleRate, 1000); tau_filtered filtfilt(flt, tau_cmd);filtfilt做零相位滤波不引入相位延迟但会带来一个步长的计算延迟需要完整信号才能滤波。若要求实时输出就改用filter函数配一阶低通代价是相位滞后对稳定性的影响。仿真项目追求与实机行为一致的话推荐保留二阶IIR滤波加Group Delay补偿这个细节直接决定机身姿态震荡的大小。5. 仿真调参的三个技巧从模型验证到复杂地形适应性运动5.1 技巧一轮子与腿部混合模式的MPC权重切换轮式四足在“纯滚动”和“抬腿越障”两种模式下MPC权重矩阵需要动态切换。纯滚动时将Q矩阵中质心水平速度的权重提高同时限制地面反作用力在轮子接触平面内让系统更“顺从”地面。抬腿越障时期望质心高度轨迹出现明显抬升此时把Q_pz权重提高同时缩小预测时域以降低计算负担。仿真代码里可以写一个简单的状态机根据前方地形的粗糙度阈值比如连续三个预测点的高程差超过5cm触发模式切换。这个技巧能让同一组MPC参数同时适应平地高速滚动和爬坡、上台阶。5.2 技巧二雅可比矩阵病态检测代码前面提到膝关节接近伸直时雅可比矩阵会退化。这种退化不是一蹴而就的而是随着腿部伸展度逐渐发生。我习惯在每个控制周期里跑一段检测逻辑% 条件数检测条件数 20 时限制该腿最大承载力 cond_J cond(J_leg); if cond_J 20 f_max_scale min(1, 20 / cond_J); % 将地面反作用力上限按比例缩小后重新求解MPC end如果不做这个检测在某些高难度动作比如从台阶跳下落地缓冲中地面反作用力会瞬间放大到极限关节力矩计算饱和仿真模型直接飞掉。5.3 技巧三用Simscape的接触力传感器做闭环校准MPC算出的期望地面反作用力是否真正被地面接受取决于接触模型。仿真中可以直接在Simscape的接触力模块输出端接一个Buses信号实时对比期望力和实际力。两者的差值持续过大说明接触参数刚度、摩擦系数或者MPC预瞄的地形高度有偏差。校准流程我一般分三步前向运动学验证关节角→轮心位置 vs Simscape传感器读数、静平衡验证四轮着地时MPC输出的力应等于1/4机身重力、动态跟踪验证斜坡适应时法向力误差控制在10%以内。这套验证跑通后再进入复合地形场景——把斜坡、台阶、离散石块组合起来观察MPC给出的地面反作用力是否始终维持在摩擦锥内部最终以整个运动过程中的ZMP轨迹落在支撑多边形内作为通过标准。本文还有配套的精品资源点击获取 基于模型预测控制的轮式四足机器人Simscape动力学仿真
简介面向机器人工程与仿生技术学习者这是一份基于模型预测控制MPC算法的轮式四足机器人动态运动控制仿真项目。内容聚焦轮腿复合机器人在复杂地形下的适应性运动围绕地面反作用力优化、关节力矩计算、雅可比矩阵变换与正向运动学解算等核心问题展开能够帮助读者系统理解MPC在机器人运动控制中的落地方法。压缩包共10个文件以MATLAB脚本.m和Simulink多体动力学模型.slx为主另含markdown/txt说明文档与docx附赠资料包体仅170KB便于快速下载与运行调试。目前已有59人学习适合有初步机器人学基础、正在开展课程设计或毕业设计的工程师与学生参考。通过Simscape多体模型和控制器脚本使用者可复现仿真环境观察不同坡度与地形下的力矩变化和运动响应为后续算法优化提供直接的研究样本。1. 轮式四足机器人的动态运动控制为什么要选模型预测控制轮式四足机器人也叫轮腿复合机器人的难点在于它同时具备腿部机构的离散步态和轮式的连续滚动两种运动模式切换时系统动力学呈现明显的混合特征——四轮着地时是典型移动基座抬起腿越障时又退化成浮动基座。对这种系统做动态运动控制传统PID很难同时管住“身体姿态”和“轮地接触力”两个目标而模型预测控制MPC恰好能把地面反作用力作为显式决策变量写进优化问题在预测未来若干步的同时满足接触力锥形约束、关节极限约束。本文从Simscape多体建模出发走完正运动学解算、雅可比矩阵变换到关节力矩计算的全链路给出一个可以在仿真里跑通的完整方案。适合正在做轮腿机器人控制、想从纯运动学仿真升级到动力学控制的工程师。2. 用Simscape搭建轮腿机器人多体模型正运动学与雅可比矩阵的仿真映射2.1 从DH参数到Simscape刚体树正运动学解算的建模起点轮腿复合机器人的正运动学解算理论上可以从经典的DH参数表出发。比如每条腿设计为“髋关节偏航—髋关节俯仰—膝关节俯仰—轮毂电机”的4自由度结构轮子本身不参与姿态解算但在速度层要计入它的滚动自由度。DH参数表里最关键的是髋关节到轮心的齐次变换矩阵它决定了后续雅可比矩阵的形态。在Simscape Multibody里搭建时我一般不用手写变换矩阵而是直接把它做成rigidBodyTree对象再用getTransform()验证正运动学% 构建刚体树注意基座坐标系定义在机身质心 rbt rigidBodyTree(DataFormat, column, MaxNumBodies, 20); % ... 逐段添加刚体与关节略去重复代码 ... % 给定一组关节角计算左前腿末端轮心位姿 q [0.1; -0.4; 0.8; 0]; % 髋偏航、髋俯仰、膝俯仰、轮转角 T getTransform(rbt, q, wheel_front_left); pos tform2trvec(T); % 轮心在基座坐标系下的位置这里getTransform返回的是4×4齐次矩阵tform2trvec提取平移分量。需要注意Simscape里关节角度的正方向必须与DH定义一致否则后续雅可比矩阵的符号会反——这个问题在仿真里隐蔽性强建议每搭一条腿就用一组已知构型验证一次。Simscape与原生的rigidBodyTree有个明显的差异Simscape里刚体质量、惯性张量直接影响动力学仿真而rigidBodyTree只做运动学。二者必须建立对应关系我通常先在Simscape里完成质量属性设置再用exportrobot把刚体树导出为URDF网格化模型后回灌到Simscape中保证两套模型的对象一致。2.2 轮地接触建模地面反作用力的仿真物理基础轮式四足机器人在地面滚动时真正“撑住”机器人的是轮地接触力而MPC的输出恰恰是期望的地面反作用力——模型和控制器之间必须用一个可量化的接触模型衔接。Simscape里实现轮地接触力常见做法是用Spatial Contact Force模块设置接触刚度和阻尼系数。轮子我建议简化为圆柱体不要做带胎纹的精细几何否则仿真步长会被接触检测拖慢一个量级。法向力采用Hertz接触模型F_normal max(0, k * penetration^1.5 c * penetration_dot)切向力用库仑摩擦模型加Stribeck速度修正% 切向摩擦力计算伪代码在Simscape内部通过自定义力实现 v_tangent norm(v_t); % 接触点切向滑移速度 mu mu_k (mu_s - mu_k) * exp(-abs(v_tangent) / vs); F_tangent -mu * abs(F_normal) * v_tangent / (v_tangent eps);mu_s是静摩擦系数mu_k是动摩擦系数vs是Stribeck速度。这四个参数直接影响机器人在斜坡上的表现mu_k设得偏小MPC计算出的期望反作用力超出摩擦锥机器人就会在仿真里原地打滑mu_s设得偏大平地启动时会出现高频抖动。一个初始参考值是mu_s 0.8、mu_k 0.6、vs 0.1 m/s接触刚度设为1e5 N/m量级。2.3 从Simscape读取正运动学与雅可比仿真里的实际做法Simscape本身不直接提供雅可比矩阵但可以通过两种途径获得一是用Simscape的Transform Sensor测出各关节相对基座的位姿配合数值求导二是直接调用rigidBodyTree的geometricJacobian函数把关节角从Simscape传出来计算。工程上第二条路更稳代码很短% 假设joint_pos是从Simscape里读出的8×1关节角向量四腿各2个主动关节4个轮角 % 基座到轮心的雅可比输出6×N矩阵N为关节数 J_left_front geometricJacobian(rbt, joint_pos, wheel_front_left);这里geometricJacobian返回6行前3行是线速度映射后3行是角速度映射。轮腿机器人的特殊之处在于轮子的滚动自由度让雅可比矩阵出现了“欠驱动”行——即使所有关节锁死轮子滚动依然会让机身移动。因此算地面反作用力到关节力矩的映射时要单独考虑轮子接触点处的约束不能简单对全矩阵求伪逆。3. 模型预测控制器的核心设计预测模型、约束与滚动优化3.1 预测模型选择为什么用单刚体动力学而非全阶多体模型轮腿机器人的全阶动力学模型自由度非常多机身6自由度腿部关节12个直接拿来做MPC的预测模型优化求解时间完全跟不上20~50 Hz的控制频率。仿真项目里通常采用单刚体动力学Single Rigid Body DynamicsSRBD模型作为预测模型。SRBD模型的思路是忽略腿的质量把机器人等效为一个刚体质心和四条腿末端轮子接触点构成的空间力系。动力学方程分两部分——质心平移和姿态旋转% 质心平移方程 m * (p_ddot - g) sum(f_i) % 姿态方程 I * omega_dot omega × (I * omega) sum(r_i × f_i) % 其中 r_i 是接触点到质心的向量f_i 是地面反作用力变量的约定p是质心位置omega是机体角速度I是转动惯量张量在机体坐标系下f_i是第i个接触点的地面反作用力世界坐标系。把这个连续方程用前向欧拉离散步长取MPC的控制周期dt 0.02s预测时域N 10每个步长里的地面反作用力即为优化变量维度是3×4×10 120QP求解规模很小。离散化后任意时刻的质心位姿都表达成地面反作用力的线性组合这正是MPC能把动力学约束转化为线性等式的关键。代码里可以用稀疏矩阵预计算每一时刻的系数N 10; nf 12; % 4个接触点每个3维力 % 预计算从力向量到质心位移的递推矩阵 A_pred (3N × nf*N) % 第k步质心位置 p_k p_0 k*dt*v_0 dt^2/m * A_pred(:,:,k) * f_seq A_pred build_centroid_map(N, dt, mass);3.2 MPC优化目标与约束地面反作用力优化怎么实现MPC每一拍要解的优化问题目标函数有三个层次跟踪误差质心位置、速度、姿态、地面反作用力变化率保证力的平滑性、松弛变量防止约束过死导致无解。min sum_k ( x_k - x_ref_k ) Q ( x_k - x_ref_k ) sum_k ( f_k - f_prev ) R ( f_k - f_prev ) rho * epsilon^2 s.t. 动力学等式约束SRBD fz_i fz_min 接触点不能有拉力 |fx_i| mu * fz_i 摩擦锥约束 |fy_i| mu * fz_i |tau_j| tau_max 关节力矩极限约束 |epsilon| 上限Q矩阵的权重分配我实测下来的经验是质心高度误差权重调大Q_pz 200水平位置误差权重适中Q_px 100姿态角误差权重取Q_rpy 150。速度误差权重可以设小20~50因为速度跟踪主要靠反作用力大小控制权重过大会引发高频振荡。R矩阵控制力的平滑度设R 1e-4 * I。松弛变量权重rho设到1e6它只在任务本身超出物理可行性时激活正常运动应该让松弛变量恒为0。求解器方面仿真项目里直接用quadprog或OSQP都行。OSQP的优势是支持热启动——上一帧的最优解作为下一帧的初值迭代次数能减少40%以上。代码如下% 构建QP矩阵稀疏形式 prob optimproblem(Objective, objective, Constraints, cons); % 或者直接调 osqp 的 MATLAB 接口 solver osqp; solver.setup(P, q, A_lin, l, u, warm_starting, true); results solver.solve();注意每帧的参考轨迹可能变化所以q向量目标函数一次项要逐帧更新而P矩阵如果系统模型不变可以只在初始化时构建一次。3.3 复杂地形适应性运动MPC如何“看到”前方路面轮式四足机器人要适应复杂地形关键在于MPC预测时域内的接触点和接触法向必须随地形变化。仿真中最直接的实现方式预先把地形高度图或数值高程函数传给控制器每个预测步长内的接触点高度根据当前机身预测位置插值获得。% 预测第k步的接触点位置简化处理默认接触点在轮子正下方 contact_pos_x(:, k) p_x_pred(:, k) r_offset_x; contact_pos_z(:, k) terrain_height(contact_pos_x(:, k)); % 更新动力学方程中的 r_i质心到接触点的向量 r_i(:, :, k) contact_pos(:, k) - p_pred(:, k);这就实现了“预瞄”——当机器人前方有台阶时MPC提前几个步长感知到接触点抬高于是调整机身姿态和轮力分配。实际测试中预测时域N10、步长dt0.02s时最远预瞄距离为前方0.5米左右对30cm高的台阶、15°斜坡能提前响应。地形突变时要注意摩擦锥的方向也要跟着旋转——接触面法向不再是竖直方向。这个修正体现在约束矩阵A_lin中让切向力约束沿着路面切平面重新投影。如果省略这一步在斜坡上MPC计算出的“竖直方向反作用力”会被地面分解导致实际法向力过小、侧滑失稳。4. 从任务空间到关节力矩雅可比矩阵变换与逆动力学解算4.1 力雅可比转置地面反作用力怎么变成关节力矩MPC给出的最优解是质心系下每个轮子处的地面反作用力向量f_i。要把这个力“分配”到各条腿的关节核心关系是力雅可比转置tau_leg J_leg * f_i这里的J_leg是腿的雅可比矩阵中线性速度对应的前3行维度3×2髋关节俯仰、膝关节俯仰两个主动关节偏航关节若忽略。轮子处的力通过腿的连杆结构转化为髋关节和膝关节的力矩。实际操作中必须考虑雅可比矩阵的奇异位形。当膝关节完全伸直时力雅可比矩阵条件数趋近无穷大地面反作用力映射到关节力矩会出现极端放大。仿真中的约束手段有两个一是MPC约束里限制质心高度让腿永远保持弯曲状态参考质心高度设为腿长总长的85%二是对tau_leg做饱和限幅超过关节极限就截断。更精细的做法是在MPC里加入关节极限的线性近似约束用当前关节角处的雅可比矩阵线性外推力矩范围% 关节力矩极限约束J * f tau_max J_lin geometricJacobian(rbt, q_current, wheel_front_left); A_tau J_lin(1:3, :); % 转置后作为约束矩阵 l_tau -tau_max * ones(3,1); u_tau tau_max * ones(3,1);注意这个约束是基于当前时刻的雅可比预测时域内腿的构型会变严格来说力矩约束的系数矩阵也随时间变化。好在轮腿机器人在“轮式行驶”模式下腿部构型变化不大固定雅可比近似在仿真中足够。腿抬起越障时建议减少MPC预测时域并在高优先级约束里保证足端可达性。4.2 关节力矩计算的完整通道重力补偿与摩擦补偿MPC直接输出的关节力矩只是“净力矩”——让机身产生预期加速度的那部分。实际发送给仿真模型的关节力矩还要叠加重力项和摩擦项tau_cmd tau_mpc tau_gravity tau_friction tau_coriolis重力补偿在Simscape里可以直接用externalForce和inverseDynamics函数计算% 给定当前关节角和零速度提取重力力矩 tau_g inverseDynamics(rbt, q_current, zeros(8,1), zeros(8,1), ... Gravity, [0 0 -9.81]); % 最终的关节指令力矩 tau_cmd tau_mpc tau_g tau_friction;摩擦力矩在Simscape接触模块里已经包含了轮地接触摩擦但关节内部的库仑摩擦和粘滞摩擦不会自动计算需要加一个前馈补偿% 关节摩擦补偿根据实测得到的摩擦参数 v_joint joint_velocities; tau_friction sign(v_joint) .* tau_coulomb v_joint .* tau_viscous;补偿系数tau_coulomb和tau_viscous可以通过Simscape里给每个关节加恒定驱动力矩、测量稳态速度来标定。如果疏忽了这一步MPC优化出的力在关节层面会打折扣实际仿真中机身会出现明显的“低头”——重力补偿没做干净。4.3 仿真中的力矩执行为何要在信号路径上加滤波器MPC以20~50 Hz频率输出期望轮力直接乘上雅可比转置后得到的关节力矩天然带有阶梯状跳变。Simscape中的电机模型如果带宽较高会把这个阶跃转化为机械振动。所以在关节力矩进入Simscape之前需要加一个二阶低通滤波% 二阶低通截止频率50Hz阻尼比0.7 flt designfilt(lowpassiir, FilterOrder, 2, ... HalfPowerFrequency, 50, SampleRate, 1000); tau_filtered filtfilt(flt, tau_cmd);filtfilt做零相位滤波不引入相位延迟但会带来一个步长的计算延迟需要完整信号才能滤波。若要求实时输出就改用filter函数配一阶低通代价是相位滞后对稳定性的影响。仿真项目追求与实机行为一致的话推荐保留二阶IIR滤波加Group Delay补偿这个细节直接决定机身姿态震荡的大小。5. 仿真调参的三个技巧从模型验证到复杂地形适应性运动5.1 技巧一轮子与腿部混合模式的MPC权重切换轮式四足在“纯滚动”和“抬腿越障”两种模式下MPC权重矩阵需要动态切换。纯滚动时将Q矩阵中质心水平速度的权重提高同时限制地面反作用力在轮子接触平面内让系统更“顺从”地面。抬腿越障时期望质心高度轨迹出现明显抬升此时把Q_pz权重提高同时缩小预测时域以降低计算负担。仿真代码里可以写一个简单的状态机根据前方地形的粗糙度阈值比如连续三个预测点的高程差超过5cm触发模式切换。这个技巧能让同一组MPC参数同时适应平地高速滚动和爬坡、上台阶。5.2 技巧二雅可比矩阵病态检测代码前面提到膝关节接近伸直时雅可比矩阵会退化。这种退化不是一蹴而就的而是随着腿部伸展度逐渐发生。我习惯在每个控制周期里跑一段检测逻辑% 条件数检测条件数 20 时限制该腿最大承载力 cond_J cond(J_leg); if cond_J 20 f_max_scale min(1, 20 / cond_J); % 将地面反作用力上限按比例缩小后重新求解MPC end如果不做这个检测在某些高难度动作比如从台阶跳下落地缓冲中地面反作用力会瞬间放大到极限关节力矩计算饱和仿真模型直接飞掉。5.3 技巧三用Simscape的接触力传感器做闭环校准MPC算出的期望地面反作用力是否真正被地面接受取决于接触模型。仿真中可以直接在Simscape的接触力模块输出端接一个Buses信号实时对比期望力和实际力。两者的差值持续过大说明接触参数刚度、摩擦系数或者MPC预瞄的地形高度有偏差。校准流程我一般分三步前向运动学验证关节角→轮心位置 vs Simscape传感器读数、静平衡验证四轮着地时MPC输出的力应等于1/4机身重力、动态跟踪验证斜坡适应时法向力误差控制在10%以内。这套验证跑通后再进入复合地形场景——把斜坡、台阶、离散石块组合起来观察MPC给出的地面反作用力是否始终维持在摩擦锥内部最终以整个运动过程中的ZMP轨迹落在支撑多边形内作为通过标准。本文还有配套的精品资源点击获取