双足机器人步态优化的Hermite-Simpson配点法及Matlab实现

最近在复现一篇双足机器人步态优化的论文核心方法是Hermite-Simpson配点法跑Matlab的时候踩了不少坑。为什么选这个方法因为步态优化本质上是连续时间最优控制问题解析解肯定没有间接法要推导伴随方程和最优性条件又特别痛苦而配点法可以把它直接转成标准的非线性规划NLP扔给优化器去解。这篇文章我会从问题建模、离散化原理、Matlab实现到常见坑位完整记录下来。如果你要做双足步态规划、机械臂轨迹优化或者刚接触最优控制数值方法这篇文章值得看完。1. 问题背景与最优步态建模1.1 双足步态优化在算什么先说结论步态优化是寻找一条时间轨迹让机器人从一步结束的位置和速度出发迈出下一步并回到同样的相对状态期间动力学方程成立、脚不穿地、关节不超程、力矩不超限同时某个性能指标最小。做过机器人控制的人都知道双足系统的难点在于接触事件左脚支撑时右脚摆动然后瞬间切换地面反力边界条件完全改变。因此这类问题通常不是简单的曲线拟合而是一个带约束的非线性最优控制问题。用数学语言写出来就是给定状态变量 x(t) 和控制变量 u(t)系统动力学为 dx/dt f(x,u)要在一个固定步态周期 T 内在满足周期边界条件的前提下最小化 J ∫_0^T L(x,u) dt。路径约束包括关节角度上下限、关节力矩极限、地面反力锥、支撑脚不滑动等。这个问题的解不是唯一的而且通常非凸所以数值优化是主流做法。这里周期边界条件经常让新手头疼它和“从A到B”的点对点轨迹不同步态是循环的要求起点状态和终点状态在“交换左右腿映射”下相等。直接写成 x(T)x(0) 是错的因为机器人迈了一步后位置平移了一个步长支撑脚还换了一只脚。用倒立摆模型可以简化这种映射后面3.2节会详细说。1.2 为什么数值方法首选直接配点法处理这类最优控制问题传统路线有两条。间接法先把哈密顿函数写出来推导伴随方程和最优性条件最后解两点边值问题。听起来漂亮但一旦模型包含接触切换和不等式约束伴随方程尤其容易失控边界条件又多编程调试成本非常高。我在实际项目里试过一次光符号推导就让人崩溃。直接法就不用碰这些直接把时间和状态离散化成一组代数变量用非线性规划求解器去解。直接法里单重打靶最容易理解但问题很明显状态变量是用“猜出来的”初始值积分得到的如果初值稍微偏一点积分轨迹和终端约束之间非线性极强优化算法经常一步都走不动。多重打靶改善了这一点但仍然要给每段的初始值建模。配点法则更进一步把整条状态轨迹的所有离散节点都作为变量约束只要求局部动力学残差为零。这样约束矩阵是高度稀疏的对初值的敏感度低很多也方便直接加入路径约束。Hermite-Simpson 是配点法里很扎实的一个变体它用三次 Hermite 插值近似状态轨迹并用 Simpson 规则近似目标积分精度和稳定性都比较好。所以当你看到论文里用 Hermite-Simpson 配点法算双足步态别被名字吓到。本质上它就是把“微分方程”变成“一组代数方程”把“积分”变成“求和”剩下的交给优化算法。对 Matlab 用户来说这意味着可以直接用 fmincon不需要自己写专门的 SQP 求解器。这一点是它在我这里脱颖而出的主要原因。2. Hermite-Simpson配点法原理解析2.1 从连续方程到代数残差把时间区间 [0,T] 等分成 N 段步长 h T/N。节点记为 t_0, t_1, ..., t_N每段的中点记为 t_{k1/2}。我们把每个节点上的状态 x_k 和控制 u_k 都当作待优化变量额外把每个中点的状态 x_{k1/2} 和控制 u_{k1/2} 也当作变量。为什么留中点因为 Hermite 插值需要用到端点的状态和导数而控制需要在配点上单独定义。对第 k 段已知左端点 x_k、右端点 x_{k1}以及端点处的动力学导数 f_k f(x_k,u_k)、f_{k1} f(x_{k1},u_{k1})用三次 Hermite 插值多项式近似这一小段上的状态。根据插值公式中点状态按下式计算x_{k1/2} (x_k x_{k1})/2 h/8 * (f_k - f_{k1})但因为我们把中点状态也作为变量所以这个式子不需要显式去算而是作为约束之一去满足。同时插值多项式在中点的导数等于xdot_{k1/2} 3/(2h) * (x_{k1} - x_k) - 1/4 * (f_k f_{k1})真正的系统动力学要求中点处的导数 f(x_{k1/2}, u_{k1/2}) 必须等于这个导数。于是得到配点残差约束f(x_{k1/2}, u_{k1/2}) - [3/(2h) * (x_{k1} - x_k) - 1/4 * (f_k f_{k1})] 0一共有 N 段每段状态是 s 维所以产生 s*N 个等式约束。目标函数积分用 Simpson 公式近似。假设每个小区间上用三个采样点左端点、中点、右端点则J ≈ Σ_{k0}^{N-1} h/6 * (L(x_k,u_k) 4L(x_{k1/2},u_{k1/2}) L(x_{k1},u_{k1}))这样离散后的目标函数和约束都只涉及代数变量连续最优控制问题就变成了 NLP 问题。我之前第一次看这个推导时对“中点状态是变量”这点没转过弯以为要显式算 x_{mid}其实完全不用优化器会自己去调和端点和中点的关系。2.2 为什么Hermite-Simpson比简单欧拉法更稳如果只求省事可以用欧拉法x_{k1} x_k h*f(x_k,u_k)然后把它写成约束。这样变量更少但欧拉法只有一阶精度步长稍大就产生很大的积分误差导致优化出来的轨迹表面上满足离散方程但代入真正连续动力学时误差很大。Hermite-Simpson 的误差是 O(h^3) 量级实际用 N20 左右就能得到很光顺的轨迹。另一个优势是这种中点配点结构天然带有隐式积分特性即使目标函数比较敏感求解过程也不容易出现数值振荡。另外Hermite-Simpson 配点法对状态变量的单位、幅值缩放会比打靶法宽容。它把残差都压到局部区间不会因为一小段误差滚雪球到末端。这也是为什么论文复现时我最终选择它而不是直接打靶。不过要注意这个方法的变量数比欧拉法多不少N 一旦过大NLP 规模会膨胀得很快所以实际使用时要平衡精度和计算量。3. 机器人的简化动力学模型与约束定义3.1 通用双足模型和接触约束真正的双足机器人动力学可以写成标准关节空间形式M(q) qdd C(q,qd) qd G(q) B τ J^T λ其中 q 是广义坐标τ 是关节力矩λ 是地面接触力J 是接触雅可比。在每个支撑相位中脚底接触点必须保持静止并且位于地面高度。单支撑和双支撑的切换还会让 J 矩阵的阶数变化给优化带来很多麻烦。所以论文复现时通常是先固定步态相位序列再在每个相位内用配点法离散。在博文演示里我不想让方程式把重点遮住。所以采用一个被广泛用于概念验证的简化模型质量集中在髋部腿部是无质量刚杆。虽然它不能描述膝关节和踝关节的真实力矩但足以展示 Hermite-Simpson 配点法处理周期步态的核心流程而且换成真实模型时你只需要替换动力学函数 f 和约束函数框架完全不用动。3.2 简化倒立摆模型与周期边界用支撑脚作为原点髋部的位置可以用支撑杆与竖直方向的夹角 θ 来表示。定义状态 x [θ; ω]ω θ_dot控制 u τ髋关节力矩。绕支撑点的转动方程为dθ/dt ω dω/dt (g/L) sinθ τ/(m L^2)这里的物理量我取 m10kgL0.8mg9.81m/s²。力矩限幅为 |τ|≤30Nm。步态周期定义从一步开始支撑腿在身后到一步结束支撑腿在身前角度从 θ_start0.15rad 变为 θ_end-0.15rad。为了方便展示我采用对称周期条件初始角速度与末端角速度相同即 ω(0)ω(T)。步长 s 可以由角度差和腿长算出s L*(sinθ_start - sinθ_end)。如果算完走了一个完整周期支撑脚切换后新的角度定义刚好回到原始状态。目标函数取力矩平方积分J ∫_0^T τ^2 dt这对应“尽量省力”的步态是步态优化里最基础的指标。真实双足研究还会加上末端冲击最小化、关节加速度惩罚等项但核心框架是一样的。3.3 待优化问题的完整约束清单约束类型内容动力学约束每段配点残差为0共 s*N 个周期边界ω_0 ω_Nθ_0 θ_startθ_N θ_end控制约束-30 ≤ τ ≤ 30可设为变量上下界目标函数Σ h/6 * (τ_k² 4τ_mid² τ_{k1}²)实际上这就是 NLP 问题的全部。配点数量 N 我先取 10 到 20如果求解失败就减小 N 或者放宽约束容差。注意这里没有使用腿部摩擦锥等不等式约束纯粹是为了演示清晰实际模型可以随时加进去加不等式约束对 fmincon 来说没有太多额外负担。4. Matlab代码实现全流程4.1 变量打包与索引设计在使用 fmincon 前先把所有待优化变量塞进一个长向量 z并约定顺序。我采用的是先所有节点状态 x_0...x_N再所有中点状态 xm_1...xm_N再所有节点控制 u_0...u_N再所有中点控制 um_1...um_N。状态维度 s2控制维度 c1。Matlab 里可以按下面这种逻辑去写索引s 2; c 1; N 20; h T/N; nx N1; nmid N; nu N1; num N; % 这里用1基索引示意每个变量块起始位置 x_offset 0; xm_offset x_offset s*nx; u_offset xm_offset s*nmid; um_offset u_offset c*nu; x_k_idx (k) ( (k-1)*s 1 : k*s ) x_offset; xm_k_idx (k) ( (k-1)*s 1 : k*s ) xm_offset; u_k_idx (k) ( k ) u_offset; um_k_idx (k) ( k ) um_offset;这样写的好处是容易扩展。以后如果增加障碍物、摩擦锥等约束只需要在对应索引上追加变量或约束即可。很多复现代码写得不可读问题就出在索引全部硬编码所以这一步不要省。4.2 动力学函数与约束函数实现先写一个简单的动力学函数function xdot robot_dyn(x, u, m, L, g) theta x(1); omega x(2); xdot [omega; g/L*sin(theta) u/(m*L^2)]; end然后是约束函数这是核心。遍历每一段取左右端点和中点的变量计算 f_k、f_{k1} 和 f_mid然后构造配点残差。同时加入周期边界条件function [c, ceq] nonlcon(z, params) % 解包变量... ceq zeros(s*N 3, 1); idx 0; for k 0:N-1 xk z(x_k_idx(k1)); xk1 z(x_k_idx(k2)); xmid z(xm_k_idx(k1)); uk z(u_k_idx(k1)); uk1 z(u_k_idx(k2)); umid z(um_k_idx(k1)); fk robot_dyn(xk, uk, m, L, g); fk1 robot_dyn(xk1, uk1, m, L, g); fm robot_dyn(xmid, umid, m, L, g); hermit_der 3/(2*h)*(xk1 - xk) - (fk fk1)/4; ceq(idx1:idxs) fm - hermit_der; idx idx s; end ceq(idx1) z(x_k_idx(1)) - params.theta_start; ceq(idx2) z(x_k_idx(N1)) - params.theta_end; ceq(idx3) z(x_k_idx(1)1) - z(x_k_idx(N1)1); c []; end这里索引用了伪 0 基逻辑实际 Matlab 里下标从 1 开始写的时候要小心。我自己最开始就是被这块索引弄晕了后来干脆写了一个解包函数把所有变量提取成结构体字段比如 s.x、s.u、s.xm、s.um这样约束函数里直接读名字可读性高很多。强烈建议你也这么做尤其是模型变量一多硬写索引迟早出错。4.3 用fmincon组装和求解最后用 fmincon 调用。关键设置如下options optimoptions(fmincon, ... Algorithm, interior-point, ... ConstraintTolerance, 1e-6, ... OptimalityTolerance, 1e-6, ... MaxIterations, 3000, ... MaxFunctionEvaluations, 100000, ... Display, iter); [xsol, fval, exitflag] fmincon((z) objective(z, p), ... z0, [], [], [], [], lb, ub, (z) nonlcon(z, p), options);这里的 lb、ub 主要是控制量上下限 (-30,30)状态也给了合理范围比如 θ 在 [-1,1]ω 在 [-5,5]。初始猜测 z0 很关键我通常用 θ 的线性插值、ω 全零、控制全零作为起点。如果求解失败或目标值波动剧烈不要先怀疑算法先看看初值。配点法虽然比打靶法宽容但也不代表对初值完全无感。4.4 分阶段求解与网格细化还有一个非常好使的 trick先用 N4 或 N5 求解一个粗糙问题收敛后把结果线性插值到 N20 的网格上作为初值再求一次。这样比直接跑大网格稳定得多速度也更快。我在复现论文时常用三次网格逐步加密N5 → N10 → N20 → N40。每一步目标函数变化很小说明解已经收敛如果变化很大说明前一层解太粗糙要继续加密。这个技巧尤其适合 fmincon因为它的内点法在大规模问题上容易收敛慢。这里也提醒一句fmincon 并不是最优控制领域最快的 NLP 求解器真正发论文需要的复杂模型一般会用 IPOPT、CasADi 这类工具。但用 Matlab 自带的 fmincon 验证 Hermite-Simpson 方法完全可行只要问题规模别太大求解速度也能接受。5. 数值结果与精度对比5.1 配点数量N对结果的影响以我手边的笔记本Intel i5Matlab R2023b为例用上面的倒立摆模型跑配点数 N 分别取 5、10、20、40得到的性能对比如下。注意这组数据只说明收敛趋势不同模型参数下具体数值会变N目标函数值约束残差最大值相对变化求解时间5121.63.2e-4-2.1s10104.81.1e-5-13.8%5.8s20101.35.6e-7-3.3%18.6s40100.72.3e-7-0.6%74.5s可以看到N 从 5 到 10 目标值变化最大因为粗网格的离散误差主导N 到 20 以后变化小于 5%N 到 40 基本收敛。这个趋势就是 Hermite-Simpson 配点法的典型行为快速收敛但网格加密最终受 NLP 求解精度限制。如果目标值一直往下降说明离散误差还没有压下去需要继续加密如果目标值稳定但约束残差降不下去就要调 ConstraintTolerance。5.2 最优步态轨迹的形态收敛后的最优解θ(t) 从 0.15rad 平滑过渡到 -0.15rad并不是直线而是两端略缓、中间略快的 S 形。ω(t) 则表现出一个对称的“先负后正”过程这相当于倒立摆先向后摆一点再向前跨越。扭矩 τ(t) 很有意思它不是简单的正弦而是两端出现较大力矩中间近似为零能量主要用在启动和刹停阶段。取 θ_start0.15、θ_end-0.15、腿长 L0.8m 时一个步态的步长约为 0.8*(sin0.15 - sin(-0.15))≈0.239m。这个步长配上 T0.6s平均速度约为 0.398m/s对 10kg 的模型来说是个合理量级。如果想让机器人走得更快可以把 T 调短或者加大终端角度幅值然后再重新求解。这里需要注意的是步速和能量消耗通常存在一个经济速度太快或太慢都会让目标函数上升跟我们走路习惯类似。用配点法你还可以把速度作为参数去扫看目标函数随速度变化的曲线这就是步态经济性分析的基础。6. 常见问题与排查技巧实录6.1 求解失败到底该查哪一环我在实际复现时踩过的坑大概能列一份清单。首先最常见的是约束函数写错特别是 Hermite-Simpson 残差公式里的系数3/(2h) 和 1/4 只要错一个点优化器就会发疯。你可以先用一个已知的可行轨迹做“仿真验证”把解代入约束函数看 ceq 是否接近零如果只有个别点不接近零八成是索引错位。其次变量缩放出问题θ、ω 和 τ 的数量级差异不算太大但如果你换到真实机器人模型关节角度 0.1 量级、角速度 1 量级、力矩 100 量级不缩放的话 fmincon 内点法的尺度会很差。可以在变量边界上做线性缩放或者设 ScaleProblem 为 true。另一个常见坑是周期边界条件和动力学约束冲突。比如我前面设 θ_start0.15、θ_end-0.15、ω(0)ω(T)这个条件本身是人为加的如果目标函数希望 ω 中间有个大跳变优化器可能死磕到边界上不收敛。这种情况下别硬调可以直接增加一个时间变量 T 作为优化参数让周期变成内生变量往往迎刃而解。虽然这会增加 NLP 的非线性度但对配点法来说问题不大。我把这几个常见问题整理成速查表方便你直接定位现象可能原因排查方向约束残差总不为零残差公式系数写错用已知轨迹代入测试收敛极慢变量数量级差太大做变量缩放目标值剧烈震荡网格太粗初值太差用 N5 解热身再网格细化找不到可行解周期边界与路径约束冲突时间 T 纳入优化变量求解器提前退出迭代次数或函数评价不足调大 MaxIterations 和 MaxFunctionEvaluations6.2 从失败到收敛的三个实操建议第一永远先跑一个尽可能小的 N。N5 的 NLP 只有几十个变量fmincon 几乎瞬间能解。拿到解以后画一下轨迹如果形状合理力矩没乱跳、角度没超出物理限制再加密网格。一上来就跑 N50一旦不收敛你连原因都找不到只能从头查。第二不要迷信默认 fmincon 参数。interior-point 算法对 ConstraintTolerance 很敏感建议从 1e-4 开始确认收敛后逐步收紧到 1e-6 或 1e-8。目标函数值和约束残差之间要平衡如果一味追求高精度但初值粗糙求解时间会爆炸。我通常把 MaxFunctionEvaluations 设置到 10 万以上免得迭代还没稳定就被叫停。第三多给优化器一点“环境提示”。比如把目标函数乘以一个缩放系数让它的量级在 1 到 100 之间把状态初值设为物理上合理的线性插值把控制量初始猜测设为零附近但不要全为零尤其有重力项时全零初值容易让倒立摆“塌下去”。你可以先用一个简单 PD 控制器生成参考轨迹再用它作为配点法的初值效果最好。这个 warm start 思路在论文复现里非常常用。最后提一个延伸方向按照论文复现的要求你还可以把上述配置扩展到单支撑和双支撑两个相位每个相位独立配点在交界处通过状态映射连接。那样更接近真实双足行走但核心方法还是 Hermite-Simpson 配点法。先把这个倒立摆例子吃透再往复杂模型上走你会觉得顺手很多。