NAO 之運動學計算

如圖4.7 所示，圖中為建立 D-H model 時，所設置在各自由度的三軸座標方向。

透過圖4.3 所建立的 D-H model，我們可以得到機器人 D-H model 的參數，

A 據此模型可知此結構為wrist-partitioned 的類型，所以位置屬於前三軸來得到，

後兩軸處理旋轉，因此我們先利用轉移矩陣乘積中之第四行來得到前三軸自由度

θ Atan2 p , p (4.13)

sin θ n cos θ sin θ n cos θ n sin θ (4.24) sin θ n cos θ n sin θ (4.25) cos θ o cos θ o sin θ (4.26) 因此可知，角度範圍在0~π時有一組解為:

θ Atan2 sin θ , cos θ (4.27) θ Atan2 sin θ , cos θ (4.28) 角度範圍在 π~0時有另一組解為:

θ Atan2 sin θ , cos θ (4.29) θ Atan2 sin θ , cos θ (4.30) 由上述的求解方式我們可從一組位置解出每個關節角度的所有解，而所有解 的組合如圖4.4 所示，共有八組，而此八組解需搭配表 4.1 中雙腳各個自由度可 活動的範圍來進行篩選，最後只留下一組可執行解。到此，我們已經可以將想要 之末端座標經由逆向運動學轉換成為各自由度之旋轉角度，達到機器人行走時的 步態規劃和姿態控制。

其中角度值θ 的 i 表示第 i 個自由度，j 表示第 j 個解 4.3 NAO 之感測器

NAO 搭載多種感測器:

圖 4.4 一組位置可解出之所有角度解

 雙腳腳底安裝了八個腳底壓力感測計(FSRs)，此壓力傳感器利用測量電 進行補償控制的應用，因為NAO 官方 Aldebaran Robotics 公司已經利用一套算

法，透過陀螺儀和加速規的感測值，計算出軀幹的姿態角度。

NAO 官方表示此套算法利用了每個感測器的優良特性:當在靜態模式時，加 速規是唯一的絕對參考，藉此得到準確的軀幹角度；當有運動被偵測到時，輸出 角度計算將與陀螺儀一起運算，因為陀螺儀有好的動態性能。而對於上述的偏差 值，此套算法也是透過加速規與陀螺儀的融合計算來達到消除此偏差值。

另外，為了驗證行走的步態規劃是符合靜態步行的設計，機器人的重心投影 必須全落在腳部的支撐多邊形區域內，因此我們將再利用八個腳底壓力感測計 (FSRs)，測量機器人與地面之間的作用力，經由計算來得到 ZMP 軌跡。NAO 腳 底壓力感測計配置位置如圖4.5 所示，此圖中除了壓力感測器的相關位置之外，

我們也標示了在行走時雙腳腳底在前後和左右方向的長度，來定義出ZMP 的穩 定區間。此感測訊號透過類比轉數位轉換後傳輸至控制系統中，並經過公式計算 得到ZMP，如式(4.31)，成為量測機器人平衡的環境資訊。在式(4.31)中， 為第 j 個感測器所量到的壓力， 為第 j 個感測器的位置向量，P 為 ZMP 的位置向量，

n 為壓力感測器數量。因此，將機器人全部的 8 個感測器帶入所算出的 ZMP 為 總ZMP，而如果只將單一隻腳的 4 個感測器帶入計算則可得到對應此腳的 ZMP，

而利用單腳的ZMP 也可得知地面分別對左右腳的受力情形，而在單腳支撐相時，

支撐腳之單腳ZMP 即為總 ZMP，雙腳支撐相時，總 ZMP 必定會在左右腳單腳

圖 4.5 NAO 足部壓力計分布圖

ZMP 之連線上。透過這樣的計算方式，可以簡易且直覺的計算出雙足機器人於

因此我們就可以使用公式(4.32)用來計算出目前機器人 ZMP 的位置，在式(4.32) 中， 為第j 個感測器所量到的壓力， 為感測器輸出值， 為感測器懸空時量

Foot-Position 500g 200g 100g 0g Right-RearRight 0.132 0.071 0.0478 0.03 Right-FrontRight 0.117 0.058 0.036 0.02 Right-RearLeft 0.095 0.038 0.02 0 Right-FrontLeft 0.071 0.0275 0.0073 0

