兩輪機(jī)器人的舞步運(yùn)動(dòng)規(guī)劃和仿真研究

孫 亮，呼德夫

SUN Liang,HU De-fu

（北京工業(yè)大學(xué) 人工智能與機(jī)器人研究所，北京 100124）

0 引言

兩輪自平衡機(jī)器人屬于輪式機(jī)器人的范疇，這種機(jī)器人兩輪共軸，獨(dú)立驅(qū)動(dòng)，作為差動(dòng)驅(qū)動(dòng)式，通過運(yùn)動(dòng)控制保持平衡，可直立行走。它結(jié)構(gòu)簡單、運(yùn)動(dòng)靈活，適于在狹小和危險(xiǎn)的空間內(nèi)工作，具有較好的機(jī)動(dòng)性。兩輪自平衡機(jī)器人屬于非線性、時(shí)變、欠驅(qū)動(dòng)、非完整約束系統(tǒng) 。目前，兩輪機(jī)器人的自平衡控制方面的研究較多，常用DSP或單片機(jī)作為主控制器，陀螺儀和加速度計(jì)采集機(jī)器人姿態(tài)和傾角數(shù)據(jù)，同時(shí)電機(jī)編碼器采集車輪轉(zhuǎn)速數(shù)據(jù)，將數(shù)據(jù)傳到主控制器，通過卡爾曼濾波數(shù)據(jù)融合和PID算法來進(jìn)行機(jī)器人車輪轉(zhuǎn)速控制，從而實(shí)現(xiàn)自平衡控制效果。兩輪機(jī)器人在實(shí)現(xiàn)自平衡的前提下可以完成多種動(dòng)作。若機(jī)器人能夠響應(yīng)舞曲優(yōu)美的韻律，完成諸如華爾茲這樣的特定舞步動(dòng)作，則會(huì)最大程度地體現(xiàn)出兩輪機(jī)器人動(dòng)態(tài)性能好、機(jī)動(dòng)性強(qiáng)的特點(diǎn)。本文基于兩輪機(jī)器人的基本運(yùn)動(dòng)方式，提出機(jī)器人完成簡單舞步動(dòng)作的運(yùn)動(dòng)規(guī)劃，給出了基于有限狀態(tài)自動(dòng)機(jī)的運(yùn)動(dòng)規(guī)劃描述方法，并用基于廣義雅可比矩陣的兩輪機(jī)器人分解運(yùn)動(dòng)速度控制算法 進(jìn)行了仿真實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明算法具有良好的準(zhǔn)確性與可靠性。

1 運(yùn)動(dòng)學(xué)模型

兩輪移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型如圖1所示。其中主要有：C為機(jī)器人車身前端一點(diǎn)；O為機(jī)器人平臺(tái)質(zhì)心；b為兩車輪間距；r為車輪半徑；lc為點(diǎn)C與質(zhì)心O點(diǎn)之間距離；φ為機(jī)器人車身傾角；θl和θr分別為左右輪轉(zhuǎn)角。

圖1 兩輪移動(dòng)機(jī)器人幾何模型

J為兩輪機(jī)器人的廣義雅克比矩陣，反映機(jī)器人上點(diǎn)C速度與車輪轉(zhuǎn)速之間的關(guān)系[2]。

2 基于有限狀態(tài)自動(dòng)機(jī)的運(yùn)動(dòng)規(guī)劃描述

為了規(guī)劃兩輪機(jī)器人的運(yùn)動(dòng)軌跡，使其按照華爾茲舞步軌跡運(yùn)動(dòng)，可以用有限狀態(tài)自動(dòng)機(jī)來描述其狀態(tài)變化情況。有限狀態(tài)自動(dòng)機(jī)（Finite State Automata,FSA）定義為一個(gè)五元變量組[3]，即：

其中，T為對(duì)應(yīng)于基元的符號(hào)集合，Q為機(jī)器狀態(tài)集合，δ為映射關(guān)系集合，q0為機(jī)器的初始狀態(tài)，q0∈Q， F為終端狀態(tài)集合，F(xiàn)∈Q。

兩輪機(jī)器人的符號(hào)集合為：

