[發明專利]四足機器人的步態規劃方法

有效

• 申請號: 201510368506.2
• 申請日: 2015-06-29
• 公開（公告）號: CN104986241B
• 公開（公告）日: 2018-04-24
• 發明（設計）人: 陳騰;郝延哲;宋勇;李貽斌
• 申請（專利權）人: 山東大學（威海）
• 主分類號: B62D57/032
• 分類號: B62D57/032
• 代理公司: 濟南圣達知識產權代理有限公司37221
• 代理人: 張勇

1.一種四足機器人的步態規劃方法，其特征是，包括如下步驟：

步驟(1)：機器人初始化；

步驟(2)：機器人接收上位機指令；將指令解碼，通過OLED液晶顯示屏顯示；

步驟(3)：機器人對上位機指令進行判斷并執行指令，判斷是舵機調試指令、步態變換指令、方向改變指令還是其他功能指令，所述其他功能指令包括自適應穩定性、拍照指令和自動避障指令；

如果是舵機調試指令，則開始單個舵機調試，判斷是否收到調試值，如果收到就開始步態實現，步態實現后返回步驟(2)；如果沒收到就結束；

如果是步態變換指令，則開始步態實現，步態實現后返回步驟(2)；

如果是方向改變指令，則調整運動學方程，開始步態實現，步態實現后返回步驟(2)；

如果是其他功能指令，則根據指令完成穩定性調整、拍照和自動避障功能，返回步驟(2)；

如果收到其他功能指令，首先判斷是自適應穩定性指令、拍照指令還是自動避障指令；

如果是自適應穩定性指令，則加速度計傳感器采集當前機器人軀體的傾角并和穩態時比較；計算偏差；步態實現；

如果是拍照指令，則網絡攝像頭拍照，將照片通過單片機MT7620內嵌的OpenWRT，上傳給單片機STM32，最終通過藍牙模塊上傳給上位機；

所述步態實現的具體步驟如下：

步驟(4-1)：建立DH坐標系，設定初始坐標點；

步驟(4-2)：建立運動學方程；求解逆運動學方程；

步驟(4-3)：設計運動軌跡；

步驟(4-4)：舵機關節映射；

步驟(4-5)：PWM輸出控制舵機實現步態；

所述步驟(4-1)的步驟為：

首先，利用D-H方法對機器人建立坐標系，以此來表示各關節變量；第一關節的坐標系為(X1,Y1,Z1)，第二關節的坐標系為(X2,Y2,Z2)，第三關節的坐標系為(X3,Y3,Z3)，機器人軀干的中心位置的坐標系為(X0,Y0,Z0)；

根據建立的D-H坐標系建立機器人D-H坐標參數表；

表1 機器人D-H坐標參數表

a：表示每條公垂線的長度；α：兩個相鄰兩坐標系z軸間的角度；

θ：繞當前關節坐標系z軸旋轉的角度；d:在當前關節坐標系z軸上兩條相鄰的公垂線之間的距離；關節1表示橫向髖關節；關節2表示縱向髖關節；關節3表示膝關節；a₀表示橫向髖關節和縱向髖關節之間的連桿長度；a₁表示縱向髖關節和膝關節之間的連桿長度；a₂表示膝關節關節和腿末端節之間的連桿長度；k的取值范圍是1～3；d_ik的參數含義是k和k-1坐標系中z軸上兩條相鄰的公垂線之間的距離；θ_i(k-1)的參數含義是z軸從k-1坐標系旋轉到k坐標系的角度；θ_i0的參數含義是需要求解的橫向髖關節轉動角度；θ_i1的參數含義是需要求解的縱向髖關節轉動角度；θ_i2的參數含義是需要求解的膝關節轉動角度；

所述步驟(4-2)的步驟為：

建立運動學方程

x_i＝-a₁s₁-a₂s₁₂+δl(1)

y_i＝a₀s₀+a₁s₀c₁+a₂s₀c₁₂+λw(2)

z_i＝-a₀c₀-a₁c₀c₁-a₂c₀c₁₂-h(3)

其中：

δ=1i=0,1-1i=2,3]]>

λ=1i=0,3-1i=1,2]]>

