華北舵狗王帶你一起做四足機器人5 （機器人步態演算法補充--爬行步態演算法）
Moco-8僅有8自由度因此以對角步態為主，而對四足機器人來說爬行步態是更重要的一種步態其能實現在崎嶇路面上的移動特別是上下樓梯大多以爬行步態為主，相比如波士頓動力公司所設計機器人採用的液壓或高性能伺服驅動，受體積和成本限制其關節多採用舵機及減速組傳動，因此關節響應速度較慢同時由於缺少關節角度反饋和足底力反饋使其難以實現可靠的對角小跑步態，因此爬行步態是微型四足機器人中最主要的移動方式。
優酷視頻由於爬行步態任意時刻最少有三條腿來支撐機身，因此其具有較高的靜態穩定性。近年來研究人員對爬行步態演算法已經進行了大量研究，主要使用絕對穩定裕度來衡量機器人的穩定性並進行步態設計，當穩定裕度為正時機器人就能保證靜態穩定。如在文獻中作者使用多項式曲線擬合演算法規劃重心在下一步態周期中的移動軌跡，進一步使用運動學逆解控制足尖移動並保證重心時刻處於著地平面內，基於機器人設計參數進行步態周期的合理調整、限制移動速度就能保證多數情況下的穩定行走。該類演算法雖然簡單、易於實現但難以引入閉環反饋控制，因此自適應能力較差。目前四足機器人主要採用三角支撐原理（TSP）來設計爬行步態，基於TSP原理規劃重心移動位置，其核心是保證跨腿時機器人重心投影（COG）或零力矩點（ZMP）處於著地三角形平面內。另外通過採用多個三角形重疊支持或對軌跡添加動力學約束能進一步提高TSP步態的穩定性。機器人移動時除了能前進後退外還需要具有側向移動和轉向能力，傳統TSP演算法往往以離散周期的形式對重心進行規劃，因此多直接降低速度或提高步頻來保證變向中機器人不側翻。為得到更連續的爬行步態一些研究人員對步態時序進行細分，通過增加重心側向調整實現連續行走，另外還有一些演算法通過TSP與CPG神經網路等振蕩器相結合的方式來得到連續爬行步態。綜上，採用TSP原理能規划出穩定可靠的爬行步態但仍然有一定優化改進空間，如在現有演算法多採用固定步態周期，而在實際環境中機器人會遇到突發情況需要進行快速步態調整；另外機器人往往會受到外力干擾如果不對其進行控制則很有可能出現側翻，而現有演算法往往需要增加複雜邏輯來抵抗外力，其加大了步態演算法的邏輯複雜度。
本文介紹一個由本人提出的改進ZMP智能決策爬行步態演算法，其針對現有TSP爬行演算法採用固定步態周期自適應能力低、抗外力演算法邏輯複雜和步態連續性仍待優化的問題，演算法在著地區域中實時規劃ZMP期望軌跡並進行反饋控制，實現了平滑快速的機體移動，同時基於所提演算法能方便地引入感測器數據構成反饋控制。為提高步態規劃自適應性，文中對傳統TSP演算法中採用固定步態周期的方法進行改進，基於面積貪婪決策實現自主跨腿。通過在微型四足機器人上的實驗，驗證了該演算法能夠實現穩定的全向行走和轉向，通過突加擾動測試驗證了所提步態演算法能在一定程度上克服外力，所提演算法邏輯簡單易於軟體實現。
四足機器人 爬行步態測試 無著地反饋信號
2 基於反饋控制機器人重心規劃演算法
基於反饋控制的思想，本文設計了一種可以連續調節ZMP點的規劃演算法。該演算法以ZMP點作為反饋，採用非線性PD控制器進行反饋控制。由於爬行步態同一時刻最多有一隻腳抬起，因此將軌跡規劃分為四腳著地和三腳著地兩種情況進行設計。
2.1 四腳著地時期望ZMP軌跡設計
上圖為四腳著地期望ZMP軌跡示意圖，設當前ZMP位置為 其計算方法如下：
其中 為機器人質心在足支撐平面投影坐標，h為機體質心高度，
是慣性感測器測量到機體坐標系下的加速度，g是重力加速度。另設機體坐標系下期望速度矢量
(矢量角度為
，
是速度矢量模值)，期望重心位置為
，點
為著地腳構成四邊形的中心，根據穩態裕度計算公式可知在該點位置穩定性最好。過點
以速度矢量
方向做直線L1，過點
做直線L1的垂線L2， 它們的交點為
，由於：
則坐標可以由下式計算得到:
進一步得到期望ZMP點 為：
其中 為期望速度增益，dt為重心控制周期。
2.2 三腳著地時期望重心軌跡設計
上圖為三腳著地時期望ZMP軌跡示意圖，設機器人各腳當前投影位置為 ，
，
，
三腳著地時可以細分為兩種情況，速度矢量前方只有一條著地腳如上圖所示，速度矢量前方有兩條著地腳如上圖所示。期望ZMP點計算方法與2.1小節中計算方法類似，定義最小邊界裕度d1和d2為直線L1上
和
離著地區域邊界的距離，則三腳著地時期望ZMP軌跡可表示為如下分段函數。
其中
同理計算出L1，L2交點後，由上式得到最終期望ZMP位置
。
2.3 ZMP點反饋控制器
在2.1和2.2小節中得到了不同著地腳情況下期望ZMP位置 ，則以當前ZMP位置
作為反饋，設計如下的非線性PD控制器：
其中 ，
。
上式中 為控制器輸出，
和
為控制器可調參數。c是著地區域權值，該權值的作用是在著地面積過小時減小重心調整速率，避免機器人蹬腿過快造成側翻，保證重心平穩調整；
是非線性函數參數，
是當前著地腿構成多邊形的面積，
是設定的最小著地面積，
是為c的下界。則各著地腿足尖水平移動速度如下：
其中 ，
，
，
為期望機體角速度，L為機體長度，W為機體寬度。
3 基於面積貪婪決策的步態規劃演算法
本文所設計的步態規劃演算法流程如圖3所示，所提ZMP點控制演算法在每個程序周期執行，跨腿則由條件一（任意腿超出最大可移動範圍）和條件二（ZMP點超出重心可移動閾值半徑）觸發。前者具有較高優先順序即某一腿超出範圍後優先跨腿，在僅滿足條件二時通過所提面積貪婪決策來選擇需要跨的腿。面積貪婪決策是本文步態規劃的核心演算法，為實現該演算法首先做出下述兩個假設。
假設一：機器人總希望跨腿後著地腿構成多邊形面積增大。
假設二：機器人因跨腳需要重心移動時，總希望有最大的區域能讓重心移動。
上述假設保證了機體側身移動中有足夠的著地區域讓ZMP點在其中移動，在機器人腿向外側展開時能保證三腿著地期間機器人具有較大的穩定裕度，另外機器人腿部內縮時有可能會出現由於著地面積過小造成重心調整側向力太大、機器人側翻，該問題則基於採用2.3小節中所設計控制器的非線性控制輸出解決，同時通過對各腿可移動範圍進行限制能保證一個最小著地面積避免上述情況出現。
根據上述假設，以圖4中所示情況為示例，給出本文最終設計的面積貪婪決策演算法偽代碼如表1所示。
上圖為機器人前進過程中步態規劃示意圖，圖中黑色方框為機器人機體，棕色橢圓為各腿可移動範圍(長軸為l1，短軸為l2)，黃色圓形區域為重心可移動範圍，其圓心為當前著地腿構成多邊形中心，半徑為可調閾值Rs，虛線數字標誌表示當前需要跨的腿。
在上圖（a）中各腿未超出自己的移動範圍而ZMP點超出黃色區域，根據所設計步態規劃演算法此時需要跨腿，由面積貪婪決策可知機器人B，D腿跨腿會導致著地面積的減小，因此選擇速度矢量前方的腿A號和C號腿作為候選，根據面積貪婪決策演算法速度矢量前方側身區域較大面積為藍色區域，則C號腿需要跨出。在完成移動後機器人狀態如上圖（b）所示，此時D號腳超出可移動範圍需要優先跨腿。同理上圖（d），（e）採用面積貪婪決策演算法選擇所需跨的腿。針對不同四足機器人的結構參數，通過設置合適的l1，l2和Rs就可以得到穩定性較高的爬行步態。
在確定跨腿ID後需要進行落足點選擇和足尖規劃軌跡，本文採用擺線軌跡保證足尖著地速度為零，同時為保證機器人能克服側翻，文中使用機體慣性感測器(IMU)計算出的橫滾角 來對著地點位置進行二次修正，使機器人在側翻時做出足尖側擴來增大著地面積提高落地穩定性。設跨腿所需時間為
，抬腿的高度為H;
為期望落足點位置，
為當前足尖位置。則擺動腿軌跡數學描述如下：
其中 為最大橫滾角修正外擴跨腿距離，
。
如上圖所示，文中演算法運行在STM32單片機控制器中，系統上電後單片機開始從機載IMU中讀取加速度和陀螺儀數據，通過互補濾波進行姿態解算得到機體加速度和實時姿態角，控制器以100Hz的頻率進行所提ZMP反饋控制。首先基於遙控器期望移動速度和足尖位置進行ZMP軌跡規劃，所提反饋控制器基於期望ZMP軌跡和實時ZMP點進行反饋控制，實現機器人重心移動。在每個控制周期內程序基於所提自適應跨腿觸發演算法判斷是否需要跨腿，當檢測到跨腿觸發信號後以使用所提貪婪決策演算法確定需要跨腿的ID號，跨腿規劃器則對該腿基於機器人實時姿態和跨腿參數規劃軌跡，軌跡跟蹤器輸出期望足尖位置。最終，運動學逆解模塊以足尖期望速度和位置計算機器人關節角度，同時將其轉換為PWM信號輸出給關節舵機完成機器人步態控制。
4 步態規劃實驗
由於機器人未安裝足底力反饋感測器，因此選取平地場景來驗證所設計演算法的可行性。以前進步態為例，給定移動速度2cm/s， 矢量角度為0°，實驗過程如上圖所示。上圖（a）中反饋控制器控制重心向前移動，上圖（b）中重心超出範圍根據所提演算法規劃觸發D腿，機體受反饋控制器控制進行側身同時重心繼續向前移動，上圖（c）處A號腳超出可動範圍優先跨出，之後上圖（d）中腳C同樣由於超出範圍優先跨出，上圖（e）處B腳由於腿超出跨腿，上圖（f）出A腳由於機體前移造成重心超出跨腿。此外，機器人左右平移以及旋轉過程如下圖所示，具體步態規划過程不再詳述。
以前進為例，下圖給出了機器人在該過程中的穩態裕度變化曲線。從下圖中可以看到本文所提演算法在固定速度下穩態裕度呈周期性變化，並且在整個過程裕度均大於零即重心在移動過程中均能落在著地區域內保證機器人不會翻倒。
下圖為移動過程中機體高度和姿態角變化曲線，可以看到在移動過程中機器人姿態無明顯晃動，機體重心高度保持在設定高度11.5cm左右，所提演算法在無主動姿態控制的情況下移動較平穩，高度保持穩定。
5 外力擾動實驗
所提步態演算法通過引入IMU數據構成閉環控制，使得機器人能通過側蹬來克服一定程度的突發外力擾動，實驗中通過人為向機器人施加側向外力驗證所提反饋演算法，結果如下圖所示。
在上圖（a）機器人受突加外力作用，在上圖（b）機載IMU檢測到姿態變化和加速度變化，首先由加速度計測量值造成ZMP變化控制足尖移動使機器人側蹬，同時姿態變化使得機器人落足點向外擴張如上圖（c）所示，最終在上圖（f）中機器人通過主動調節ZMP誤差和跨腳落足點擴張保證了自身在外力作用下不會發生側翻。下圖給出了受突發外力擾動下的機器人機體ZMP曲線和加速度曲線。
上圖為機器人運行狀態變化曲線，上圖（a）為未使用反饋控制下的機器人狀態曲線，可以看到在外力作用下機器人穩態裕度產生較大波動，上圖（b）為使用所提反饋控制演算法下的狀態曲線，可以看到在外力作用下機器人保持穩定穩態裕度波動較小，可見所提演算法能有效抑制突發外力擾動。
本節介紹了由本人提出的改進爬行步態演算法，其已經被應用於早先的LittroDog四足機器人中，後續我們將開發12Dof的四足機器人Moco-12會將上述演算法移植到其中！
推薦閱讀：