其中，f表示機(jī)器人前進(jìn)（forward），b表示機(jī)器人后退（backward），r表示右輪以左輪為軸轉(zhuǎn)動(dòng)，即轉(zhuǎn)動(dòng)輪為右輪，同理l表示左輪以右輪為軸轉(zhuǎn)動(dòng)，即轉(zhuǎn)動(dòng)輪為左輪，s表示機(jī)器人無半徑自旋轉(zhuǎn)，c表示在俯視視角中機(jī)器人順時(shí)針（clockwise）轉(zhuǎn)動(dòng)，即向右轉(zhuǎn)，a表示在俯視視角中機(jī)器人逆時(shí)針（anti-clockwise）轉(zhuǎn)動(dòng)，即向左轉(zhuǎn)，45等數(shù)字表示其相應(yīng)轉(zhuǎn)動(dòng)的角度，所以ra45表示機(jī)器人右輪以左輪為軸逆時(shí)針轉(zhuǎn)動(dòng)45°，即機(jī)器人左轉(zhuǎn)45°。其他的符號(hào)以此類推。

機(jī)器人狀態(tài)集合，即機(jī)器人車身朝向狀態(tài)集合為Q={F,B,L,R,FL,FR,BL,BR}

其中，F(xiàn)表示向前方向（Forword），B表示向后方向（Backward），L表示向左方向（Left），R表示向右方向（Right），F(xiàn)L表示左前方向，F(xiàn)R表示右前方向，BL表示左后方向，BR表示右后方向，如圖2所示。

圖2 兩輪機(jī)器人舞步運(yùn)動(dòng)方向

在映射關(guān)系δ=(A,a)=B中：A表示轉(zhuǎn)移源狀態(tài)，為狀態(tài)Q的元素；a表示轉(zhuǎn)移要素或輸入信號(hào)，即符號(hào)集合T中的元素：B表示轉(zhuǎn)移終狀態(tài)，也為狀態(tài)Q的元素。

機(jī)器人的所有狀態(tài)遵循以下映射關(guān)系：

上式(5)中，qi表示第i時(shí)刻機(jī)器人的朝向狀態(tài)，當(dāng)它收到raa符號(hào)指令（即機(jī)器人右輪以左輪為軸逆時(shí)針轉(zhuǎn)動(dòng)α0）后逆時(shí)針轉(zhuǎn)α0，狀態(tài)變?yōu)閝i+1，表示為qi+α。其他的映射關(guān)系以此類推，如式(7)-(9)所示。

圖3 人的連續(xù)左轉(zhuǎn)舞步示意圖

下面以較為典型的華爾茲舞步——連續(xù)左轉(zhuǎn)為例，給出相應(yīng)的有限狀態(tài)自動(dòng)機(jī)描述方式。如圖3所示，人的連續(xù)左轉(zhuǎn)90°舞步由前進(jìn)左轉(zhuǎn)90度和后退左轉(zhuǎn)90度組成，共可以分解為六個(gè)步伐。以男士步伐為例，在圖3中以向上方向?yàn)橄蚯埃?/p>

1）邁左腳前進(jìn)；

2）再邁右腳，同時(shí)身體向左側(cè)身，開始左轉(zhuǎn)；

3）收回左腳，兩腿并攏，身體朝左，完成前進(jìn)左轉(zhuǎn)90°；

4）退右腳后退；

5）再退左腳，同時(shí)身體向左側(cè)身，開始左轉(zhuǎn)；

6）收回右腳，兩腿并攏，身體朝后，完成后退左轉(zhuǎn)90°。

同理，如圖4所示，可以得到機(jī)器人的模擬舞步運(yùn)動(dòng)示意圖，分別模擬得到上述6個(gè)步伐：

1）左右輪勻速前進(jìn)；

2）右輪繞左輪左轉(zhuǎn)（前進(jìn)）45°；

3）左輪繞右輪左轉(zhuǎn)（后退）45°，完成前進(jìn)左轉(zhuǎn)90°；

4）左右輪勻速后退；

5）左輪繞右輪左轉(zhuǎn)（后退）45°；

6）右輪繞左輪左轉(zhuǎn)（前進(jìn)）45°，完成后退左轉(zhuǎn)90°。

圖4 兩輪機(jī)器人模擬的連續(xù)左轉(zhuǎn)舞步示意圖

根據(jù)以上六個(gè)步伐分解，應(yīng)用基于有限狀態(tài)自動(dòng)機(jī)的舞步規(guī)劃方法得到連續(xù)左轉(zhuǎn)90度舞步的相應(yīng)集合為：

圖5 機(jī)器人連續(xù)左轉(zhuǎn)舞步的狀態(tài)轉(zhuǎn)移圖

4）繪制狀態(tài)轉(zhuǎn)移圖。根據(jù)步驟3）中的映射關(guān)系，得到機(jī)器人舞步的狀態(tài)轉(zhuǎn)移圖，如圖5所示。

以此類推，可以得到機(jī)器人模仿華爾茲舞步的所有映射關(guān)系及其基于FSA的表達(dá)方式。

3 分解運(yùn)動(dòng)速度控制及仿真實(shí)驗(yàn)

