往期

本文是雙足機器人系列的第三篇，在前面的文章中我們介紹了2D線性倒立擺的基本理論，詳見：

【雙足機器人（1）】線性倒立擺及其運動控制（附代碼）

在這篇文章中我們要詳細介紹3D線性倒立擺的基本內容，以及使用Python來實現3D線性倒立擺的簡單仿真。

話不多說，先上一個最后的仿真視頻：

1. 3D-LIPM模型

1.1 模型介紹

三維倒立擺是由一個集中了所有質量的點（質心）和一條無質量的腿組成，如圖所示：

支撐點處的力矩為零，可以自由轉動。質心在伸縮力的作用下可以上下移動，伸縮力可以分解為三個方向的分量：

相對于二維的情況增加了第三維（z軸）。其中，為支撐點和質心之間的距離。質心只受伸縮李和重力的作用，因而其運動方程為：

其中，為質心的質量。類似于二位情況，對于三維倒立擺，定義約束面如下：

其中，決定平面的傾斜，決定其高度。

為了使質心在約束面上運動，其加速度應該保持與約束面的法向量垂直，即：

從上式中解出，然后我們就可以得到：

因此，我們可以通過控制與腿長成正比的伸縮力來控制質心在約束面上運動：

我們可以求得質心在水平方向的運動方程如下：

上面兩式是線性方程，只有約束面的截距作為參數。由于約束面的斜率參數并沒有包含在上述兩式中，因此約束面的斜率不影響執行的水平運動，這樣的倒立擺稱為三維線性倒立擺模型（3D-LIPM）。

1.2 模型的特性介紹

直觀上，我們可以把三維線性倒立擺模型看做是在和方向上的兩個二維線性倒立擺的組合，但其運動特性卻更有趣。如果把三維線性倒立擺的空間運動軌跡投影到平面上則我們可以得到以約束面的軸截距為焦點的類似天體運動的軌跡。

1.2.1 開普勒第二定律

既然類比到天體力學，那么首先介紹一下開普勒第二定律：行星運動的面積速度守恒。

什么叫面積速度？面積速度是指單位時間內質心和原點的連接線所掃過的面積，在直角坐標系中可以由下式計算：

由此可得三維線性倒立擺的面積速度的時間變化率：

由此可以得出，與行星的運動一樣，三維線性倒立擺的面積速度保持恒定（在另一個意義上表示為角動量守恒）。

1.2.2 坐標系變換的影響

為什么要考慮坐標系變換的影響？因為在約束平面內運動的方位有多種情況，要考慮上述的運動過程是否與運動的方位有關。假設我們是從原來的參考坐標系通過旋轉角變換到新的坐標系中，如下圖所示：

那么，坐標的變幻式為：

其中，，我們可以得到：

對上面兩個式子兩邊做變換并加和，如下所示：

則變換結果為：

因此，對于任意一個旋轉角，這個結論都成立。因此證明了三維線性倒立擺的水平運動在兩個正交方向上的分量與參考坐標系的方位無關。

1.2.3 軌跡的幾何形狀

參考前面的坐標變換圖，我們可以根據軌道能量推導軌跡的幾何形狀。沿新的坐標軸的軌道能量為：

這一能量隨轉動角而變化，當坐標系的軸或軸與軌跡的對稱軸一致時，取得最大值。

然后經過一系列的復雜推導（見原書4.3.2章節），我們可以整理得到三維線性倒立擺在坐標軸與軌跡對稱軸一致的坐標系中的軌跡形狀方程：

這是雙曲線方程，當沿軸方向的軌道能量和沿軸方向的軌道能量中的一個為負時，另一個為正。

2. 三維步行模式的生成

在二維線性倒立擺的步行模式生成中，我們允許任意的支撐腿切換時間，但是在三維的線性倒立擺中，需要在軸和軸方向上同時切換支撐腿，因此不能簡單的采用與二維線性倒立擺一樣的處理辦法了。在后面的討論中，我們假設支撐腿的切換周期固定，并記每一步的支撐時間為。

2.1 步行單元

在時間段內，定義三維線性倒立擺的軌跡如下圖所示的一段曲線，它是關于軸對稱的一段雙曲線，我們稱之為步行單元。

給定和，則步行單元由其終止位置唯一確定。終止速度可由下面的計算過程得到。

首先，回顧一下線性倒立擺的解析式：

則對于三維線性倒立擺，利用步行單元的對稱性，假設沿軸的初始條件為，終止位置為，則我們有：

其中，。

可以求得終止速度為：

類似的，對步行單元的軸分量，初始條件為，終止位置為，終止位置和速度可計算為：

利用步行單元可以很容易生成步行軌跡。例如，對于步長為的直線行走，只需將相同的步行單元連接起來交替改變其軸分量的符號即可。

2.2 步行參數

在大多數情況下，我們需要直接指定腳的落地位置，也就是落腳點。這里，使用步長和步寬來定義步行參數：

其中，是前進方向上的步長，是左右方向上的步寬，上標表示第步。表中的數據稱為步行參數，描述了上圖中的腳步情況。第個計劃的落腳位置可表示如下：

其中，表示初始的支撐腳位置，在這里是步行開始時的右腳位置。如果開始時支撐腳是左腳，那么上式中的要替換為。第步的步行參數由第步的步長和步寬決定，如下：

