一種移動(dòng)式服務(wù)機(jī)器人多軸協(xié)調(diào)運(yùn)動(dòng)控制系統(tǒng)

施成章

（上海贄匠智能科技有限公司研發(fā)部，上海201418）

按照國(guó)際機(jī)器人協(xié)會(huì)的定義，服務(wù)機(jī)器人是一種以半自主或全自主的方式操作，為人們提供各種幫助的機(jī)器人［1］。移動(dòng)式服務(wù)機(jī)器人具有可移動(dòng)性強(qiáng)、機(jī)械結(jié)構(gòu)實(shí)用便利、人機(jī)交互能力強(qiáng)、智能化程度高等特點(diǎn)，已遍布軍事、航空航天、工業(yè)物流等自動(dòng)化領(lǐng)域［2］，主要有履帶式結(jié)構(gòu)、足式結(jié)構(gòu)和輪式結(jié)構(gòu)［3］?；趹?yīng)用場(chǎng)景、機(jī)動(dòng)性和運(yùn)動(dòng)系統(tǒng)特性，可設(shè)計(jì)不同類型的移動(dòng)機(jī)器人［4］。4輪獨(dú)立驅(qū)動(dòng)原理移動(dòng)平臺(tái)的4個(gè)輪子都是驅(qū)動(dòng)輪，在平面內(nèi)的3個(gè)自由度可被控制在平面內(nèi)任何方向上運(yùn)動(dòng)［5］。

目前，輪式服務(wù)機(jī)器人的驅(qū)動(dòng)原理主要有兩種，一種是兩輪差動(dòng)驅(qū)動(dòng)原理，如圖1所示。圖中，v為機(jī)器人車身運(yùn)動(dòng)的速度；vL為機(jī)器人左前輪運(yùn)動(dòng)的速度；vR為機(jī)器人右前輪運(yùn)動(dòng)的速度。

圖1 兩輪差動(dòng)驅(qū)動(dòng)原理

機(jī)器人后輪為從動(dòng)輪，兩輪差動(dòng)驅(qū)動(dòng)方式的移動(dòng)平臺(tái)由兩個(gè)驅(qū)動(dòng)輪和若干個(gè)從動(dòng)輪組成，其中兩個(gè)驅(qū)動(dòng)輪的軸線平行，若兩個(gè)驅(qū)動(dòng)輪運(yùn)動(dòng)速度相同時(shí)，移動(dòng)式服務(wù)機(jī)器人做直線運(yùn)動(dòng)；若兩個(gè)驅(qū)動(dòng)輪運(yùn)動(dòng)速度不相同時(shí)，移動(dòng)式服務(wù)機(jī)器人進(jìn)行轉(zhuǎn)彎運(yùn)動(dòng)［6］。但雙輪差速機(jī)器人在目標(biāo)點(diǎn)橫向移動(dòng)有誤差時(shí)，調(diào)整效率則會(huì)較低［7］。另一種是4輪獨(dú)立驅(qū)動(dòng)原理，如圖2所示。

圖2 4輪獨(dú)立驅(qū)動(dòng)原理

4輪獨(dú)立驅(qū)動(dòng)原理移動(dòng)平臺(tái)的4個(gè)輪子都是驅(qū)動(dòng)輪，可在平面內(nèi)任何方向上運(yùn)動(dòng)，機(jī)器人改變直線運(yùn)動(dòng)方向時(shí)，車身可以不用改變方向，只需調(diào)節(jié)4個(gè)輪子的運(yùn)動(dòng)方向，且機(jī)器人可以原地轉(zhuǎn)向，轉(zhuǎn)彎半徑為零［5，8］。