由式(4)可知，由機(jī)器人上一點(diǎn)C的速度也可以求出車輪轉(zhuǎn)速。此時(shí)車輪控制量如下：

采用計(jì)算機(jī)控制，該方程變?yōu)殡x散式。速度項(xiàng)可以表示為單位時(shí)間里（控制周期Δt）的位移增量， 即：

圖6 兩輪機(jī)器人的期望運(yùn)動(dòng)軌跡

圖7 兩輪機(jī)器人運(yùn)動(dòng)軌跡仿真結(jié)果

式中：Δp為一個(gè)控制周期內(nèi)的位移變化；ΔΦ為同一時(shí)間間隔內(nèi)的角度變化。如果給定C點(diǎn)的軌跡，當(dāng)Δt選定時(shí)，可以確定Δp，將其代人上式求出各個(gè)角度增量ΔΦ，可得到各個(gè)角度的給定值，最后由角度伺服控制系統(tǒng)去實(shí)現(xiàn)位置控制。差動(dòng)驅(qū)動(dòng)式移動(dòng)機(jī)器人的分解運(yùn)動(dòng)速度控制是一種基于逆廣義雅可比矩陣的連續(xù)路徑跟蹤控制方法。該算法要考慮合理的控制周期，通過控制每個(gè)周期內(nèi)的角度輸出量實(shí)現(xiàn)對(duì)軌跡的跟蹤或速度控制。

為了驗(yàn)證本文提出的兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃算法的正確性和可行性，在MATLAB平臺(tái)上做華爾茲基本舞步-左轉(zhuǎn)90°的模擬仿真實(shí)驗(yàn)。為了能使機(jī)器人按照指定的軌跡運(yùn)動(dòng)并移到指定的位置，可以讓機(jī)器人跟蹤某種特定的軌跡[4]。機(jī)器人的參數(shù)取：b=1.5,lc=0.5,r= 1.3。

初始位置為(xc0,yc0,φ0)=(20,0,90o)， 終止位置(xc1,yc1,φ1)=(0,43,180o)，為了實(shí)現(xiàn)機(jī)器人車身左轉(zhuǎn)，使C點(diǎn)跟蹤近似軌跡為y=40。圖6給出了機(jī)器人平臺(tái)上點(diǎn)C的期望運(yùn)動(dòng)軌跡，圖7給出了兩輪機(jī)器人運(yùn)動(dòng)過程仿真結(jié)果。從仿真結(jié)果可以看出，控制算法使兩輪機(jī)器人驅(qū)動(dòng)兩車輪轉(zhuǎn)動(dòng)可以實(shí)現(xiàn)機(jī)器人車身上一點(diǎn)跟蹤預(yù)定軌跡，完成了基本舞步左轉(zhuǎn)90°的位置與方向角的相應(yīng)改變。

4 結(jié)束語

本文用有限狀態(tài)自動(dòng)機(jī)描述了兩輪移動(dòng)機(jī)器人的舞步運(yùn)動(dòng)規(guī)劃。有限狀態(tài)自動(dòng)機(jī)是具有有限狀態(tài)的離散輸入輸出的系統(tǒng)數(shù)學(xué)模型。華爾茲基本舞步動(dòng)作是由若干個(gè)基本動(dòng)作元素組合而成的，且這些元素都是較為固定的運(yùn)動(dòng)模式。所以可以用有限狀態(tài)自動(dòng)機(jī)規(guī)范地描述兩輪機(jī)器人舞步的運(yùn)動(dòng)規(guī)劃。同時(shí)本文運(yùn)用基于廣義雅克比矩陣的分解運(yùn)動(dòng)速度控制算法來實(shí)現(xiàn)機(jī)器人對(duì)軌跡的跟蹤和速度控制，并用MATLAB驗(yàn)證了該算法的準(zhǔn)確性。

[1] 王建俊.兩輪同軸機(jī)器人的自平衡控制研究[J].數(shù)字技術(shù)與應(yīng)用,2010,7：105-106.

[2] 柳長安,李國棟,劉春陽.差動(dòng)驅(qū)動(dòng)式移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)劃[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2003,35(9)：1095-1097.

[3] 孫亮,禹晶.模式識(shí)別原理[M].北京：北京工業(yè)大學(xué)出版社,2009,2：54-56.

[4] Yoshio Yamamoto,Xiaoping Yun.Coordinating Locomotion and Manipulation of a Mobile Manipulator. IEEE Transactions on Automatic Control,1994,39(6)：1326-1332.

[5] SERJI H.A unified approach to motion control of mobile manipulators[J].The International Journal of Robotics research,1998,17(2)：10-15.