Left-RearRight 0.14 0.057 0.0253 0.0068 Left-FrontRight 0.11 0.044 0.015 0

Left-RearLeft 0.394 0.23 0.17 0.12 Left-FrontLeft 0.145 0.06 0.03 0

時將資訊回授顯示之介面。除了我們使用的方式之外，NAO 官方仍然提供了很 多方法供使用者選擇，例如:NAO 官方自行開發的 AldebaranChoregraphe 圖形化 軟體平台、C++ SDK、Matlab SDK…等。

對於電腦端Python SDK 的程式要傳送馬達控制訊號給 NAO 機器人中使

Balance Walking Algorithm

對於我們提出之平衡行走控制，我們希望在用到實體機器人之前可以先行利 用電腦模擬的方式，測試控制系統的可行性，避免真實機器人因控制系統的錯誤 而導致真實機器人硬體受到傷害，因此在我們進行真實的NAO 測試之前，我們 先利用CyberBotics[33]所開發的 Webots for NAO 的模擬軟體，Webots for NAO 是Webots7 的特定版本專門模擬 NAO，此模擬軟體提供了虛擬世界讓使用者可 以使用控制器讓預定義模擬的NAO 進行運動，測試控制器的可行性和成功性。

在此模擬器中我們可以先行選擇所需利用版本的虛擬NAO，且加入所需的 環境、地形或物品，如:上下坡、球、箱子…等。如圖 4.8 所示，左邊為環境中所 有物件的資料，供我們從中改變物理性質(如:箱子的位置、大小、旋轉角度)，我 們在此選擇了型號為Nao_H21_V33 的虛擬 NAO，在圖中左邊上下兩小圖為 NAO 頭部兩個攝影機此時看出的環境狀態，由攝影機的環境得知我們在環境中透過箱 子的旋轉加入了上下坡等地形。

對於控制虛擬NAO 運動，我們一樣使用 Python SDK 來達成程式功能且輸 出馬達所應到達的角度輸出。對於Python SDK 的程式要傳送訊號給虛擬 NAO 中使NAO 運動，用法與圖 4.7 我們連結到真實 NAO 的方式完全相同，而我們在

圖 4.8 虛擬 NAO 的模擬環境

此所需輸入的IP 位置為目前使用的電腦的固定 IP 位置，達成連接到虛擬 NAO 機器人上的虛擬嵌入式軟體NAOqi 中，如圖 4.8 右下角的文字顯示，此虛擬 NAO 在啟動時也有將虛擬的NAOqi 控制器啟動，讓使用者進行使用。

在此模擬器中對於真實NAO 有裝載的感測器，它皆有進行數值模擬，當我 們點擊圖4.8 中虛擬 NAO 的圖片位置時，會跑出各種感測器目前模擬出的狀態 數值，如圖4.9 所示，在上方可以切換不同感測器目前所提供的模擬狀態數值，

如:加速規、攝影機、陀螺儀…等。而對於我們要用來應用的上半身姿態角度的 感測值，在模擬環境中透過NAOqi 的中對於陀螺儀和加速規的模擬數值的處理 之後的上半身姿態角度值，與我們設定虛擬NAO 目前上半身的姿態角度值是相 似的誤差極小，於是此模擬感測器的模擬數值可以讓我們用來進行平衡行走控制 的模擬驗證，且當我們一樣讓姿態為站立時，此模擬值與現實機器人NAO 的感 測值進行比較是符合且接近的，證明此模擬器對於感測器的數值模擬是符合真實 情況的；另外對於腳底的壓力感測器，此模擬器中依然有進行模擬，但是在機器 人行走時腳底對於地面的衝擊力影響，此模擬器中屬於理想的情況並非如真實世 界相同會產生很大的衝擊力，此性質讓我們可以在不考慮衝擊力的影響下進行其 他平衡效果的模擬與驗證，反之對於衝擊力的效果測試就會太過於理想化無法反 映真實環境狀況。那麼由於我們要從行走步態規劃開始進行設計的驗證，需要先 不考慮衝擊力的影響，因此我們將依然利用此模擬器來達到行走設計的驗證。

圖 4.9 虛擬 NAO 站立時之 Gyro 感測器數值模擬圖

第五章 模擬與實驗結果

的曲線設計(在 2.2 節已有詳細說明 4 個振盪器的用途)，因此有些將選擇不一樣