本文移動(dòng)式服務(wù)機(jī)器人采用4輪獨(dú)立驅(qū)動(dòng)原理，每個(gè)輪子均有獨(dú)立的行進(jìn)、轉(zhuǎn)向機(jī)構(gòu)，實(shí)現(xiàn)移動(dòng)本體在平面內(nèi)的任意方向運(yùn)動(dòng)。這也意味著機(jī)器人能夠按照自己的尺度進(jìn)行轉(zhuǎn)向，甚至當(dāng)機(jī)器人前后都存在障礙物時(shí)，可通過橫向移動(dòng)進(jìn)行避障。

移動(dòng)式服務(wù)機(jī)器人整體架構(gòu)如圖3所示。機(jī)器人含電池的質(zhì)量為100 kg，在水平運(yùn)動(dòng)時(shí)最大承受載荷為150 kg，最大運(yùn)行速度為1.2 m/s，能在25°坡面上運(yùn)行，實(shí)現(xiàn)零半徑轉(zhuǎn)彎，越過80 mm的障礙物和60 mm深的水面。

圖3 移動(dòng)式服務(wù)機(jī)器人整體架構(gòu)

1 運(yùn)動(dòng)理論模型構(gòu)建

1.1 運(yùn)動(dòng)坐標(biāo)系建立

對(duì)移動(dòng)式服務(wù)機(jī)器人的平面運(yùn)動(dòng)作如下假設(shè)：移動(dòng)式服務(wù)機(jī)器人為剛性結(jié)構(gòu)，運(yùn)行在一個(gè)理想的平面上；4個(gè)驅(qū)動(dòng)輪對(duì)稱分布在同一平面；不考慮機(jī)器人、輪子之間的關(guān)聯(lián)和局部自由度；驅(qū)動(dòng)輪與地面之間點(diǎn)接觸，忽略厚度；驅(qū)動(dòng)輪在地面只有純滾動(dòng)，不產(chǎn)生打滑現(xiàn)象。圖4為移動(dòng)式服務(wù)機(jī)器人運(yùn)動(dòng)學(xué)模型。圖中，XGOGYG為平面慣性參考坐標(biāo)系；XRORYR為局部參考坐標(biāo)系；α為機(jī)器人輪子朝向與局部參考坐標(biāo)系的角度差；θ為局部參考坐標(biāo)系與平面慣性參考坐標(biāo)系的角度差。

圖4 移動(dòng)式服務(wù)機(jī)器人運(yùn)動(dòng)學(xué)模型

式中：R(θ)為旋轉(zhuǎn)矩陣，

1.2 機(jī)器人運(yùn)動(dòng)學(xué)模型建立

為完成移動(dòng)式服務(wù)機(jī)器人在平面內(nèi)的運(yùn)動(dòng)控制，建立運(yùn)動(dòng)學(xué)模型［9］?？紤]旋轉(zhuǎn)運(yùn)動(dòng)，以機(jī)器人的幾何中心作為原點(diǎn)，建立局部參考坐標(biāo)系模型，如圖5所示。圖中，L、W分別為機(jī)器人的長(zhǎng)和寬；OC（xC，yC）為機(jī)器人的旋轉(zhuǎn)中心；r為機(jī)器人旋轉(zhuǎn)中心OC與機(jī)器人底盤幾何中心點(diǎn)的距離；αi(i=1，2，3，4)為4個(gè)車輪的前進(jìn)方向與X軸的夾角；ωR為機(jī)器人的旋轉(zhuǎn)角速度；ωi(i=1，2，3，4)為各車輪的轉(zhuǎn)速。移動(dòng)式服務(wù)機(jī)器人在旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，每一個(gè)輪子的軸線延長(zhǎng)線均指向機(jī)器人的旋轉(zhuǎn)中心OC，延長(zhǎng)線ri(i=1，2，3，4)為輪子的旋轉(zhuǎn)半徑。

圖5 局部參考坐標(biāo)系模型

以O(shè)（xC，yC）為移動(dòng)式服務(wù)機(jī)器人的旋轉(zhuǎn)中心，有

每個(gè)輪子的偏轉(zhuǎn)角可表示為

每個(gè)輪子的轉(zhuǎn)速可表示為

式中：R為輪子的半徑。

