基于STM32的六足機器人運動控制系統研究

李俊敏

（南京理工大學紫金學院，江蘇 南京 210023）

摘 要：本文主要研究基于STM32的六足機器人運動控制系統，通過建立六足機器人的運動學模型，對擺動腿和支撐腿進行運動學分析，并按照六足機器人典型步態，進行步態運動規劃。同時，根據六足機器人的運動學和步態分析結果，設計了基于18個舵機協調運動的六足機器人運動控制系統，實現遠程監控和機器人的直線路徑、轉彎步態的規劃和控制等功能。

關鍵詞：六足機器人;運動控制系統;步態行走;舵機

中圖分類號：TP242 文獻標識碼：A 文章編號：1003-5168（2019）25-0032-03

Research on Motion Control System of Hexapod Robot Based on STM32

LI Junmin

（Nanjing University of Science and Technology Zijin College，Nanjing Jiangsu 210023）

Abstract： This paper mainly studied the hexapod robot motion control system based on STM32. By establishing the kinematics model of the hexapod robot， the kinematics analysis of the swing leg and the support leg was carried out， and the gait motion planning was carried out according to the typical gait of the hexapod robot. According to the kinematics analysis and gait analysis results of the hexapod robot， the hexapod robot motion control system based on the coordinated motion of 18 rudders was designed to realize the functions of remote monitoring and linear path and turning gait planning and control of the robot.

Keywords： hexapod robot;motion control system;gait walking;steering gear

隨著機器人技術及計算機科學的不斷發展，新興的特種機器人在生產生活中扮演著愈加重要的角色。然而，多足機器人在控制和制作方面都比輪式和履帶式機器人要復雜很多。由于多足機器人的運動軌跡是不規則的，可以快速適應各種困難的環境，因此，其能代替履帶式機器人和輪式機器人[1]。與其他種類的機器人相比，多足機器人對地形造成的破壞小很多，而且腿自由度相對較高，這極大地增強了機器人在各種地形下的運動能力。其可以通過多足自由快速地調整角度和長短來改變自身重心，適應地形，進而順利前進。

1 六足機器人運動學分析

本文研究的機器人由6只足和6條腿組裝而成。每個腿部關節處都有一個舵機，驅動舵機關節做旋轉運動。機器人采用三角步態的方式行走，如圖1所示。

將六足機器人的6只腳分為兩組，每組都呈一個三角形的結構分布，將1、3、5腳分為一組，組成一個三角形，剩下的2、4、6為一組。六足機器人在執行直線步態動作指令時，兩組三角步態交替執行支撐動作和前后擺動動作，可以控制六足機器人的前進或后退。

1.1 擺動腿運動學分析

根據連桿間坐標系建立原則，根關節、髖關節、膝關節及足端點建立坐標系[O0-x0-y0-z0]、[O1-x1-y1-z1]、[O2-x2-y2-z2]、[O3-x3-y3-z3]。為了方便計算，將坐標原點建立在特殊的點上，如舵機關節的中心或者是每條腿的端點上。[x]軸必須和向桿伸展的方向一致，[y]軸可以按照右手螺旋定則來判斷，[z]軸需要和各關節旋轉方向保持垂直。多足機器人單足坐標系如圖2所示。

其中，[0Ri1]、[1Ri2]、和[2Ri3]代表相應根關節到髖關節、髖關節到膝關節、膝關節到足端坐標系的旋轉矩陣方程。具體計算公式如式（1）至式（3）所示。在計算過程中，可以將公式中的[cosθ]當作[Cθ]，[sinθ]當作[Sθ]，其中[i]代表的是1至6足。

[0Ri1=Cθ1-Sθ20Sθ2Cθ20001]? ? ? ? ? ? ? ? ? ? ? ? （1）

[1Ri2=Cθ2-Sθ20Sθ2Cθ20001]? ? ? ? ? ? ? ? ? ? ? ? （2）

[2Ri3=Cθ3-Sθ30Sθ3Cθ30001]? ? ? ? ? ? ? ? ? ? ? ? ? （3）

足端點在根關節坐標系中的旋轉矩陣：

[0Ri3=0Ri11Ri22Ri3=Cθ1C（θ2+θ3）-Cθ1S（θ2+θ3）Sθ1Sθ1C（θ2+θ3）-Sθ1S（θ2+θ3）-Cθ1S（θ2+θ3）C（θ2+θ3）0]（4）

六足機器人每條腿上各相鄰坐標系間的D-H方程為：

[0Ti1=Cθ10Sθ1l1Cθ1Sθ10-Cθ1l1Sθ101000001]? ? ? ? ? ? ? ? ? ?（5）

[1Ti2=Cθ2-Sθ20l2Cθ2Sθ2Cθ20l2sθ200100001]? ? ? ? ? ? ? ? ? ?（6）

[2Ti3=Cθ3-Sθ30l3Cθ3Sθ3Cθ30l3sθ300100001]? ? ? ? ? ? ? ? ? ? （7）

[0TI3=Cθ1C（θ2+θ3）-Cθ1S（θ2+θ3）Sθ1l1+l2Cθ2+l3C（θ2+θ3）Cθ1Sθ1C（θ2+θ3）-Sθ1（θ2+θ3）-Cθ1l1+l2Cθ2+l3C（θ2+θ3）Sθ1S（θ2+θ3）Cθ2+θ30l2Sθ2+l2Sθ2+θ30001]（8）

六足機器人單腿足端點在根關節坐標系中的位置矢量為：