其中s₀表示sinθ₀，c₀表示cosθ₀，s₁表示sinθ₁，c₁表示cosθ₁,s₁₂表示sin(θ₁+θ₂)，c₁₂表示cos(θ₁+θ₂)；x_i表示第i坐標系中的x坐標，a₁表示縱向髖關節和膝關節之間的連桿長度，a₂表示膝關節關節和腿末端節之間的連桿長度，δ表示對應不同腿添加的標志位，為-1或1，l表示機器人機體的長度的一半；y_i表示第i坐標系中的y坐標；a₀表示橫向髖關節和縱向髖關節之間的連桿長度；a₁表示縱向髖關節和膝關節之間的連桿長度；a₂表示膝關節關節和腿末端節之間的連桿長度；λ表示對應不同腿添加的標志位，為-1或1；w表示機器人的寬度的一半；z_i表示第i坐標系中的z坐標，h表示機器人的基坐標和坐標系(X1,Y1,Z1)之間的高度；

求解得到逆運動學方程：

θi0=arctan(yi-λw-zi-h)---(4)]]>

θi2=(arccos(a12+a22-ξ22a1a2)-π).δ---(6)]]>

其中：

ξ=(a0+zi+hcosθi0)2+(xi-δl)2]]>

其中，

θ_i0表示第i個坐標系中橫向髖關節轉動的角度，y_i表示第i坐標系中的y坐標，λ表示對應不同腿添加的標志位，為-1或1，z_i表示第i坐標系中的z坐標，h表示機器人的基坐標和坐標系(X1,Y1,Z1)之間的高度；θ_i1表示第i個坐標系中縱向髖關節轉動的角度，表示一個綜合的公式；δ表示對應不同腿添加的標志位，為-1或1，x_i表示第i坐標系中的x坐標，l表示機器人機體的長度的一半；a₀表示橫向髖關節和縱向髖關節之間的連桿長度，z_i表示第i坐標系中的z坐標，h表示機器人的基坐標和坐標系1之間的高度；cosθ_i0表示第i個坐標系中橫向髖關節轉動角度的余弦值；θ_i2表示第i個坐標系中縱向膝關節轉動的角度，ξ表示一個綜合的公式，x_i表示第i坐標系中的x坐標；

(2)變形為：y_i-λw＝s₀(a₀+a₁c₁+a₂c₁₂) (7)

(3)變形為：z_i+h＝-c₀(a₀+a₁c₁+a₂c₁₂) (8)

聯立以上兩式得：

由(8)得：

(1)整理為：x_i-δl＝-a₁s₁-a₂s₁₂ (10)

(9)(10)兩式兩邊分別平方，并和得：

(xi-δl)2+(zi+hc0+a0)2=(-a1c1-a2c12)2+(-a1s1-a2s12)2---(11)]]>

化簡后得：

(xi-δl)2+(zi+hc0+a0)2=a12+a22+2a1a2(s1s12+c1c12)---(12)]]>

由三角函數公式化簡得：

(xi-δl)2+(zi+hc0+a0)2=a12+a22+2a1a2c2---(13)]]>

則：

θi2=(arccos(a12+a22-(a0+zi+hcosθi0)2-(xi-δl)22a1a2)-π).δ---(14)]]>

令：

則：

θi2=(arccos(a12+a22-ξ22a1a2)-π).δ---(16)]]>

求出θ_i0和θ_i2后求解θ_i1有多種方法，此處通過代入(1)求解整理后得：

θi1=(arccos(a12-a22+ξ22a1ξ)-arctan(xi-δl-a0-(zi+h)/cosθi0)).δ---(17)]]>

令：

則：

如果是自適應穩定性指令，通過加速度計傳感器測得機器人重力加速度可以得到角度信息，與之穩定態的加速度差值通過PID算法得到機器人的偏移量，單片機驅動舵機通過調節髖關節實現重心向原來方向的反作用力，使腿部的反作用力抵消外力的干擾，機器人身體的基準是不變的，根據當前偏移基準值應用增量或者位置PID算法都可以實現自動調整，考慮到位置式PID的算法需要不斷累加每次的偏移值，運算量較大，所以使用增量式PID算法；