Description Symbol Value time constant 、 、 1/2.918  1/5.848

time constant ratio τ /τ 0.8 mutual inhibition γ 2.0 adaptation constant β 2.5

tonic excitation 2.43

scale gain of input h 1

initial internal variable 0 、 0 、 0 0 0.4432  0.9924 initial internal variable 0 、 0 、 0 0 0.1885  0.5834 initial internal variable 0 、 0 、 0 0 0.4334  -0.175 initial internal variable 0 、 0 、 0 0 0.69  0.3229

後腳跨到前腳初始位置上本來就有差距值，因此從1 出發當到達 0 時也就是與支

Symbol Simulation-Value Experimental-Value Non Feedback Feedback

(mm) 415.0  415.0 

(mm)  99.272  66.18325  52.6197848

  0.355  0.355  0.5 

上，當實驗為單純驗證步態規劃的可行性之下時間仍然為1 秒，因此左右腳各走

的參數值調整( ， )，目的為讓上半身的姿態能持續的保持與行走在平地時相 度和不要讓輸出曲線有過大的over-shoot 產生的情況進行參數的調整，由於模擬 和實驗中的感測資訊的大小範圍相同，因此模擬和實驗皆用同值；對於即時補償

Symbol Simulation-Value Experimental-Value

1.0  0.24 

0.0  0.3456

0.04  0.04

0.6  0.35

0.8  0.3

平衡行走於4°斜坡之模擬過程如圖 5.1(a)~(o)所示，此時對於地形偵測的情 況與補償控制器的作用效果說明如下:

行走時偵測到在Y 方向的旋轉角度 AngY，如圖 5.2 所示，此感測值 AngY 隨上半身的前後擺盪呈現擺盪曲線，從整體變化來看，在上坡時上半身有稍微的 往前傾；另外，在下坡時上半身的傾斜角度為向後傾斜，此感測值表示補償控制 器的作用使得上半身重心的移動行為，使得行走能持續保持平穩。

圖 5.1 平衡行走於 4°斜坡之模擬過程

圖 5.2 行走過程中之 Y 方向旋轉角度曲線

如圖5.3 顯示，預測補償控制器對於直接補償到關節 Hip-Pitch 的角度補償 值在每一步上的改變，從圖中可以明顯的看出當機器人面臨到地形的變化時都能 適時的進行上半身的修正，使得上半身能相對於水平地面持續保持直立，在平地 的預測補償值都在理想值0°附近；在上坡的預測補償值都在理想值 4°附近；在 輸出曲線受到輸入曲線改變的影響，使得上半身在走到上坡有多前傾(正值變大) 少後傾(負值變小)，且在下坡時多後傾少前傾的狀況發生，如圖 5.4 所示。

圖 5.3 直接補償於關節 Hip-Pitch 之預測補償角度值

圖 5.4 控制上半身前後擺盪的 CPG 的輸入與輸出之間的變化關係

如圖5.5 顯示，在即時補償控制器中，對於補償到關節 Hip-Pitch 的角度補 償值在每一步上的改變，為了克服雙腳支撐轉換成單腳支撐時的不平衡，而產生 行走曲線的偏移，在圖中可以看出當走在平地時前後補償值平均；在走到上坡時，

補償值會先將上半身重心往前(負值)旋轉傾斜，克服抬腳時的重心往後傾，而為 了使得曲線符合在落腳時上半身往後擺盪的曲線，所以每步的最後再把重心往後 (正值)旋轉傾斜；而走到下坡時為了使得上半身不會因為抬腳時的慣性衝力和重 心對於地形的分力影響往前傾，補償值會將上半身重心往後(正值)旋轉傾斜，達 到遵守行走曲線的行走。

圖 5.5 單腳支撐相時補償於關節 Hip-Pitch 之即時補償角度值

圖 5.6 行走過程中之 X 方向旋轉角度曲線

行走時所偵測到在X 方向的旋轉角度 AngX，如圖 5.6 所示，此感測值 AngX 隨上半身的左右擺盪呈現很規律的左右擺盪，由於抬腳時的重心影響和我們設計

行走時所偵測到在X 方向的旋轉角度 AngX，如圖 5.6 所示，此感測值 AngX 隨上半身的左右擺盪呈現很規律的左右擺盪，由於抬腳時的重心影響和我們設計