基本概念

機器人運動控制的本質(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)，角度記為$?$

基座標系（O?） 設(shè)在髖關(guān)節(jié)中心，X軸向前，Y軸向左，Z軸向上。旋轉(zhuǎn)順序為ZYX，對應(yīng)的旋轉(zhuǎn)矩陣為：

$$R_{hip}=R_z(\psi )?R_y(\theta )?R_x(?)$$

正運動學模型

假設(shè)大腿長度為$l_1$，小腿長度為$l_2$，膝關(guān)節(jié)俯仰角為${\theta }_k$，腳的位置$P$在基座標系中的坐標為：
$$P=R_{hip}?([0,0,l_1]+R_y({\theta }_k)?[0,0,l_2])$$
其中：

• $R_z(\psi )=\left[\begin{array}{ccc}\cos \psi & ?\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$
• $R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ ?\sin \theta & 0& \cos \theta \end{array}\right]$
• $R_x(?)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos ?& ?\sin ?\\ 0& \sin ?& \cos ?\end{array}\right]$
  上式本質(zhì)上是坐標系變換的級聯(lián)過程，包含如下關(guān)鍵步驟：
• 1.大腿的固定長度：髖關(guān)節(jié)到膝關(guān)節(jié)的位移 $[0,0,l_1]$;
• 2.膝關(guān)節(jié)的旋轉(zhuǎn)：繞膝關(guān)節(jié)局部Y軸的旋轉(zhuǎn)$R_y({\theta }_k)$，影響小腿的延伸方向
• 髖關(guān)節(jié)的整體旋轉(zhuǎn)：將整個腿部（大腿+小腿）的姿態(tài)通過$R_{hip}$轉(zhuǎn)換到基座標系。

注：若踝關(guān)節(jié)存在，需在公式末尾添加踝關(guān)節(jié)的旋轉(zhuǎn)和平移，例如：
$$P=R_{hip}?([0,0,l_1]+R_y({\theta }_k)?[0,0,l_2])+R_{ankle}?△p$$

逆運動學求解

腿部逆運動學主要有三大解法

方法	原理	優(yōu)點	缺點
幾何解析法	利用三角函數(shù)分解幾何關(guān)系	計算速度快	僅適用于簡單結(jié)構(gòu)，如平面式機器人
代數(shù)法	求解非線性方程組	精確解	可能無解或多解
數(shù)值迭代法	雅可比矩陣+梯度下降	通用性強	計算量大，需防發(fā)散

幾何解法

1.確定腳的目標位置和方向
設(shè)目標位置為$P=[x,y,z]$，其單位方向向量為：
$$u=\frac{P}{||P||}=[u_x,u_y,u_z]$$

2.分解髖關(guān)節(jié)旋轉(zhuǎn)矩陣
根據(jù)正運動學，旋轉(zhuǎn)后的z軸應(yīng)對齊于$u$，即：
$$R_{hip}?[0,0,1{]}^T=u$$
展開后得到三個方程：
$$?\begin{array}{rlr}\cos \psi \sin \theta \cos ?+\sin \psi \sin ?& =& u_x\\ \sin \psi \sin \theta \cos ??\cos \psi \sin ?& =& u_y\\ \cos \theta \cos ?& =& u_z\end{array}$$

3.計算俯仰角$\theta$和橫滾腳$?$
從第三個方程：
$\cos \theta \cos ?=u_z??=\arccos (\frac{u_z}{\cos \theta })$
代入前兩個方程，結(jié)合幾何約束求解$\theta$和$?$，例如當$?=0$時，簡化為：
$$\theta =\arccos (u_z),\psi =\arctan 2(u_y{,}_x)$$

4.計算膝關(guān)節(jié)角度${\theta }_k$
根據(jù)余弦定理，解算膝關(guān)節(jié)角度：
$${\theta }_k=\arccos (\frac{||p|{|}^2?l_1^2?l_2^2}{2l_1{l}_2})$$

注意事項

• 多解性：可能存在多組解，需根據(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í)行器速度（線速度$\dot{\mathbf{p}}$和角速度$\mathbf{w}$）與關(guān)節(jié)速度$\dot{\mathbf{q}}$之間的關(guān)系：
$$\left[\begin{array}{c}\dot{\mathbf{p}}\\ \mathbf{w}\end{array}\right]=\mathbf{J}(\mathbf{q})?\dot{\mathbf{q}}$$
其中，$\mathbf{J}$是6 x n的雅可比矩陣（n為關(guān)節(jié)數(shù)）。

2.逆運動學迭代求解
目標是通過末端位置誤差$\mathbf{e}={\mathbf{p}}_{target}?{\mathbf{p}}_{current}$，調(diào)整關(guān)節(jié)角度$\mathbf{q}$:
$$△\mathbf{q}={\mathbf{J}}^{+}?\mathbf{e}$$
其中${\mathbf{J}}^{+}$是雅可比矩陣的偽逆（Moore-Penrose 逆），計算為：
$${\mathbf{J}}^{+}={\mathbf{J}}^{?}（\mathbf{J}{\mathbf{J}}^{?}{）}^{?1}(當\mathbf{J}行滿秩時)$$
或使用阻尼最小二乘法避免奇異：
$${\mathbf{J}}^{+}={\mathbf{J}}^{?}（\mathbf{J}{\mathbf{J}}^{?}+{\lambda }^2\mathbf{I}{）}^{?1}$$
$\lambda$為代入阻尼因子，通常取0.001~0.1。