每個(gè)輪子的旋轉(zhuǎn)半徑可表示為

當(dāng)ωR=0時(shí)，移動(dòng)式服務(wù)機(jī)器人進(jìn)行平行移動(dòng)，運(yùn)動(dòng)軌跡始終為一條直線，各個(gè)輪子的轉(zhuǎn)速、偏轉(zhuǎn)角相同，輪子的偏轉(zhuǎn)方向就是移動(dòng)式服務(wù)機(jī)器人的運(yùn)動(dòng)方向，此時(shí)移動(dòng)式服務(wù)機(jī)器人的偏轉(zhuǎn)角、轉(zhuǎn)速為

基于式（3）~式（7）可計(jì)算得出每個(gè)輪子的偏轉(zhuǎn)角、轉(zhuǎn)速，通過控制轉(zhuǎn)向電動(dòng)機(jī)、輪轂電動(dòng)機(jī)可實(shí)現(xiàn)機(jī)器人預(yù)期的運(yùn)動(dòng)模式。

2 運(yùn)動(dòng)控制系統(tǒng)總體設(shè)計(jì)方案

移動(dòng)式服務(wù)機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的組成框圖如圖6所示，其主要功能是解析運(yùn)動(dòng)狀態(tài)指令，實(shí)現(xiàn)各個(gè)輪子的轉(zhuǎn)速、偏轉(zhuǎn)角的分配，從而達(dá)到預(yù)期的運(yùn)動(dòng)狀態(tài)。主控制器模塊的程序流程如圖7所示。主控制器模塊的任務(wù)是接收上位機(jī)的運(yùn)動(dòng)狀態(tài)，并計(jì)算出4個(gè)轉(zhuǎn)向電動(dòng)機(jī)的偏轉(zhuǎn)角度和4個(gè)輪轂電動(dòng)機(jī)的轉(zhuǎn)動(dòng)速度，發(fā)送給對(duì)應(yīng)的驅(qū)動(dòng)器。驅(qū)動(dòng)執(zhí)行模塊的程序流程如圖8所示。當(dāng)從CAN總線接收到一個(gè)運(yùn)動(dòng)數(shù)據(jù)信號(hào)后，電機(jī)驅(qū)動(dòng)器將該數(shù)據(jù)經(jīng)電動(dòng)機(jī)控制程序控制執(zhí)行電機(jī)的轉(zhuǎn)動(dòng)，驅(qū)動(dòng)電動(dòng)機(jī)模塊無需主控制器模塊的參與就能滿足控制要求，并且每一個(gè)驅(qū)動(dòng)執(zhí)行模塊接收到運(yùn)動(dòng)數(shù)據(jù)信號(hào)，均能夠在同一時(shí)間控制各自對(duì)應(yīng)的執(zhí)行電機(jī)，且控制精度較高。

圖6 移動(dòng)式服務(wù)機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的組成框圖

圖7 主控制器模塊程序流

圖8 驅(qū)動(dòng)執(zhí)行模塊程序流程

本系統(tǒng)選用STM32F4系列芯片作為主控單元。與STM32F1和STM32F3系列相比，STM 32F4系列微控制器不僅包含單周期數(shù)字信號(hào)處理器指令，還集成了專門用于浮點(diǎn)運(yùn)算的處理器［10］。STM32F407IGT6存儲(chǔ)器的閃存多達(dá)1 MB，并提供3個(gè)模/數(shù)轉(zhuǎn)換器、2個(gè)數(shù)/模轉(zhuǎn)換器、10個(gè)通用型定時(shí)器、低功耗的實(shí)時(shí)時(shí)鐘、隨機(jī)數(shù)發(fā)生器［11］。

