網(wǎng)站制作 網(wǎng)站建設(shè)百度推廣怎么優(yōu)化
腿足機器人之七- 逆運動學
- 基本概念
- 腿部運動的數(shù)學表示
- 坐標系定義以及自由度說明
- 正運動學模型
- 逆運動學求解
- 幾何解法
- 數(shù)值迭代法
- 雅可比矩陣法基礎(chǔ)
- 雙足機器人步態(tài)規(guī)劃中的雅可比法應(yīng)用
- 工程挑戰(zhàn)與解決方案
- 實際應(yīng)用中的工具和算法
- 多解問題
- 高自由度機器人(如Atlas的28自由度)
基本概念
機器人運動控制的本質(zhì)是將高層指令(“向前走”,“右側(cè)走移動”)轉(zhuǎn)化為關(guān)節(jié)電機的轉(zhuǎn)動角度,上一篇的正運動學是從關(guān)節(jié)角度推導(dǎo)出機器人的末端位置,而逆運動學(Inverse Kinematics,IK)是已知機器人末端(如足端)的位置和姿態(tài),反推各關(guān)節(jié)角度的過程。
腿部自由度越多,解算難度指數(shù)級上升(以人類腿部7自由度 vs 工業(yè)機械臂6自由度為例),步態(tài)規(guī)劃中需要考慮支撐相和擺動相的切換,以及如何協(xié)調(diào)雙腿的運動。
腿部運動的數(shù)學表示
這部分包括世界坐標系、髖關(guān)節(jié)坐標系以及足端坐標系,此外還涉及D-H參數(shù)以及從D-H參數(shù)到齊次變換矩陣,在上一篇博客《腿足機器人之六- 前向運動學》已經(jīng)介紹,這里簡略展示計算過程,不清楚可以參考上一篇博客。
坐標系定義以及自由度說明
雙足人形機器人髖關(guān)節(jié)的三個自由度對應(yīng)繞基座標系的三個軸的旋轉(zhuǎn):
- 偏航(Yaw):繞Z軸旋轉(zhuǎn),角度記為 ψ \psi ψ
- 俯仰(Pitch):繞Y軸旋轉(zhuǎn),角度記為 θ \theta θ
- 橫滾(Roll):繞X軸旋轉(zhuǎn),角度記為 ? \phi ?
基座標系(O?) 設(shè)在髖關(guān)節(jié)中心,X軸向前,Y軸向左,Z軸向上。旋轉(zhuǎn)順序為ZYX,對應(yīng)的旋轉(zhuǎn)矩陣為:
R h i p = R z ( ψ ) ? R y ( θ ) ? R x ( ? ) R_{hip}=R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi) Rhip?=Rz?(ψ)?Ry?(θ)?Rx?(?)
正運動學模型
假設(shè)大腿長度為 l 1 l_1 l1?,小腿長度為 l 2 l_2 l2?,膝關(guān)節(jié)俯仰角為 θ k \theta_k θk?,腳的位置 P P P在基座標系中的坐標為:
P = R h i p ? ( [ 0 , 0 , l 1 ] + R y ( θ k ) ? [ 0 , 0 , l 2 ] ) P=R_{hip}\cdot ([0,0,l_1]+R_y(\theta_k) \cdot[0,0,l_2]) P=Rhip??([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ψ0?001? ?
- 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θ?010?sinθ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?(?)= ?100?0cos?sin??0?sin?cos?? ?
上式本質(zhì)上是坐標系變換的級聯(lián)過程,包含如下關(guān)鍵步驟: - 1.大腿的固定長度:髖關(guān)節(jié)到膝關(guān)節(jié)的位移 [ 0 , 0 , l 1 ] [0,0,l_1] [0,0,l1?];
- 2.膝關(guān)節(jié)的旋轉(zhuǎn):繞膝關(guān)節(jié)局部Y軸的旋轉(zhuǎn) R y ( θ k ) R_y(\theta_k) Ry?(θk?),影響小腿的延伸方向
- 髖關(guān)節(jié)的整體旋轉(zhuǎn):將整個腿部(大腿+小腿)的姿態(tài)通過 R h i p R_{hip} Rhip?轉(zhuǎn)換到基座標系。
注:若踝關(guān)節(jié)存在,需在公式末尾添加踝關(guān)節(jié)的旋轉(zhuǎn)和平移,例如:
P = R h i p ? ( [ 0 , 0 , l 1 ] + R y ( θ k ) ? [ 0 , 0 , l 2 ] ) + R a n k l e ? △ p P=R_{hip}\cdot ([0,0,l_1]+R_y(\theta_k) \cdot[0,0,l_2]) +R_{ankle} \cdot \triangle p P=Rhip??([0,0,l1?]+Ry?(θk?)?[0,0,l2?])+Rankle??△p
逆運動學求解
腿部逆運動學主要有三大解法
方法 | 原理 | 優(yōu)點 | 缺點 |
---|---|---|---|
幾何解析法 | 利用三角函數(shù)分解幾何關(guān)系 | 計算速度快 | 僅適用于簡單結(jié)構(gòu),如平面式機器人 |
代數(shù)法 | 求解非線性方程組 | 精確解 | 可能無解或多解 |
數(shù)值迭代法 | 雅可比矩陣+梯度下降 | 通用性強 | 計算量大,需防發(fā)散 |
幾何解法
1.確定腳的目標位置和方向
設(shè)目標位置為 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.分解髖關(guān)節(jié)旋轉(zhuǎn)矩陣
根據(jù)正運動學,旋轉(zhuǎn)后的z軸應(yīng)對齊于 u u u,即:
R h i p ? [ 0 , 0 , 1 ] T = u R_{hip} \cdot [0,0,1]^T = u Rhip??[0,0,1]T=u
展開后得到三個方程:
{ 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??===?ux?uy?uz??
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??)
代入前兩個方程,結(jié)合幾何約束求解 θ \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.計算膝關(guān)節(jié)角度 θ k \theta_k θk?
根據(jù)余弦定理,解算膝關(guān)節(jié)角度:
θ 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}) θk?=arccos(2l1?l2?∣∣p∣∣2?l12??l22??)
注意事項
- 多解性:可能存在多組解,需根據(jù)關(guān)節(jié)限制選擇。
- 奇異點:當大腿與目標方向共線時,需特殊處理。
- 姿態(tài)調(diào)整:踝關(guān)節(jié)需進一步調(diào)整以滿足腳的朝向。
數(shù)值迭代法
雅可比矩陣在機器人學中用于描述末端執(zhí)行器速度與關(guān)節(jié)速度之間的關(guān)系。逆運動學問題中,雅可比矩陣的逆或偽逆被用來迭代調(diào)整關(guān)節(jié)角度,使末端執(zhí)行器逐步接近目標位置。對于雙足機器人來說,特別是在動態(tài)運動如小跑和走路時,雅可比方法能處理實時調(diào)整的需求,因為步態(tài)需要連續(xù)更新關(guān)節(jié)角度以保持平衡和步態(tài)周期。
步態(tài)規(guī)劃中需要考慮支撐相和擺動相的切換,以及如何協(xié)調(diào)雙腿的運動??赡苄枰Y(jié)合具體的數(shù)學模型,例如線性倒立擺模型(LIPM)或零力矩點(ZMP)準則來確保動態(tài)穩(wěn)定性。雅可比方法在這里可能用于實時調(diào)整關(guān)節(jié)角度以滿足ZMP的要求。
雅可比矩陣法基礎(chǔ)
1.雅可比矩定義
雅可比矩陣描述了機器人末端執(zhí)行器速度(線速度 p ˙ \dot {\boldsymbol p} p˙?和角速度 w \boldsymbol w w)與關(guān)節(jié)速度 q ˙ \dot {\boldsymbol q} q˙?之間的關(guān)系:
[ 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為關(guān)節(jié)數(shù))。
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} e=ptarget??pcurrent?,調(diào)整關(guān)節(jié)角度 q \boldsymbol q q:
△ q = J + ? e \triangle \boldsymbol q= \boldsymbol J^{+} \cdot \boldsymbol e △q=J+?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行滿秩時) J+=J?(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} J+=J?(JJ?+λ2I)?1
λ \lambda λ為代入阻尼因子,通常取0.001~0.1。
3.迭代步驟
a.初始化:設(shè)定初始關(guān)節(jié)角度 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) ei?=ptarget??fkin?(qi?),其中 f k i n f_{kin} fkin?為正運動學函數(shù)。
c.更新關(guān)節(jié)角:
q i + 1 = q i + α ? J + ( q i ) ? e i \boldsymbol q_{i+1}=\boldsymbol q_i + \alpha \cdot \boldsymbol J^{+}(\boldsymbol q_i) \cdot \boldsymbol e_i qi+1?=qi?+α?J+(qi?)?ei?
d.終止條件:當 ∣ ∣ e i ∣ ∣ < ? ||e_i|| < \epsilon ∣∣ei?∣∣<?或達到最大迭代次數(shù)時停止。
雙足機器人步態(tài)規(guī)劃中的雅可比法應(yīng)用
對于多足機器人,涉及到多個腿的并行運動,需要對步態(tài)進行規(guī)劃,基于周期性步態(tài)(如四足機器人的對角步態(tài))或非周期性步態(tài)(如動態(tài)奔跑),結(jié)合時間參數(shù)生成關(guān)節(jié)軌跡,常用方法包括零力矩點(ZMP)理論:用于雙足機器人,確保質(zhì)心投影在支撐多邊形內(nèi)和貝塞爾曲線或樣條插值:生成平滑的足端軌跡。
- 步態(tài)特征
- 步行(Walking):交替支撐相(單腿支撐)和擺動相,重心軌跡平滑,ZMP(零力矩點)始終在支撐多邊形內(nèi)。
- 小跑(Trot):對角腿同步運動,動態(tài)平衡需求更高,存在雙支撐相和騰空相。
- 單腿逆運動學模型
以右腿為例,關(guān)節(jié)配置為髖關(guān)節(jié)(橫滾 ? \phi ?、俯仰 θ \theta θ、偏航 ψ \psi ψ)和膝關(guān)節(jié) θ k \theta_k θk?,踝關(guān)節(jié) θ a \theta_a θa?。
構(gòu)造雅可比矩陣:
對于腿部位置 p = [ x , y , z ] T \boldsymbol p=[x,y,z]^T p=[x,y,z]T,雅可比矩陣的列為各關(guān)節(jié)對末端速度的貢獻:
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?? ?
偏導(dǎo)數(shù)計算示例(髖關(guān)節(jié)俯仰角 θ \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} ?θ?p?=Rz?(ψ)?(?θ?Ry?(θ)?)?Rx?(?)?plink?
- 步行步態(tài)求解
支撐相(以右腿支撐為例):
- 目標:調(diào)整左腿擺動軌跡,同時保持軀干平衡。
- 軀干控制:通過髖關(guān)節(jié)角度補償重心偏移,雅可比矩陣需包含軀干姿態(tài)的影響。
- 迭代過程:
- 規(guī)劃左腳擺動軌跡 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) e=pfoot?(t)?pcurrent?(t)
- 用雅可比逆矩陣更新左腿關(guān)節(jié)角 △ q = J + ? e \triangle q=\boldsymbol J^+ \cdot \boldsymbol e △q=J+?e
- 同步調(diào)整右腿髖關(guān)節(jié)橫滾 ? \phi ?以維持 ZMP 在右腳支撐區(qū)內(nèi)
擺動相軌跡規(guī)劃:
常用三次多項式插值,設(shè)擺動時間 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.小跑步態(tài)求解
動態(tài)特性:雙足騰空相存在,需控制落地沖擊。
- 騰空相:腿部收縮以減少轉(zhuǎn)動慣量。
- 支撐相:對角腿(如右腿和左腿)同步支撐,雅可比矩陣需同時處理雙腿的耦合運動。
協(xié)調(diào)控制步驟:
- 規(guī)劃軀干質(zhì)心(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。
- 對每條腿獨立應(yīng)用雅可比逆矩陣法,但需引入耦合項確保軀干平衡:
△ 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}) △qleft?=Jleft+??eleft?+K?(qright??qnominal?)
K為細條增益矩陣。
5.關(guān)鍵問題與優(yōu)化
- 奇異性處理,當腿完全伸展 θ k = 0 \theta_k =0 θk?=0時雅可比矩陣秩虧,需增加阻尼項或約束關(guān)節(jié)速度。
- 多目標優(yōu)化
同時優(yōu)化腳部位置、軀干姿態(tài)和關(guān)節(jié)限制:
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 - 實時性保障預(yù)計算常見步態(tài)的雅可比矩陣,或使用高效數(shù)值庫(如 Eigen, ROS可調(diào)用eigen)進行偽逆計算。
工程挑戰(zhàn)與解決方案
實際應(yīng)用中的工具和算法
IKFast(OpenRAVE)
- 原理:自動生成解析解代碼,適用于固定結(jié)構(gòu)的機器人。
FABRIK(Forward and Backward Reaching IK)
- 特點:基于幾何的迭代算法,高效且無需矩陣運算。
ROS中的IK求解器(如TRAC-IK、KDL)
- 功能:提供數(shù)值解接口,支持關(guān)節(jié)限制和實時計算。
多解問題
選擇最優(yōu)解:根據(jù)關(guān)節(jié)限位、能耗或平滑性選擇解。
多解選擇策略
能量最優(yōu)原則:選擇關(guān)節(jié)移動總距離最小的解
避障約束:排除導(dǎo)致機械干涉的解
硬件限制:關(guān)節(jié)角度限位、速度飽和(示例:舵機最大轉(zhuǎn)角180°)
高自由度機器人(如Atlas的28自由度)
雅可比典型問題與解決方案
奇異位形崩潰:當腿部完全伸直時雅可比矩陣不可逆
解決方案:在軌跡規(guī)劃階段避開危險區(qū)域
動態(tài)擾動影響:奔跑時地面反作用力導(dǎo)致解算失效
應(yīng)對措施:結(jié)合IMU數(shù)據(jù)實時修正期望位姿