第三章 步態規劃
3.1 步行軌跡產生系統架構
3.2.1 腰部及腳部軌跡產生
首先是圖 3.1 中第一區塊的功能描述，由於吾人需要進行腳部之軌跡規畫，
來取得想要之運動軌跡。吾人為了使雙足機器人步行運動能近似人類步伐，在此 使用了擺線輪廓曲線函數來規劃雙足機器人步行時的腳掌座標位置[27][28]，設 定擺線輪廓曲線函數所需的參數由使用者於程式中設定，需要設定的參數包括擺 線輪廓曲線的高度、長度與取樣切分數。如圖 3.2 中虛線所示，其中黑色代表是 左腳，而灰色代表的是右腳。此擺線輪廓曲線函數提供一簡單函數來描述人類正 常步行之腳踝的運動軌跡。且此函數容易修改，適合不同的步行模式。左右腳的 軌跡也都相同，僅只是順序不同輪流出現。
令擺動腳之運動軌跡如式(3.1)所示。其中， 為跨一步於 Z 軸方向之半跨距，
也就是擺線總長度之一半；h 為跨步高度；T 為跨一步之取樣數，也就是將一週 期擺線分為多少等分。圖 3.3 顯示出雙足機器人行走模擬圖，其中紅色(較淺色) 代表是左腳，而藍色(較深色)代表的是右腳，上方黑色則是代表軀幹。模擬參數 分別為半跨距 60mm、跨步高度 h=40mm、跨步取樣 T=128、桿件長度為上：
108.6mm，下：135.4mm。
x t 2πT sin 2πT
z t 1 cos 2πT , 0 t T (3.1)
圖 3. 2 擺線輪廓曲線函數示意圖
圖 3. 3 雙足機器人行走模擬側視圖
另外，由於在本論文中所使用的是靜態步行，除了腳部軌跡之外，還必須規 畫腰部的軌跡，藉以規劃機器人之重心位置來使得機器人的重心可以落於支撐多 邊形內，軌跡設定的方式就是在單腳支撐相時，將重心置於支撐腳上方，而在雙 腳支撐相時就將重心往另一隻支撐腳移動，為了避免軌跡轉折點造成的不穩定，
腰部軌跡的產生同樣使用了可調整的擺線輪廓曲線函數作為軌跡，軌跡函數為式 (3.2)，使腰部可以進行水平方向平滑的左右移動，如圖 3.4 所示，使機器人可以 在單腳支撐相時將重心偏移至支撐腳上，而雙腳支撐相時再將重心經由支撐多邊 形偏移至一隻腳支撐面內，而一個完整週期步行中共需 2 個腰部的擺線輪廓曲線 軌跡。
x t 2πT sin 2πT
y t 1 cos 2πT , 0 t T (3.2)
圖 3. 4 單週期腰部軌跡示意圖
3.2.2 腳部至腰部向量計算
此節描述的是圖 3.1 中第二區塊的功能，吾人根據前節決定好的腰部軌跡以 及腳部軌跡即可相減算出左右兩腳所應該產生之由腳部至腰部的末端點向量軌 跡，如圖 3.5 所示。而此位置向量即是從腳底指向腰部的的操作臂末端點位置，
因此我們需要將其帶入反向運動學進行角度求解。
圖 3. 5 腳部至腰部位置向量計算示意圖
3.2.3 雙足機器人運動學
機器人的運動學又可分為順向運動學（Forward‐Kinematics）及逆向運動學
（Inverse‐Kinematics）兩部分。順向運動學為給定各關節軸之旋轉角度數據，進 而求得末端桿件（link）在絕對座標系中的空間位置；而逆向運動學則是以給定 末端桿件在絕對座標系中的空間位置，反過來求解各軸關節的旋轉角度，這也就 是圖 3.1 中第三區塊主要執行的功能。若已知各桿件向量和各關節旋轉角的資料，
可利用順向運動學方程式推導出各關節點之空間座標，也可由連續的腳掌點座標 形成行走軌跡。為了模擬雙足機器人步行的情形，必須先規劃步行姿態使機器人 動作。其中，兩隻腳掌座標以及髖部座標必須先規劃完成，而後使用這三點之座 標軌跡，透過逆向運動學之計算，即可求解出機器人步行時各關節旋轉軸之絕對 角度。而透過步行姿態規畫也可改善或調整機器人之穩定性。在本論文中，吾人 採用的是 D‐H model 將機器人建立運動學模型，用以進行順向運動學以及逆向運 動學的計算。D‐H model 是一種針對連桿運動設計的運動學分析法，可以對連桿 在 3 度空間的移動與轉動有著簡單而完整的描述。
進行分析之前，本論文先做下列之假設：
(1) 身體中心的位置將一直保持在離地面固定高度上，將運動從 3D 降為 2D，
身體的姿態也將在運動中保持不變，也就是身體主要是處於水平移動 的狀態。
(2) 移動中的腳底板的姿態會與初始狀態相同，也就是維持水平且方向不 變。
(3) 各桿件為剛體。
(4) 桿件間旋轉接頭處視為一點。
(5) 不考慮雙足機器人受地面衝擊之振動行為。
(6) 假設雙足機器人支撐腳底不與地面產生滑動。
接著利用D‐H model來建立運動學模型，建立順序為由腳底到腰部，如圖3.6
所示，表3.1為雙足機器人D‐H model的參數表，其中每4個D‐H參數（θ、α、a、d）
圖 3. 6(a)腳部模型(b)腳部 D‐H model
θ α a d
因此本論文之雙足機器人模型可建立以下 6 個轉換矩陣，如式(3.4‐3.9)，除
根據模型可知此種結構為 wrist‐partitioned 的類型，求解 6 自由度操作臂之時，
有容易解的算法。也就是因為末端位置的決定只受到 A1‐A3 轉換矩陣的影響，
A4‐A6 轉換矩陣只會影響到末端姿態，因此可以藉由末端空間的三個座標變數(X、
Y、Z)來解得三個矩陣的角度變數(θ 、θ 、θ )，而產生將 6 自由度的問題拆成 2
個 3 自由度問題的效果。令吾人所想取得的末端空間座標值 P 與系統的前三自由 度轉移矩陣 A1*A2*A3 之第四行，設為 T13，分別為式(3.10‐3.11)，式(3.11)即為順 向運動學推導出之本系統末端之空間座標。
θ =Atan2(sin θ , cos θ ) (3.22)
R ＝
圖 3. 7 一組位置可解出之所有角度解
接著將前節所得到之向量軌跡依序帶入反運動學求解，即可得到所應產生之 旋轉角度軌跡，而利用解出之角度帶入順向運動學中，畫出的軌跡模擬圖，如圖 3.8 所示，腰部軌跡參數：半跨距 30mm、距中心橫移距離 h1=70mm、跨步 取樣 T1=64；腳部軌跡參數：半跨距 60mm、跨步高度 h2=40mm、跨步週期 T2=32、桿件長度為上：108.6mm，下：135.4mm，全週期取樣為 128，左右腳軌 跡相差半個週期，單腳支撐相及雙腳支撐相時間長度相等並交互出現，腰部在 Y 方向以擺線輪廓曲線左右橫移。
圖 3. 8 單週期雙足機器人行走模擬 3D 圖
3.2.4 雙足機器人行走之分析
一般而言，過去的研究將兩足機器人之行走的過程中的姿態分為單腳支撐相
（Single‐support phase）與雙腳支撐相（Double‐support phase）兩種[22][29]。而 機器人在行走的過程中，便是這兩種相位循環交替，形成週期性的動作而前進，
如圖 3.9。其中，假設灰色的腿為左腳，而黑色的腿為右腳。首先由右腳跨出一 步，開始跨步時，從圖 3.9 中由左到右歷經雙腳支撐相與單腳支撐相之後，最後 再回到雙腳支撐相，如此反覆循環，達成連續行走之姿態。
因此將連續 2 組單雙支撐相組合起來成為一個新週期，如圖 3.10 所示，機 器人的動作將會是完全重複。因此在圖 3.1 中第四區塊的功能中，主要是將吾人 設計出之一完整週期之步態軌跡，按照命令輸入的步數連接起來重複組合使用，
展延出所需的步行距離，並且吾人所設定之單腳支撐相與雙腳支撐相之取樣時間 皆為相等。因此，吾人之做法為在機器人控制起始之時先將所需之全步態軌跡運 算完成，而後再將運算出之分段取樣軌跡逐個輸出使機器人產生運動。
1 2 3 4
雙腳支撐相Double 單腳支撐相 單腳支撐相 雙腳支撐相
support phase
Single support phase
Single support phase
Double support phase
圖 3. 9 雙足機器人行走相位示意圖
圖 3. 10 雙足機器人步行之完整週期
當靜態步行在移動的過程中，為了不至於傾倒，都必須將重心置於支撐腳的 多邊形內，如圖 3.11，虛線代表的是腳浮空，圖中紅色所代表的是機器人的重心 位置，而重心是隨著步伐的移動而改變的，但是都是落在支撐腳多邊形上，如此 便能保持平衡不跌倒。因此，在機器人學的分析中，機器人的關節運動軌跡規畫 是重要的，因為這對於兩足機器人之平衡有著絕對的關係。一般來說，吾人先經 由運動學之分析，以驗證關節（joint）運動軌跡之可行性，而後經由實驗驗證以 完成控制器。
雙腳支撐相 單腳支撐相 單腳支撐相 雙腳支撐相
1 2 3 4
重心 左腳
右腳
圖 3. 11 雙足機器人行走重心示意圖
第四章 補償控制器設計
在前節中，吾人已經設計好了一個可以進行平地行走的軌跡，使雙足機器人
得以進行直線靜態步行，但是當機器人一但遭遇障礙物或非理想地形時，就很容 易產生不穩甚至於傾倒的現象。因此，設計一個控制器可以在行走過程中感測地 形的變化，並根據變化輸出補償去微調原本的步行軌跡，不使重心的移動產生過 大的變動，並在變化解除之後得以還原至原本的軌跡將補償值歸零，便是本節的 重點。
4.1 補償控制器架構
本論文中補償控制器的架構，如圖 4.1 所示，是屬於每個步行取樣中都會啟 動的及時控制方式，內容概述如下：
在雙足機器人的步行取樣中，根據裝置在雙足機器人腳底的壓力感測器所取 得的數值，傳送到補償器中，藉由自行建表內插轉換成為壓力值，再根據 ZMP 計算公式算出 ZMP 的位置。將算出之 ZMP 位置與使用者設定之相關參數送入補 償產生器，計算本次步行取樣所需的補償角或補償歸零角累加至總補償角中於每 次取樣時輸出。
接著於每次取樣時，步行軌跡產生器與補償器都會送出數值進行加總，一個 是本次取樣時理論上應執行的角度，另一個是累積至本次取樣時之總補償角度，
合成後則為本次取樣實際輸出角度。將實際輸出角度連續輸出則成為新步行軌跡。
角度合成如式(4.1)，令 為最終輸入給馬達的角度， 則是步行軌跡所產
生的馬達角度，∆ 就是補償控制器所產生的總補償角，以上三個皆為長度 12 的向量，分別對應 12 個馬達自由度，但在總補償角向量中，由於吾人只針對踝
生的馬達角度，∆ 就是補償控制器所產生的總補償角，以上三個皆為長度 12 的向量，分別對應 12 個馬達自由度，但在總補償角向量中，由於吾人只針對踝