程式控制流程圖如圖5.5 所示，開始對整個系統送上電源或是分別重新致能 dsPIC 微控制器後，dsPIC 微控制器立即進入系統初始化階段，隨即讓人型步行 機器人保持在開始步行的初始姿態，並設定步行軌跡的相關參數後。接著進入影 像追蹤系統的控制迴圈，讀取 PC 端送來的 RS-232 的控制訊號，若人型機器人 與目標物保持在預先設定的距離內，則機器人固定不移動，否則依據與目標物的 遠近來決定移動的方向。
程式開始
系統初始化
步行軌跡設定
讀取PC端的控制 訊號
判斷訊號
前進 固定不動 後退
機器人初始姿態
左轉 右轉
圖5.5 影像追蹤系統的主程式控制流程
軌跡設定：
人型步行機器人之平面步行是沿著已規劃好的軌跡前進，而軌跡規劃之前則 須先輸入步行的跨距及步行的週期，然後再進行軌跡規劃。有了步行的跨距，便 可得知在單腳支撐時腳掌的初始位置及終點位置，以及腰部的起始位置與終點位 置，再將起始位置及終點位置分成數段區間，並在每段區間的結束點給一適當的 位置作為軌跡點。而馬達的運動量便是從各個軌跡點利用反運動方程式求得，在 每段區間馬達的運動軌跡是沿著已計算好的擺線輪廓曲線前進。
步行軌跡設定的流程圖如圖5.6 所示，設定的步驟依序如下。首先設定步行 軌跡的相關參數，包括腰部前進的距離(
X )、足部步行的距離(
hX )以及步行的
f 週期（Ts
）。接著產生腰部的步行軌跡以及足部的步行軌跡，藉由給定的腰部 與足部軌跡利用解反運動學求出x − z
平面上 6 個馬達的旋轉位置。接著改變z
y
− 平面上 4 個馬達的旋轉位置如圖 5.7 所示，調整姿態讓系統重心落於支撐腳 足部範圍 (單腳支撐相模式)或落於兩腳之間(双腳支撐相模式)。設 定 步 行 距 離 及 步 行 週 期
解 反 運 動 學 產 生 腰 部 的 軌 跡
產 生 足 部 的 軌 跡
座 標 轉 換 重 心 偏 移
圖5.6 產生步行軌跡的程式控制流程
圖5.7 重心偏移 實驗結果：
1. 平面步行
人形步行機器人實際於平面步行之結果如圖5.8 及圖 5.9 所示。(a)～(e)為右 腳向前跨出的步行週期，(f)～(j)為左腳向前跨出的步行週期，每次的步行週期約 費時5 秒，其每次步幅約 2cm。
(a) (b) (c) (d) (e)
(f) (g) (h) (i) (j)
圖5.8 人形步行機器人於平面步行時之實驗結果(正視圖)
(a) (b) (c) (d) (e)
(f) (g) (h) (i) (j)
圖5.9 人形步行機器人於平面步行時之實驗結果(側視圖) 2. 原地旋轉
人形步行機器人實際於原地旋轉之結果如圖 5.10 及圖 5.11。(a)～(e)為一個 完整的旋轉週期，(f)～(j)為另一個旋轉週期，每次的旋轉週期會讓人型步行機器 人旋轉10 度，每個旋轉週期約費時 5 秒。
(a) (b) (c) (d) (e)
(f) (g) (h) (i) (j)
(f) (g) (h) (a) (j)
(a) (b) (c) (d) (e)
圖5.11 人形步行機器人原地旋轉之實驗結果(側視圖)
5.2.2 階梯步行
整個階梯步行的控制流程可分為遠、中、近三個階段。遠距離示意圖如圖 5.12 所示，起初人形步行機器人離階梯的距離很遠(20 公分以上)，先使用影像處 理辨識階梯地形並計算出階梯的階數，再利用立體視覺計算階梯與人形步行機器 人之間的距離。根據計算出階梯的距離以及人形步行機器人的步行距離，估算到 達階梯邊緣所需的步行數目。但由於人形步行機器人每次步行時都會有些許誤 差，為了避免步行時的誤差影響到人形步行機器人到達階梯的準確度，故讓人形 步行機器人與階梯保留一段 3~5 公分的特定距離(d)，作為人形步行機器人步行 至階梯時校正用。