轉(zhuǎn)向電動(dòng)機(jī)是實(shí)現(xiàn)機(jī)器人轉(zhuǎn)向的部件，控制簡(jiǎn)單且具有較高的轉(zhuǎn)向精度。目前，廣泛應(yīng)用于測(cè)量轉(zhuǎn)角的電機(jī)有閉環(huán)步進(jìn)電動(dòng)機(jī)、伺服電動(dòng)機(jī)［12］，步進(jìn)電動(dòng)機(jī)的轉(zhuǎn)速快慢取決于指令的脈沖頻率的高低，而旋轉(zhuǎn)角度大小取決于給出的脈沖數(shù)量［13］，且輸出功率較小，完全可以作為轉(zhuǎn)向機(jī)構(gòu)。

編碼器的工作方式分為增量式、絕對(duì)式［14］。為了測(cè)量轉(zhuǎn)向電動(dòng)機(jī)的轉(zhuǎn)動(dòng)角度，本文選取單圈數(shù)字絕對(duì)型編碼器MAB25，該編碼器具有很強(qiáng)的抗震性能，且價(jià)格便宜［15］。

3 實(shí)驗(yàn)與結(jié)果分析

3.1 轉(zhuǎn)速控制實(shí)驗(yàn)

根據(jù)移動(dòng)式服務(wù)機(jī)器人的相關(guān)參數(shù)，取L=825 mm、W=630 mm，經(jīng)大量的整機(jī)運(yùn)行效果對(duì)比實(shí)驗(yàn)，選擇較高的初始前進(jìn)速度v0=1 m/s，較低的初始前進(jìn)速度v0=0.5 m/s進(jìn)行轉(zhuǎn)速控制實(shí)驗(yàn)。針對(duì)4個(gè)車輪的轉(zhuǎn)速控制精度表現(xiàn)情況進(jìn)行實(shí)驗(yàn)，車輪的實(shí)際轉(zhuǎn)速和參考轉(zhuǎn)速的變化曲線如圖9~圖12所示。

圖9 前左輪轉(zhuǎn)速隨車速和轉(zhuǎn)向角度的變化

圖10 前右輪轉(zhuǎn)速隨車速和轉(zhuǎn)向角度的變化

圖11 后左輪轉(zhuǎn)速隨車速和轉(zhuǎn)向角度的變化

圖12 后右輪轉(zhuǎn)速隨車速和轉(zhuǎn)向角度的變化

由圖9~圖12可知，當(dāng)機(jī)器人剛開始運(yùn)動(dòng)時(shí)，每個(gè)車輪的速度和整體的運(yùn)動(dòng)速度大致相同，隨著轉(zhuǎn)向角φ在-45°~+45°范圍內(nèi)變化時(shí)，v1、v3隨著轉(zhuǎn)向角的增加逐漸減小，v2、v4則相反。φ為負(fù)表示機(jī)器人向右轉(zhuǎn)，左邊車輪轉(zhuǎn)速大于右邊車輪轉(zhuǎn)速；同理φ為正表示機(jī)器人向左轉(zhuǎn)，右邊車輪轉(zhuǎn)速大于左邊車輪轉(zhuǎn)速，因此符合實(shí)際情況。當(dāng)轉(zhuǎn)向角φ=0°時(shí)，4輪轉(zhuǎn)速保持一致，小車沿直線運(yùn)動(dòng)，小車輪子轉(zhuǎn)速一致。轉(zhuǎn)向角從-45°的激勵(lì)信號(hào)開始，每個(gè)車輪的轉(zhuǎn)速響應(yīng)曲線出現(xiàn)了明顯振蕩，每個(gè)轉(zhuǎn)向角激勵(lì)信號(hào)采樣周期為10 ms，直至-30°左右的激勵(lì)信號(hào)時(shí)，控制系統(tǒng)基本達(dá)到穩(wěn)態(tài)。同時(shí)在初始速度v0=0.5 m/s時(shí)的振蕩幅度比v0=1 m/s低，初始速度v0=0.5 m/s時(shí)達(dá)到穩(wěn)態(tài)的速度比v0=1 m/s時(shí)更快，由于激勵(lì)信號(hào)每隔10 ms變化，因此開始運(yùn)行階段誤差較大，經(jīng)過調(diào)節(jié)，轉(zhuǎn)速響應(yīng)曲線逐漸平穩(wěn)，轉(zhuǎn)速更加穩(wěn)定。