3.迭代步驟
a.初始化：設(shè)定初始關(guān)節(jié)角度${\mathbf{q}}_0$，容差$?$，步長$\alpha$。
b.計算誤差：${\mathbf{e}}_i={\mathbf{p}}_{target}?f_{kin}({\mathbf{q}}_i)$，其中$f_{kin}$為正運動學函數(shù)。
c.更新關(guān)節(jié)角：
$${\mathbf{q}}_{i+1}={\mathbf{q}}_i+\alpha ?{\mathbf{J}}^{+}({\mathbf{q}}_i)?{\mathbf{e}}_i$$
d.終止條件：當$||e_i||<?$或達到最大迭代次數(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)和貝塞爾曲線或樣條插值：生成平滑的足端軌跡。

1. 步態(tài)特征

• 步行（Walking）：交替支撐相（單腿支撐）和擺動相，重心軌跡平滑，ZMP（零力矩點）始終在支撐多邊形內(nèi)。
• 小跑（Trot）：對角腿同步運動，動態(tài)平衡需求更高，存在雙支撐相和騰空相。

2. 單腿逆運動學模型
   以右腿為例，關(guān)節(jié)配置為髖關(guān)節(jié)（橫滾$?$、俯仰$\theta$、偏航$\psi$）和膝關(guān)節(jié)${\theta }_k$，踝關(guān)節(jié)${\theta }_a$。
   構(gòu)造雅可比矩陣：
   對于腿部位置$\mathbf{p}=[x,y,z{]}^T$，雅可比矩陣的列為各關(guān)節(jié)對末端速度的貢獻：
   $$\mathbf{J}=\left[\begin{array}{cccc}\frac{?x}{??}& \frac{?x}{?\theta }& \frac{?x}{?\psi }& \frac{?x}{?{\theta }_k}\\ \frac{?y}{??}& \frac{?y}{?\theta }& \frac{?y}{?\psi }& \frac{?y}{?{\theta }_k}\\ \frac{?z}{??}& \frac{?z}{?\theta }& \frac{?z}{?\psi }& \frac{?z}{?{\theta }_k}\end{array}\right]$$

偏導(dǎo)數(shù)計算示例（髖關(guān)節(jié)俯仰角$\theta$）：
$$\frac{?\mathbf{p}}{?\theta }=R_z(\psi )?(\frac{?{\mathbf{R}}_y(\theta )}{?\theta })?{\mathbf{R}}_x(?)?{\mathbf{p}}_{link}$$

3. 步行步態(tài)求解
   支撐相（以右腿支撐為例）：

• 目標：調(diào)整左腿擺動軌跡，同時保持軀干平衡。
• 軀干控制：通過髖關(guān)節(jié)角度補償重心偏移，雅可比矩陣需包含軀干姿態(tài)的影響。
• 迭代過程：
  1. 規(guī)劃左腳擺動軌跡${\mathbf{p}}_{foot}(t)$
  2. 計算當前左腳位置誤差$e={\mathbf{p}}_{foot}(t)?{\mathbf{p}}_{current}(t)$
  3. 用雅可比逆矩陣更新左腿關(guān)節(jié)角$△q={\mathbf{J}}^{+}?\mathbf{e}$
  4. 同步調(diào)整右腿髖關(guān)節(jié)橫滾$?$以維持 ZMP 在右腳支撐區(qū)內(nèi)

擺動相軌跡規(guī)劃：
常用三次多項式插值，設(shè)擺動時間$T$，起點${\mathbf{p}}_0$，終點${\mathbf{p}}_f$，則：
$$\mathbf{p}(t)={\mathbf{p}}_0+(3(\frac{t}{T}{)}^2?2(\frac{t}{T}{)}^3)({\mathbf{p}}_f?{\mathbf{p}}_0)$$

4.小跑步態(tài)求解
動態(tài)特性：雙足騰空相存在，需控制落地沖擊。

• 騰空相：腿部收縮以減少轉(zhuǎn)動慣量。
• 支撐相：對角腿（如右腿和左腿）同步支撐，雅可比矩陣需同時處理雙腿的耦合運動。

協(xié)調(diào)控制步驟：

1. 規(guī)劃軀干質(zhì)心（CoM）軌跡，使其符合倒立擺動力學。
2. 對角腿的腳部軌跡${\mathbf{p}}_{foot,left}(t)$和${\mathbf{p}}_{foot,right}(t)$相位差$180^o$。
3. 對每條腿獨立應(yīng)用雅可比逆矩陣法，但需引入耦合項確保軀干平衡：
   $$△{\mathbf{q}}_{left}={\mathbf{J}}_{left}^{+}?{\mathbf{e}}_{left}+K?({\mathbf{q}}_{right}?{\mathbf{q}}_{nominal})$$

K為細條增益矩陣。

5.關(guān)鍵問題與優(yōu)化

• 奇異性處理，當腿完全伸展${\theta }_k=0$時雅可比矩陣秩虧，需增加阻尼項或約束關(guān)節(jié)速度。
• 多目標優(yōu)化
  同時優(yōu)化腳部位置、軀干姿態(tài)和關(guān)節(jié)限制：
  $${\min }_{△q}(||\mathbf{J}△\mathbf{q}?e|{|}^2+\lambda ||△\mathbf{q}|{|}^2)$$
  其解為：
  $$△\mathbf{q}=({\mathbf{J}}^{?}\mathbf{J}+\lambda \mathbf{I}{)}^{?1}{\mathbf{J}}^{?}\mathbf{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ù)實時修正期望位姿