[0Pi3=0xi30yi30zi3=l1+l2Cθi2+l3C（θi2+θi3）Cθi1l1+l2Cθi2+l3C（θi2+θi3）Sθi1l2Sθi2+l3S（θi2+θi3）]? ? ? ? ? ? ? （9）

各腿足端點在機體坐標系中的變換矩陣為：

[cTi3=cTi00Ti3=C（β1+θ1）Cθ23-Cβ1+θ1Cθ23Sβ1+θ1Sβ1+θ1Cθ23-Sβ1+θ1C?23-Cβ1+θ1cPi3Sθ23Cθ2300001]? ? ? ?（10）

1.2 支撐腿運動學分析

六足機器人運動時，必須同時有三條以上的腿著地作為支撐腿，這樣才能保持機體穩定[6]。這時，支撐腿就和地面以及六足機器人主體之間處于一種可以隨時改變的并聯狀態。

機體和支撐腿構成并聯結構。為了方便，可以選擇將地面作為支撐腿與機體運動的參考坐標系。如圖3所示，將地面作為參考對象建立坐標軸。開始時，機體坐標系的位置與地面坐標系方向[OG-XGYGZG]一致，機體由支撐腿帶動。首先，機體坐標系[OC-XCYCZC]繞[ZG]轉動[α]°，然后繞[YG]轉動[β]°，最后繞[XG]轉動[γ]°，再平移到點[XC，YC，ZC]處，由此可得到矩陣[GTC]。

[GTc=cosαcosβcosαsinβsinγ-sinαcosγcosαsinβcosγ+sinαsinγGxcsinαcosβsinαsinβcosγ+cosαcosγsinαsinβcosγ-cosαsinγGyc-sinβcosβsinγcosβcosγGzc0001]? （11）

當式（11）中[α]、[β]、[γ]均為0時，機體中心的坐標為[GXC=0]，[GYC=0]，可以得到六足機器人支撐腿正運動學公式：

[Gxi3=cxi0-sβi（l1sθ1+l2sθ1cθ2+l3sθ1cθ23）+cβi（l1cθ1+l2cθ1cθ2+l3cθ1cθ23）Gyi3=cyi0+cβ（l1sθ1+l2sθ1cθ2+l3sθ1cθ23）+sβ（l1cθ1+l2cθ1cθ2+l3cθ1cθ23）Gzi3-48=l2sθ2+l3sθ23]（12）

逆運動學分析主要就是通過機體的位姿和足端點在地面參考坐標系中的坐標來求解機器人支撐腿關節轉角的角度。

2 六足機器人控制系統設計

2.1 硬件設計

六足機器人控制系統設計選擇嵌入式作為主處理系統。硬件包括STM32VCT6單片機控制板、電源管理模塊、驅動模塊、外圍傳感器模塊及六足機器人支撐架構和六足關節。硬件部分采用PCB板，制作出整個六足機器人。控制系統硬件部分的總體框架如圖4所示。

六足機器人控制系統中通過使用18路舵機實現其正常運動，分成6組，每個定時器包含四個通道。四個通道相互獨立，互不影響。由于定時器中能捕獲的通道是定時器1、2、3、4、5、8，產生對應舵機的PWM信號，因此用作舵機驅動，TIM6用作時基，通用定時器可以選擇不同的分頻系數，最大值不超過65 535，其脈沖寬度可以進行調整，以滿足不同項目的要求。脈沖寬度最小能達到微秒級別，最大可以調整為毫秒級別。

2.2 軟件設計

六足機器人控制系統軟件部分利用模塊化編寫程序的思想，將程序分為相應的功能模塊進行編寫。通過STM32實現六足機器人的三角步態行走，根據外圍傳感器調整姿態。圖5是六足機器人控制系統主程序流程。

2.3 調試與試驗

本文設計的六足機器人樣機如圖6所示。在安裝機器人腿部關節時，需要調整舵機處于中位以保證機器人前后足不會發生碰撞，并調整機器人各舵機的偏差，減少機器人舵機安裝時的誤差，以保證后期機器人調試順利進行。測試過程選擇的動作為500、1000、1500、2000、2500，把這些動作都添加保存，選擇一個適當的[T]值。設置完成后可以在線運行，就能看到舵機轉動的相應效果，保存動作，可以讓舵機重復運行。所設計的六足仿生機器人可實現12種仿生動作，3種仿生姿態，5種可調節速度。該仿生機器人既能通過PS2手柄和紅外控制其在多種復雜環境下完成必要的動作，實現所需功能，又能自主脫機運行仿生避障、桌面防跌等附加功能。

3 結語

本文通過對六足機器人擺腿和支腿進行運動學分析，優化了機器人的機身結構，并使用STM32對六足機器人的控制系統進行軟硬件設計，編寫相應的軟件程序，同時進行系統調試。該六足機器人可以較好地模仿并拓展六足機器人的運動方式，較輪式或履帶式機器人而言適應性更強。

參考文獻：

[1]Angeles J. Fundamentals of robotic mechanical systems： theory， methods， and algorithms[M].Berlin：Springer，2007.

[2]Collins C. Stiffness modeling and force distribution for the all-terrainhex-limbed extraterrestrialex[C]//ASME2007 enter national design engineering technical conferences & computers and information in engineer conference， September.2007.

[3]B.Iannotta.Creating Robots for Space Repairs[J]. Aerospace America，2005（5）：36-40.

[4]李程.六足機器人控制系統設計[D].秦皇島：燕山大學，2016.

[5]陳剛.六足步行機器人位姿控制及步態規劃研究[D].杭州：浙江大學，2014.

[6]劉德高.六足機器人運動控制系統設計與實現[D].哈爾濱：哈爾濱工業大學，2013.