3.2 定位誤差實(shí)驗(yàn)

為使移動(dòng)式服務(wù)機(jī)器人定位精確、誤差小，對(duì)其進(jìn)行定位誤差實(shí)驗(yàn)。實(shí)驗(yàn)分為直線運(yùn)動(dòng)模式誤差實(shí)驗(yàn)和綜合運(yùn)動(dòng)模式誤差實(shí)驗(yàn)。

3.2.1 直線運(yùn)動(dòng)模式誤差實(shí)驗(yàn) 運(yùn)行路程分別為1、6和12 m時(shí)，以不同的速度運(yùn)行，記錄實(shí)際停止點(diǎn)與目標(biāo)點(diǎn)之間的間距，即為系統(tǒng)的定位誤差。每一種路程分別有8種運(yùn)動(dòng)速度，每一種運(yùn)動(dòng)速度都重復(fù)測(cè)量10次。表1為直線運(yùn)行時(shí)的誤差測(cè)試結(jié)果。由表1可知，當(dāng)移動(dòng)式服務(wù)機(jī)器人以不同的運(yùn)動(dòng)速度，運(yùn)行不同的行駛距離時(shí)，會(huì)存在一定的定位誤差，且運(yùn)動(dòng)速度越大，其定位誤差呈逐漸增加的趨勢(shì)。定位誤差未超過1.5 cm，滿足設(shè)計(jì)指標(biāo)。

表1 直線運(yùn)行誤差測(cè)試結(jié)果

3.2.2 綜合運(yùn)動(dòng)模式誤差實(shí)驗(yàn) 移動(dòng)式服務(wù)機(jī)器人綜合運(yùn)動(dòng)模式的運(yùn)動(dòng)路徑，既有直線運(yùn)動(dòng)又有旋轉(zhuǎn)運(yùn)動(dòng)，整個(gè)路徑長(zhǎng)度為15.14 m。表2為綜合運(yùn)動(dòng)模式的誤差測(cè)試結(jié)果。由表可知，移動(dòng)式服務(wù)機(jī)器人在規(guī)定的運(yùn)動(dòng)路徑上以不同的運(yùn)動(dòng)速度行駛時(shí)，運(yùn)動(dòng)速度越大，其定位誤差也越大，定位誤差不超過1.5 cm，滿足設(shè)計(jì)指標(biāo)。

表2 綜合運(yùn)動(dòng)模式時(shí)運(yùn)行誤差測(cè)試結(jié)果

綜合以上兩種定位誤差實(shí)驗(yàn)可知，移動(dòng)式服務(wù)機(jī)器人以不同運(yùn)動(dòng)速度在不同的運(yùn)動(dòng)模式下都會(huì)產(chǎn)生定位誤差，但定位誤差都小于1.5 cm，且機(jī)器人運(yùn)動(dòng)較為穩(wěn)定，未出現(xiàn)抖動(dòng)現(xiàn)象，滿足設(shè)計(jì)要求。

3.3 運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn)

為檢驗(yàn)移動(dòng)式服務(wù)機(jī)器人的有效性，對(duì)其進(jìn)行多次重復(fù)運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn)，主要分為直線運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn)、旋轉(zhuǎn)運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn)和綜合運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn)。

3.3.1 直線運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn) 移動(dòng)式服務(wù)機(jī)器人在直線運(yùn)動(dòng)時(shí)，運(yùn)行路程分別為1、3和5 m，將運(yùn)動(dòng)速度設(shè)置為0.6 m/s，每種運(yùn)動(dòng)路徑各測(cè)6次。直線運(yùn)動(dòng)軌跡誤差測(cè)試結(jié)果如表3所示?？梢钥闯鲋本€運(yùn)行軌跡誤差較小，直線運(yùn)動(dòng)軌跡的最大測(cè)試誤差、平均測(cè)試誤差與運(yùn)行路程關(guān)系不大，但由于各種外界干擾，直線運(yùn)動(dòng)軌跡誤差控制在2 cm以內(nèi)，直線運(yùn)動(dòng)良好。

