基于STM32的六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)研究

李俊敏

（南京理工大學(xué)紫金學(xué)院，江蘇 南京 210023）

摘 要：本文主要研究基于STM32的六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，通過建立六足機(jī)器人的運(yùn)動(dòng)學(xué)模型，對(duì)擺動(dòng)腿和支撐腿進(jìn)行運(yùn)動(dòng)學(xué)分析，并按照六足機(jī)器人典型步態(tài)，進(jìn)行步態(tài)運(yùn)動(dòng)規(guī)劃。同時(shí)，根據(jù)六足機(jī)器人的運(yùn)動(dòng)學(xué)和步態(tài)分析結(jié)果，設(shè)計(jì)了基于18個(gè)舵機(jī)協(xié)調(diào)運(yùn)動(dòng)的六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，實(shí)現(xiàn)遠(yuǎn)程監(jiān)控和機(jī)器人的直線路徑、轉(zhuǎn)彎步態(tài)的規(guī)劃和控制等功能。

關(guān)鍵詞：六足機(jī)器人;運(yùn)動(dòng)控制系統(tǒng);步態(tài)行走;舵機(jī)

中圖分類號(hào)：TP242 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：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

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

1 六足機(jī)器人運(yùn)動(dòng)學(xué)分析

本文研究的機(jī)器人由6只足和6條腿組裝而成。每個(gè)腿部關(guān)節(jié)處都有一個(gè)舵機(jī)，驅(qū)動(dòng)舵機(jī)關(guān)節(jié)做旋轉(zhuǎn)運(yùn)動(dòng)。機(jī)器人采用三角步態(tài)的方式行走，如圖1所示。

將六足機(jī)器人的6只腳分為兩組，每組都呈一個(gè)三角形的結(jié)構(gòu)分布，將1、3、5腳分為一組，組成一個(gè)三角形，剩下的2、4、6為一組。六足機(jī)器人在執(zhí)行直線步態(tài)動(dòng)作指令時(shí)，兩組三角步態(tài)交替執(zhí)行支撐動(dòng)作和前后擺動(dòng)動(dòng)作，可以控制六足機(jī)器人的前進(jìn)或后退。

1.1 擺動(dòng)腿運(yùn)動(dòng)學(xué)分析

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

