青岛网站排名外包,企业logo设计软件,wordpress x 主题,网站开发用什么系统比较好腿足机器人之七- 逆运动学 基本概念腿部运动的数学表示坐标系定义以及自由度说明正运动学模型 逆运动学求解几何解法数值迭代法雅可比矩阵法基础双足机器人步态规划中的雅可比法应用 工程挑战与解决方案实际应用中的工具和算法多解问题高自由度机器人#xff08;如Atlas的28自… 腿足机器人之七- 逆运动学 基本概念腿部运动的数学表示坐标系定义以及自由度说明正运动学模型 逆运动学求解几何解法数值迭代法雅可比矩阵法基础双足机器人步态规划中的雅可比法应用 工程挑战与解决方案实际应用中的工具和算法多解问题高自由度机器人如Atlas的28自由度 基本概念
机器人运动控制的本质是将高层指令“向前走”“右侧走移动”转化为关节电机的转动角度上一篇的正运动学是从关节角度推导出机器人的末端位置而逆运动学Inverse KinematicsIK是已知机器人末端如足端的位置和姿态反推各关节角度的过程。 腿部自由度越多解算难度指数级上升以人类腿部7自由度 vs 工业机械臂6自由度为例步态规划中需要考虑支撑相和摆动相的切换以及如何协调双腿的运动。
腿部运动的数学表示
这部分包括世界坐标系、髋关节坐标系以及足端坐标系此外还涉及D-H参数以及从D-H参数到齐次变换矩阵在上一篇博客《腿足机器人之六- 前向运动学》已经介绍这里简略展示计算过程不清楚可以参考上一篇博客。
坐标系定义以及自由度说明
双足人形机器人髋关节的三个自由度对应绕基座标系的三个轴的旋转
偏航Yaw绕Z轴旋转角度记为 ψ \psi ψ俯仰Pitch绕Y轴旋转角度记为 θ \theta θ横滚Roll绕X轴旋转角度记为 ϕ \phi ϕ
基座标系O₀ 设在髋关节中心X轴向前Y轴向左Z轴向上。旋转顺序为ZYX对应的旋转矩阵为 R h i p R z ( ψ ) ⋅ R y ( θ ) ⋅ R x ( ϕ ) R_{hip}R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi) RhipRz(ψ)⋅Ry(θ)⋅Rx(ϕ)
正运动学模型
假设大腿长度为 l 1 l_1 l1小腿长度为 l 2 l_2 l2膝关节俯仰角为 θ k \theta_k θk脚的位置 P P P在基座标系中的坐标为 P R h i p ⋅ ( [ 0 , 0 , l 1 ] R y ( θ k ) ⋅ [ 0 , 0 , l 2 ] ) PR_{hip}\cdot ([0,0,l_1]R_y(\theta_k) \cdot[0,0,l_2]) PRhip⋅([0,0,l1]Ry(θk)⋅[0,0,l2]) 其中 R z ( ψ ) [ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] R_z(\psi)\begin{bmatrix} \cos \psi -\sin \psi 0 \\ \sin \psi \cos \psi 0 \\ 0 0 1 \end{bmatrix} Rz(ψ) cosψsinψ0−sinψcosψ0001 R y ( θ ) [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_y(\theta)\begin{bmatrix} \cos \theta 0 \sin \theta \\ 0 1 0 \\ -\sin \theta 0 \cos \theta \end{bmatrix} Ry(θ) cosθ0−sinθ010sinθ0cosθ R x ( ϕ ) [ 1 0 0 0 cos ϕ − sin ϕ 0 sin ϕ cos ϕ ] R_x(\phi)\begin{bmatrix} 1 0 0 \\ 0 \cos \phi -\sin \phi \\ 0 \sin \phi \cos \phi \end{bmatrix} Rx(ϕ) 1000cosϕsinϕ0−sinϕcosϕ 上式本质上是坐标系变换的级联过程包含如下关键步骤1.大腿的固定长度髋关节到膝关节的位移 [ 0 , 0 , l 1 ] [0,0,l_1] [0,0,l1];2.膝关节的旋转绕膝关节局部Y轴的旋转 R y ( θ k ) R_y(\theta_k) Ry(θk)影响小腿的延伸方向髋关节的整体旋转将整个腿部大腿小腿的姿态通过 R h i p R_{hip} Rhip转换到基座标系。
注若踝关节存在需在公式末尾添加踝关节的旋转和平移例如 P R h i p ⋅ ( [ 0 , 0 , l 1 ] R y ( θ k ) ⋅ [ 0 , 0 , l 2 ] ) R a n k l e ⋅ △ p PR_{hip}\cdot ([0,0,l_1]R_y(\theta_k) \cdot[0,0,l_2]) R_{ankle} \cdot \triangle p PRhip⋅([0,0,l1]Ry(θk)⋅[0,0,l2])Rankle⋅△p
逆运动学求解
腿部逆运动学主要有三大解法
方法原理优点缺点几何解析法利用三角函数分解几何关系计算速度快仅适用于简单结构如平面式机器人代数法求解非线性方程组精确解可能无解或多解数值迭代法雅可比矩阵梯度下降通用性强计算量大需防发散
几何解法
1.确定脚的目标位置和方向 设目标位置为 P [ x , y , z ] P[x,y,z] P[x,y,z]其单位方向向量为 u P ∣ ∣ P ∣ ∣ [ u x , u y , u z ] u\frac{P}{||P||}[u_x, u_y, u_z] u∣∣P∣∣P[ux,uy,uz]
2.分解髋关节旋转矩阵 根据正运动学旋转后的z轴应对齐于 u u u即 R h i p ⋅ [ 0 , 0 , 1 ] T u R_{hip} \cdot [0,0,1]^T u Rhip⋅[0,0,1]Tu 展开后得到三个方程 { cos ψ sin θ cos ϕ sin ψ sin ϕ u x sin ψ sin θ cos ϕ − cos ψ sin ϕ u y cos θ cos ϕ u z \left\{ \begin{aligned} \cos \psi \sin \theta \cos \phi \sin \psi \sin \phi u_x \\ \sin \psi \sin \theta \cos \phi - \cos \psi \sin \phi u_y \\ \cos \theta \cos \phi u_z \end{aligned} \right. ⎩ ⎨ ⎧cosψsinθcosϕsinψsinϕsinψsinθcosϕ−cosψsinϕcosθcosϕuxuyuz
3.计算俯仰角 θ \theta θ和横滚脚 ϕ \phi ϕ 从第三个方程 cos θ cos ϕ u z ⇒ ϕ arccos ( u z cos θ ) \cos \theta \cos \phi u_z \Rightarrow \phi\arccos(\frac{u_z}{\cos \theta}) cosθcosϕuz⇒ϕarccos(cosθuz) 代入前两个方程结合几何约束求解 θ \theta θ和 ϕ \phi ϕ例如当 ϕ 0 \phi 0 ϕ0时简化为 θ arccos ( u z ) , ψ arctan 2 ( u y , x ) \theta \arccos(u_z), \psi\arctan2(u_y,_x) θarccos(uz),ψarctan2(uy,x)
4.计算膝关节角度 θ k \theta_k θk 根据余弦定理解算膝关节角度 θ k arccos ( ∣ ∣ p ∣ ∣ 2 − l 1 2 − l 2 2 2 l 1 l 2 ) \theta_k\arccos(\frac{||p||^2-l_1^2-l_2^2}{2l_1l_2}) θkarccos(2l1l2∣∣p∣∣2−l12−l22)
注意事项
多解性可能存在多组解需根据关节限制选择。奇异点当大腿与目标方向共线时需特殊处理。姿态调整踝关节需进一步调整以满足脚的朝向。
数值迭代法
雅可比矩阵在机器人学中用于描述末端执行器速度与关节速度之间的关系。逆运动学问题中雅可比矩阵的逆或伪逆被用来迭代调整关节角度使末端执行器逐步接近目标位置。对于双足机器人来说特别是在动态运动如小跑和走路时雅可比方法能处理实时调整的需求因为步态需要连续更新关节角度以保持平衡和步态周期。
步态规划中需要考虑支撑相和摆动相的切换以及如何协调双腿的运动。可能需要结合具体的数学模型例如线性倒立摆模型LIPM或零力矩点ZMP准则来确保动态稳定性。雅可比方法在这里可能用于实时调整关节角度以满足ZMP的要求。
雅可比矩阵法基础
1.雅可比矩定义 雅可比矩阵描述了机器人末端执行器速度线速度 p ˙ \dot {\boldsymbol p} p˙和角速度 w \boldsymbol w w与关节速度 q ˙ \dot {\boldsymbol q} q˙之间的关系 [ p ˙ w ] J ( q ) ⋅ q ˙ \begin{bmatrix} \dot {\boldsymbol p} \\ \boldsymbol w \end{bmatrix}\boldsymbol J(\boldsymbol q) \cdot \dot {\boldsymbol q} [p˙w]J(q)⋅q˙ 其中 J \boldsymbol J J是6 x n的雅可比矩阵n为关节数。
2.逆运动学迭代求解 目标是通过末端位置误差 e p t a r g e t − p c u r r e n t \boldsymbol e\boldsymbol p_{target}-\boldsymbol p_{current} eptarget−pcurrent调整关节角度 q \boldsymbol q q: △ q J ⋅ e \triangle \boldsymbol q \boldsymbol J^{} \cdot \boldsymbol e △qJ⋅e 其中 J \boldsymbol J^ J是雅可比矩阵的伪逆Moore-Penrose 逆计算为 J J ⊤ J J ⊤ − 1 ( 当 J 行满秩时 ) \boldsymbol J^\boldsymbol J^\top\boldsymbol J \boldsymbol J^\top^{-1} (当\boldsymbol J行满秩时) JJ⊤JJ⊤−1(当J行满秩时) 或使用阻尼最小二乘法避免奇异 J J ⊤ J J ⊤ λ 2 I − 1 \boldsymbol J^\boldsymbol J^\top\boldsymbol J \boldsymbol J^\top \lambda^2 \boldsymbol I^{-1} JJ⊤JJ⊤λ2I−1 λ \lambda λ为代入阻尼因子通常取0.001~0.1。
3.迭代步骤 a.初始化设定初始关节角度 q 0 \boldsymbol q_0 q0容差 ϵ \epsilon ϵ步长 α \alpha α。 b.计算误差 e i p t a r g e t − f k i n ( q i ) \boldsymbol e_i \boldsymbol p_{target} - f_{kin}(\boldsymbol q_i) eiptarget−fkin(qi)其中 f k i n f_{kin} fkin为正运动学函数。 c.更新关节角 q i 1 q i α ⋅ J ( q i ) ⋅ e i \boldsymbol q_{i1}\boldsymbol q_i \alpha \cdot \boldsymbol J^{}(\boldsymbol q_i) \cdot \boldsymbol e_i qi1qiα⋅J(qi)⋅ei d.终止条件当 ∣ ∣ e i ∣ ∣ ϵ ||e_i|| \epsilon ∣∣ei∣∣ϵ或达到最大迭代次数时停止。
双足机器人步态规划中的雅可比法应用
对于多足机器人涉及到多个腿的并行运动需要对步态进行规划基于周期性步态如四足机器人的对角步态或非周期性步态如动态奔跑结合时间参数生成关节轨迹常用方法包括零力矩点ZMP理论用于双足机器人确保质心投影在支撑多边形内和贝塞尔曲线或样条插值生成平滑的足端轨迹。
步态特征
步行Walking交替支撑相单腿支撑和摆动相重心轨迹平滑ZMP零力矩点始终在支撑多边形内。小跑Trot对角腿同步运动动态平衡需求更高存在双支撑相和腾空相。
单腿逆运动学模型 以右腿为例关节配置为髋关节横滚 ϕ \phi ϕ、俯仰 θ \theta θ、偏航 ψ \psi ψ和膝关节 θ k \theta_k θk踝关节 θ a \theta_a θa。 构造雅可比矩阵 对于腿部位置 p [ x , y , z ] T \boldsymbol p[x,y,z]^T p[x,y,z]T雅可比矩阵的列为各关节对末端速度的贡献 J [ ∂ x ∂ ϕ ∂ x ∂ θ ∂ x ∂ ψ ∂ x ∂ θ k ∂ y ∂ ϕ ∂ y ∂ θ ∂ y ∂ ψ ∂ y ∂ θ k ∂ z ∂ ϕ ∂ z ∂ θ ∂ z ∂ ψ ∂ z ∂ θ k ] \boldsymbol J \begin{bmatrix} \frac{\partial x}{\partial \phi} \frac{\partial x}{\partial \theta} \frac{\partial x}{\partial \psi} \frac{\partial x}{\partial \theta_k} \\ \frac{\partial y}{\partial \phi} \frac{\partial y}{\partial \theta} \frac{\partial y}{\partial \psi} \frac{\partial y}{\partial \theta_k} \\ \frac{\partial z}{\partial \phi} \frac{\partial z}{\partial \theta} \frac{\partial z}{\partial \psi} \frac{\partial z}{\partial \theta_k} \end{bmatrix} J ∂ϕ∂x∂ϕ∂y∂ϕ∂z∂θ∂x∂θ∂y∂θ∂z∂ψ∂x∂ψ∂y∂ψ∂z∂θk∂x∂θk∂y∂θk∂z
偏导数计算示例髋关节俯仰角 θ \theta θ ∂ p ∂ θ R z ( ψ ) ⋅ ( ∂ R y ( θ ) ∂ θ ) ⋅ R x ( ϕ ) ⋅ p l i n k \frac{\partial \boldsymbol p}{\partial \theta} R_z(\psi) \cdot(\frac{\partial \boldsymbol R_y(\theta)}{\partial \theta}) \cdot \boldsymbol R_x(\phi) \cdot \boldsymbol p_{link} ∂θ∂pRz(ψ)⋅(∂θ∂Ry(θ))⋅Rx(ϕ)⋅plink
步行步态求解 支撑相以右腿支撑为例
目标调整左腿摆动轨迹同时保持躯干平衡。躯干控制通过髋关节角度补偿重心偏移雅可比矩阵需包含躯干姿态的影响。迭代过程 规划左脚摆动轨迹 p f o o t ( t ) \boldsymbol p_{foot}(t) pfoot(t)计算当前左脚位置误差 e p f o o t ( t ) − p c u r r e n t ( t ) e\boldsymbol p_{foot}(t) - \boldsymbol p_{current}(t) epfoot(t)−pcurrent(t)用雅可比逆矩阵更新左腿关节角 △ q J ⋅ e \triangle q\boldsymbol J^ \cdot \boldsymbol e △qJ⋅e同步调整右腿髋关节横滚 ϕ \phi ϕ以维持 ZMP 在右脚支撑区内
摆动相轨迹规划 常用三次多项式插值设摆动时间 T T T起点 p 0 \boldsymbol p_0 p0终点 p f \boldsymbol p_f pf则 p ( t ) p 0 ( 3 ( t T ) 2 − 2 ( t T ) 3 ) ( p f − p 0 ) \boldsymbol p(t) \boldsymbol p_0 (3(\frac{t}{T})^2 - 2(\frac{t}{T})^3)(\boldsymbol p_f - \boldsymbol p_0) p(t)p0(3(Tt)2−2(Tt)3)(pf−p0)
4.小跑步态求解 动态特性双足腾空相存在需控制落地冲击。
腾空相腿部收缩以减少转动惯量。支撑相对角腿如右腿和左腿同步支撑雅可比矩阵需同时处理双腿的耦合运动。
协调控制步骤
规划躯干质心CoM轨迹使其符合倒立摆动力学。对角腿的脚部轨迹 p f o o t , l e f t ( t ) \boldsymbol p_{foot,left}(t) pfoot,left(t)和 p f o o t , r i g h t ( t ) \boldsymbol p_{foot,right}(t) pfoot,right(t)相位差 18 0 o 180^{o} 180o。对每条腿独立应用雅可比逆矩阵法但需引入耦合项确保躯干平衡 △ q l e f t J l e f t ⋅ e l e f t K ⋅ ( q r i g h t − q n o m i n a l ) \triangle \boldsymbol q_{left} \boldsymbol J_{left}^{} \cdot \boldsymbol e_{left} K \cdot (\boldsymbol q_{right}-\boldsymbol q_{nominal}) △qleftJleft⋅eleftK⋅(qright−qnominal)
K为细条增益矩阵。
5.关键问题与优化
奇异性处理当腿完全伸展 θ k 0 \theta_k 0 θk0时雅可比矩阵秩亏需增加阻尼项或约束关节速度。多目标优化 同时优化脚部位置、躯干姿态和关节限制 min △ q ( ∣ ∣ J △ q − e ∣ ∣ 2 λ ∣ ∣ △ q ∣ ∣ 2 ) \mathop{\min}_{\triangle q} (||\boldsymbol J \triangle \boldsymbol q - e||^2 \lambda|| \triangle \boldsymbol q||^2) min△q(∣∣J△q−e∣∣2λ∣∣△q∣∣2) 其解为 △ q ( J ⊤ J λ I ) − 1 J ⊤ e \triangle \boldsymbol q (\boldsymbol J^{\top} \boldsymbol J \lambda \boldsymbol I)^{-1}\boldsymbol J^{\top} \boldsymbol e △q(J⊤JλI)−1J⊤e实时性保障预计算常见步态的雅可比矩阵或使用高效数值库如 Eigen, ROS可调用eigen进行伪逆计算。
工程挑战与解决方案
实际应用中的工具和算法
IKFastOpenRAVE
原理自动生成解析解代码适用于固定结构的机器人。
FABRIKForward and Backward Reaching IK
特点基于几何的迭代算法高效且无需矩阵运算。
ROS中的IK求解器如TRAC-IK、KDL
功能提供数值解接口支持关节限制和实时计算。
多解问题
选择最优解根据关节限位、能耗或平滑性选择解。 多解选择策略 能量最优原则选择关节移动总距离最小的解 避障约束排除导致机械干涉的解 硬件限制关节角度限位、速度饱和示例舵机最大转角180°
高自由度机器人如Atlas的28自由度
雅可比典型问题与解决方案 奇异位形崩溃当腿部完全伸直时雅可比矩阵不可逆 解决方案在轨迹规划阶段避开危险区域 动态扰动影响奔跑时地面反作用力导致解算失效 应对措施结合IMU数据实时修正期望位姿