步行單元的終止速度可計算為：

用這種方法確定的步行單元序列在步行開始和結束時是不連續的，連續的和可實現的步行模式的生成方法見下個章節。

2.3 落腳點的調整

在步行周期和落腳時間固定的情況下，可以通過調整落腳點來控制步行的速度，簡單來說就是，在較近處落地則速度加大，在較遠處落地則速度減小。如下圖所示：

為了計算實際的落腳點，我們來分析一下單個步行單元的過程。如下圖所示：

我們現在考慮沿軸運動的情況，沿軸運動的結果類似。假設調整后的落腳位置為，則相對于地面上的固定坐標系，線性倒立擺的運動方程為：

其解析解為：

其中，為第步開始時的初始條件。因此，落腳點 與第步的最終狀態（LIPM解析值）之間的關系如下：

至于最終狀態的目標值，可用地面參考坐標系中最終步行單元的終止狀態（理論遞增值）表示：

為了計算落腳點導致的最終狀態與目標狀態之間的接近程度，可采用下面的評估函數：

其中，為取值大于零的加權因子，根據條件，可求得使誤差評估函數值最小的落腳點：

其中，

上述算法可以總結為：

設定步行周期和步行參數、質心的初始位置以及初始落腳點；

初始化；

從時刻到，對線性倒立擺方程式積分；

；

計算下一個落腳點；

計算下一個步行單元；

計算方向上的目標狀態，根據對應的式子計算方向上的目標狀態；

計算調整落腳點的位置（方向上也同樣計算）；

返回第三步，進行下一次邁步。

我們來理解一下這個算法：

理論上，如果我們給定了倒立擺的初始狀態和支撐階段的持續時間，那么我們就可以根據倒立擺的動力學解析方程式計算得到在當前支撐時間結束的時刻處的倒立擺終止狀態，它在支撐階段結束后就應該是這個狀態。但是在支撐階段結束以后，我們需要的下一步落腳點在哪里呢？這個是一個開放式的問題，還需要額外的第三個量來計算。在二維的情況下，我們可以根據切換以后我們期望的倒立擺的運動狀態來計算得到，也就是切換以后下一個倒立擺的支撐周期的軌道能量。在三維的情況下，理論上我們也可以這樣做，拆分為兩個方向上的二維倒立擺來計算。

實際上，在前面的計算過程中，我們知道當前的腳的位置，又給定了下一步的步行參數，這個步行參數是用來指定下一步的落腳點的，即，我們希望倒立擺模型能夠在本次支撐階段結束后的切換時刻，另一只腳剛好到達這個落腳點，此時，在這個落腳點處，倒立擺的目標狀態可以反向計算為，其中分別可通過前面的公式計算得到。

但由于步行參數可能是一個手動指定的結果，如果將下一步的落腳點設置到這個步行參數確定的位置上，此時倒立擺的終止狀態不一定與上面第1條使用得到的倒立擺終止狀態一致，存在誤差。

從上面的第1條和第2條我們可以得到，給定倒立擺的初始狀態和支撐時間我們可以得到一種倒立擺的終止狀態，給定倒立擺初始的步行參數，我們又可以得到另一種倒立擺的終止狀態，并且這兩種終止狀態還不一定是一致的，可能存在較大的誤差，因此，我們需要引入一個方法來最小化這個誤差：。代入所有的公式，最后可以求得一個能同時平衡和的下一步落腳點，這個落腳點才是我們需要的。

當然，如果參考二維線性倒立擺的情況，我們可以結合當前的軌道能量和給定期望的方向上軌道能量來計算下一步的落腳點。

需要注意的細節是，我們需要在每次切換支撐腳的時刻就計算好下一步的切換支撐腳的落腳點了，這樣，我們才能提前規劃好擺動腿在這一步中的擺動過程，并控制擺動腿的關節向前運動到達下一步。如下圖所示，我把書中的圖拓展了一下，方便理解：

其中，在時刻，切換支撐腿，此時我們應該計算好下一步的落腳點，從而方便我們進行擺動腿的空間軌跡規劃以及控制倒立擺從時刻到時刻的運動過程。

2.4 步行方向的改變

為了改變行走方向，需要在步行參數中追加有關方向的信息。假設每步踏足的方向用表示，如下圖所示：

則第步計劃的落腳點由下式確定：

第步的步行單元給定如下：

步行單元的速度計算方式：

使用上面的結果分別替換前面沒有加入轉角的各個計算結果可以得到連續轉彎的步行模式。

3.仿真實驗結果

向前直線運動，走幾步后停止向前行走：

持續左轉彎：

持續右轉彎：

本文所提到的公式已經使用Python實現，詳見：https://github.com/chauby/BipedalWalkingRobots

參考文獻

Shuuji Kajita, Hirohisa Hirukawa et al. Introduction to Humanoid Robots, 2004.

往期文章：

【雙足機器人（1）】線性倒立擺及其運動控制（附代碼）

【雙足機器人（2）】倒立擺運動學模型構建（附代碼）

總結

以上是生活随笔為你收集整理的【双足机器人（3）】3D线性倒立摆Python仿真（附代码）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。