其中，[0Ri1]、[1Ri2]、和[2Ri3]代表相應(yīng)根關(guān)節(jié)到髖關(guān)節(jié)、髖關(guān)節(jié)到膝關(guān)節(jié)、膝關(guān)節(jié)到足端坐標(biāo)系的旋轉(zhuǎn)矩陣方程。具體計(jì)算公式如式（1）至式（3）所示。在計(jì)算過程中，可以將公式中的[cosθ]當(dāng)作[Cθ]，[sinθ]當(dāng)作[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）

足端點(diǎn)在根關(guān)節(jié)坐標(biāo)系中的旋轉(zhuǎn)矩陣：

[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）

六足機(jī)器人每條腿上各相鄰坐標(biāo)系間的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）

六足機(jī)器人單腿足端點(diǎn)在根關(guān)節(jié)坐標(biāo)系中的位置矢量為：

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

各腿足端點(diǎn)在機(jī)體坐標(biāo)系中的變換矩陣為：

[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 支撐腿運(yùn)動(dòng)學(xué)分析

六足機(jī)器人運(yùn)動(dòng)時(shí)，必須同時(shí)有三條以上的腿著地作為支撐腿，這樣才能保持機(jī)體穩(wěn)定[6]。這時(shí)，支撐腿就和地面以及六足機(jī)器人主體之間處于一種可以隨時(shí)改變的并聯(lián)狀態(tài)。

機(jī)體和支撐腿構(gòu)成并聯(lián)結(jié)構(gòu)。為了方便，可以選擇將地面作為支撐腿與機(jī)體運(yùn)動(dòng)的參考坐標(biāo)系。如圖3所示，將地面作為參考對(duì)象建立坐標(biāo)軸。開始時(shí)，機(jī)體坐標(biāo)系的位置與地面坐標(biāo)系方向[OG-XGYGZG]一致，機(jī)體由支撐腿帶動(dòng)。首先，機(jī)體坐標(biāo)系[OC-XCYCZC]繞[ZG]轉(zhuǎn)動(dòng)[α]°，然后繞[YG]轉(zhuǎn)動(dòng)[β]°，最后繞[XG]轉(zhuǎn)動(dòng)[γ]°，再平移到點(diǎn)[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）

當(dāng)式（11）中[α]、[β]、[γ]均為0時(shí)，機(jī)體中心的坐標(biāo)為[GXC=0]，[GYC=0]，可以得到六足機(jī)器人支撐腿正運(yùn)動(dòng)學(xué)公式：

[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）

逆運(yùn)動(dòng)學(xué)分析主要就是通過機(jī)體的位姿和足端點(diǎn)在地面參考坐標(biāo)系中的坐標(biāo)來求解機(jī)器人支撐腿關(guān)節(jié)轉(zhuǎn)角的角度。

2 六足機(jī)器人控制系統(tǒng)設(shè)計(jì)

2.1 硬件設(shè)計(jì)

六足機(jī)器人控制系統(tǒng)設(shè)計(jì)選擇嵌入式作為主處理系統(tǒng)。硬件包括STM32VCT6單片機(jī)控制板、電源管理模塊、驅(qū)動(dòng)模塊、外圍傳感器模塊及六足機(jī)器人支撐架構(gòu)和六足關(guān)節(jié)。硬件部分采用PCB板，制作出整個(gè)六足機(jī)器人。控制系統(tǒng)硬件部分的總體框架如圖4所示。

六足機(jī)器人控制系統(tǒng)中通過使用18路舵機(jī)實(shí)現(xiàn)其正常運(yùn)動(dòng)，分成6組，每個(gè)定時(shí)器包含四個(gè)通道。四個(gè)通道相互獨(dú)立，互不影響。由于定時(shí)器中能捕獲的通道是定時(shí)器1、2、3、4、5、8，產(chǎn)生對(duì)應(yīng)舵機(jī)的PWM信號(hào)，因此用作舵機(jī)驅(qū)動(dòng)，TIM6用作時(shí)基，通用定時(shí)器可以選擇不同的分頻系數(shù)，最大值不超過65 535，其脈沖寬度可以進(jìn)行調(diào)整，以滿足不同項(xiàng)目的要求。脈沖寬度最小能達(dá)到微秒級(jí)別，最大可以調(diào)整為毫秒級(jí)別。

2.2 軟件設(shè)計(jì)

六足機(jī)器人控制系統(tǒng)軟件部分利用模塊化編寫程序的思想，將程序分為相應(yīng)的功能模塊進(jìn)行編寫。通過STM32實(shí)現(xiàn)六足機(jī)器人的三角步態(tài)行走，根據(jù)外圍傳感器調(diào)整姿態(tài)。圖5是六足機(jī)器人控制系統(tǒng)主程序流程。

2.3 調(diào)試與試驗(yàn)

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

3 結(jié)語

本文通過對(duì)六足機(jī)器人擺腿和支腿進(jìn)行運(yùn)動(dòng)學(xué)分析，優(yōu)化了機(jī)器人的機(jī)身結(jié)構(gòu)，并使用STM32對(duì)六足機(jī)器人的控制系統(tǒng)進(jìn)行軟硬件設(shè)計(jì)，編寫相應(yīng)的軟件程序，同時(shí)進(jìn)行系統(tǒng)調(diào)試。該六足機(jī)器人可以較好地模仿并拓展六足機(jī)器人的運(yùn)動(dòng)方式，較輪式或履帶式機(jī)器人而言適應(yīng)性更強(qiáng)。

參考文獻(xiàn)：

[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]李程.六足機(jī)器人控制系統(tǒng)設(shè)計(jì)[D].秦皇島：燕山大學(xué)，2016.

[5]陳剛.六足步行機(jī)器人位姿控制及步態(tài)規(guī)劃研究[D].杭州：浙江大學(xué)，2014.

[6]劉德高.六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2013.