通過超聲波傳感器得到當前機器人與前方物體的距離，當距離大于安全距離時機器人前行；當小于安全距離時，機器人頭部舵機向左或者右搖動，檢測該方向的障礙情況，當距離符合安全距離時記錄該方向標識位，并以此向該方向移動；

四足機器人包括：機器人的軀干，所述機器人的軀干前端的上方通過機器人頸部連接機器人頭部，所述機器人軀干的下方連接四條腿：第一、第二、第三和第四條腿；其中第一、第二條腿安裝在機器人軀干的前端，第三、第四條腿安裝在機器人軀干的后端；每條腿均包括從下而上依次連接的小腿、大腿、垂直桿和水平桿四個部分；所述小腿與大腿、大腿與垂直桿、垂直桿與水平桿之間均設有舵機；所述軀干上設有控制裝置，控制裝置通過控制舵機來控制機器人的平穩行走；

所述控制裝置包括：單片機STM32，所述單片機STM32分別與十四個舵機、加速度計傳感器和超聲波傳感器連接，所述單片機STM32還通過藍牙模塊與上位機連接，所述單片機STM32還通過單片機MT7620與網絡攝像頭連接，所述單片機STM32還通過穩壓電路與電源連接，所述單片機STM32還通過單片機MEGA644與OLED液晶顯示屏連接；

所述單片機STM32作為主控芯片是機器人控制的核心，通過定時器模塊產生20ms定時中斷作為機器人的運動周期，通過輸出PWM波控制舵機轉角完成機器人的步態，同時stm32通過采集加速度計反饋的值得到機器人的姿態，控制機器人的平衡，控制超聲波模塊的定時工作來采集機器人周圍的障礙物情況，同時通過串口和藍牙、mt7620連接來進行通信；所述電源，用于給單片機供電；所述穩壓電路用于提供5v、3.3v、6v不同的電壓分別給加速度計傳感器、超聲波傳感器、arm芯片和舵機供電；所述舵機用于驅動機器人進行步態行走和頭部搖動；所述網絡攝像頭，用于遠程監控，傳送機器人周圍的環境信息；所述單片機MT7620，用于掛載OpenWRT系統，將攝像頭的圖像通過wifi發送出；所述加速度計傳感器安裝在機器人的機體中心位置用于采集機器人實時的姿態；所述超聲波傳感器，安裝在機器人頭部正上方用于檢測機器人周圍是否有障礙物；所述單片機MEGA644，用于和stm32通信得到控制信息并解碼后通過液晶顯示；所述OLED液晶顯示屏，用于顯示機器人當前信息；所述藍牙模塊用于和andorid上位機間通信，傳輸指令和數據；所述上位機用于發送指令和接收機器人反饋的數據；

第一條腿中，小腿與大腿之間通過第一舵機連接，大腿和垂直桿之間通過第二舵機連接，垂直桿與水平桿之間通過第三舵機連接；

第二條腿中，小腿與大腿之間通過第四舵機連接，大腿和垂直桿之間通過第五舵機連接，垂直桿與水平桿之間通過第六舵機連接；

第三條腿中，小腿與大腿之間通過第七舵機連接，大腿和垂直桿之間通過第八舵機連接，垂直桿與水平桿之間通過第九舵機連接；

第四條腿中，小腿與大腿之間通過第十舵機連接，大腿和垂直桿之間通過第十一舵機連接，垂直桿與水平桿之間通過第十二舵機連接；

所述機器人頸部設有第十三舵機；所述機器人頭部設有第十四舵機；

所述第三舵機，也稱水平髖關節，仿照哺乳動物髖部關節的外擺內收功能，負責機器人平行于身體方向的垂直方向運動，是機器人的轉向運動時的驅動源，其旋轉量用θ₀表示；

所述第二舵機，也稱垂直髖關節，仿照哺乳動物的髖關節的前后收腿功能，負責機器人的平行于身體方向的運動，是整個機器人的承重關節，其旋轉量用θ₁表示；

所述第一舵機，也稱膝關節，所述膝關節仿照哺乳動物的膝關節的前后運動關節，負責驅動機器人腿部與地面接觸，是機器人的步態軌跡的最終呈現關節，其旋轉量用θ₂表示。