表3 直線運(yùn)動(dòng)軌跡誤差測(cè)試

3.3.2 旋轉(zhuǎn)運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn) 移動(dòng)式服務(wù)機(jī)器人在旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，線速度設(shè)為0.6 m/s，分別在半徑為0.5、1.0和1.5 m做旋轉(zhuǎn)運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn)，每種運(yùn)動(dòng)路徑各測(cè)6次。旋轉(zhuǎn)運(yùn)動(dòng)軌跡誤差測(cè)試結(jié)果如表4所示。

表4 旋轉(zhuǎn)運(yùn)動(dòng)軌跡誤差測(cè)試

由表4可知，相對(duì)于直線運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn)，旋轉(zhuǎn)運(yùn)動(dòng)軌跡的最大測(cè)試誤差、平均測(cè)試誤差均比直線運(yùn)動(dòng)軌跡誤差大。由于慣性的原因，當(dāng)轉(zhuǎn)彎半徑增大時(shí)，旋轉(zhuǎn)運(yùn)動(dòng)軌跡的最大測(cè)試誤差和平均測(cè)試誤差都減小，旋轉(zhuǎn)運(yùn)動(dòng)軌跡誤差控制在3 cm以內(nèi)。

3.3.3 綜合運(yùn)動(dòng)軌跡誤差實(shí)驗(yàn) 移動(dòng)式服務(wù)機(jī)器人綜合運(yùn)動(dòng)的速度設(shè)為0.6 m/s，在長(zhǎng)為3.5 m、寬為2.5 m及4個(gè)圓角半角為0.5 m的路徑上運(yùn)動(dòng)，運(yùn)動(dòng)圈數(shù)分別為1、3、5圈，每種圈數(shù)各測(cè)試5次。綜合運(yùn)動(dòng)軌跡誤差測(cè)試結(jié)果如表5所示。

表5 綜合運(yùn)動(dòng)軌跡誤差測(cè)試

選取運(yùn)動(dòng)圈數(shù)為1圈的一組數(shù)據(jù)繪制綜合運(yùn)動(dòng)軌跡圖，如圖13所示。移動(dòng)式服務(wù)機(jī)器人綜合運(yùn)動(dòng)最大測(cè)試誤差為2.98 cm，平均測(cè)試誤差為2.03 cm，滿足設(shè)計(jì)要求。由圖13可見，移動(dòng)式服務(wù)機(jī)器人在直線運(yùn)動(dòng)時(shí)，運(yùn)行效果較好，僅存在較小的抖動(dòng)，而在旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，抖動(dòng)較多，但基本滿足設(shè)計(jì)要求。

圖13 綜合運(yùn)動(dòng)軌跡圖

4 結(jié) 論

本文根據(jù)移動(dòng)式服務(wù)機(jī)器人的設(shè)計(jì)要求，設(shè)計(jì)了一種輪式結(jié)構(gòu)的移動(dòng)式服務(wù)機(jī)器人及其運(yùn)動(dòng)控制系統(tǒng)，并對(duì)運(yùn)動(dòng)控制系統(tǒng)及測(cè)試數(shù)據(jù)進(jìn)行了實(shí)驗(yàn)驗(yàn)證和分析。移動(dòng)式服務(wù)機(jī)器人的轉(zhuǎn)速控制實(shí)驗(yàn)及運(yùn)動(dòng)可靠性實(shí)驗(yàn)表明，機(jī)器人的轉(zhuǎn)速誤差不斷減小且轉(zhuǎn)速平穩(wěn)，具有一定保護(hù)自身安全和性能的能力，達(dá)到了預(yù)期的設(shè)計(jì)要求。