球形全向移動自平衡機(jī)器人的建模與控制

王江華， 張 莉， 曾建學(xué)

(華北科技學(xué)院 電子信息工程學(xué)院，河北 三河 065201)

0 引 言

輪式機(jī)器人按照平衡性能分類，可分為靜態(tài)平衡機(jī)器人與動態(tài)平衡機(jī)器人兩種[1]。靜態(tài)平衡機(jī)器人一般具有3個以上的自由度，以保證機(jī)器人的靜態(tài)穩(wěn)定性，但由于自由度的增加，勢必會造成驅(qū)動冗余，使得運(yùn)動效率降低，同時由于機(jī)器人自由度的增加，使其對崎嶇路面的適應(yīng)性變差；相比之下，動態(tài)平衡機(jī)器人由于其自由度少于平面運(yùn)動的自由度，導(dǎo)致其不穩(wěn)定性增加，但隨之而來的是靈活的運(yùn)動特性和較高的運(yùn)動效率。由于動態(tài)平衡機(jī)器人機(jī)構(gòu)的獨(dú)特性，必須采用獨(dú)特的動態(tài)平衡原理才能實(shí)現(xiàn)正常運(yùn)動。

本文所研究的球形全向移動自平衡機(jī)器人作為輪式機(jī)器人中的一種，由于其采用具有完整約束的全向輪，使其可以實(shí)現(xiàn)橫向移動而不打滑，輕易在平面內(nèi)實(shí)現(xiàn)3個自由度運(yùn)動，具有全向移動的特點(diǎn)[2]。機(jī)器人采用一個球形輪作為驅(qū)動輪，外觀上只具有一個自由度，因而其運(yùn)動效率極高，理論上在任何路況中都能夠保證與路面等接觸。

該機(jī)器人理論原型為倒立擺，故也稱為“倒立擺機(jī)器人”，區(qū)別于傳統(tǒng)線性倒立擺，球形全向移動自平衡機(jī)器人將線性約束擴(kuò)展為空間約束，這就對控制系統(tǒng)的穩(wěn)定性與靈活性提出了更高的要求[3]。與傳統(tǒng)機(jī)器人相比，球形全向移動自平衡機(jī)器人具有以下特點(diǎn)：① 依靠單個球形輪作為支撐輪，能輕易實(shí)現(xiàn)360°全向運(yùn)動；② 可以實(shí)現(xiàn)0半徑轉(zhuǎn)彎；③ 依據(jù)輪式機(jī)器人對地面的連續(xù)接觸性，故適用于全地形路面。

從理論研究角度，該機(jī)器人系統(tǒng)具有多變量、多驅(qū)動量、多自由度的特點(diǎn)，是一個極為復(fù)雜的MIMO非線性系統(tǒng)，再加上欠驅(qū)動的系統(tǒng)特性，可以為如今先進(jìn)控制理論的驗(yàn)證和優(yōu)化提供理想實(shí)驗(yàn)平臺。對球形全向移動自平衡機(jī)器人的研究不僅對于提高機(jī)器人對復(fù)雜地形環(huán)境的適應(yīng)能力具有現(xiàn)實(shí)意義，還推動了先進(jìn)控制理論的研究應(yīng)用。

本文從球形全向移動自平衡機(jī)器人運(yùn)動原理著手，對機(jī)器人的逆Atlas球面運(yùn)動平臺(Inverse Atlas Spherical Motion Platform, IASMP)模型[5-6]進(jìn)行分析，推導(dǎo)機(jī)器人運(yùn)動控制方案；介紹機(jī)器人控制系統(tǒng)的設(shè)計(jì)和仿真模型的搭建,依據(jù)控制系統(tǒng)仿真模型，在Ballbot機(jī)器人上設(shè)計(jì)控制程序，以進(jìn)行實(shí)物驗(yàn)證，檢驗(yàn)機(jī)器人控制系統(tǒng)對實(shí)現(xiàn)機(jī)器人的直立控制與運(yùn)動控制的合理性與有效性。

1 基于IASMP模型的運(yùn)動學(xué)和動力學(xué)模型分析

由圖1可知，Ballbot系列機(jī)器人使用3個直流無刷減速電機(jī)驅(qū)動3個全向輪，以控制機(jī)器人的球形輪，進(jìn)而驅(qū)動機(jī)器人運(yùn)動[4]，這是一個典型的IASMP模型。研究將從IASMP模型出發(fā)，對機(jī)器人進(jìn)行運(yùn)動學(xué)和動力學(xué)模型分析。

圖1 機(jī)器人仿真設(shè)計(jì)圖

機(jī)器人在不自旋的情況下，其運(yùn)動可通過XOZ與YOZ兩個正交平面實(shí)現(xiàn)。定義圖1中YOZ平面中Y軸正向?yàn)闄C(jī)器人左向，定義XOZ平面中X軸正向?yàn)闄C(jī)器人前進(jìn)方向。圖2表示機(jī)器人全向輪與球形輪的位置關(guān)系，圖中各參數(shù)的含義如表1所示。

(a)

(b)

圖2 機(jī)器人坐標(biāo)系定義

表1 參數(shù)定義

機(jī)器人3個全向輪在機(jī)器人本體坐標(biāo)系中的位置向量為P，方向?yàn)橛勺鴺?biāo)原點(diǎn)指向全向輪與球面接觸點(diǎn)的方向[7]。實(shí)驗(yàn)機(jī)器人其全向輪與球面接觸點(diǎn)和球心的連線與Z軸組成的傾角φ=45°

(1)

(2)

機(jī)器人采用雙鋁全向輪能夠?qū)崿F(xiàn)360°運(yùn)動，具有旋轉(zhuǎn)和側(cè)身機(jī)動性。對全向輪速度進(jìn)行分解，分為沿驅(qū)動方向的速度vs和垂直于驅(qū)動方向的速度vf。其中vf是球形輪轉(zhuǎn)動所產(chǎn)生的速度，真正能夠改變球形輪運(yùn)動狀態(tài)的是驅(qū)動方向的速度vs。故引出驅(qū)動向量si，按照圖2所示的全向輪在機(jī)器人坐標(biāo)系的位置，可得到3個全向輪的驅(qū)動方向：

(3)

至此可推導(dǎo)出全向輪沿驅(qū)動方向的速度vsv：

(4)

由式(4)即可得到各全向輪驅(qū)動方向的實(shí)時轉(zhuǎn)速，對照圖2，可得出機(jī)器人向X、Y方向的實(shí)時速度。

當(dāng)輪子繞著Y軸旋轉(zhuǎn)時，表示機(jī)器人正在沿前后方向運(yùn)動，由圖2可得其驅(qū)動方向的速度為：

(5)

當(dāng)輪子繞著X軸旋轉(zhuǎn)時，表示機(jī)器人正在沿左右方向運(yùn)動，由圖2可得其驅(qū)動方向的速度為：

(6)

若考慮機(jī)器人的自旋，則其繞Z軸驅(qū)動方向的速度為：

(7)

由式(5)～(7)得出機(jī)器人X、Y、Z3個方向的速度合成之后所得到的3個全向輪的最終速度vi為：

(8)

由于機(jī)器人滾球輪X、Y方向的球面線速度即是機(jī)器人當(dāng)前的速度，故記：

(9)

則式(8)可改寫成：

(10)

若設(shè)全向輪旋轉(zhuǎn)角速度為ωi，由式(2)可推導(dǎo)出球形輪的角速度矢量Ω與全向輪旋轉(zhuǎn)角速度的關(guān)系[8]：

(11)

式中,J表示機(jī)器人結(jié)構(gòu)的雅可比矩陣，它將每個全向輪轉(zhuǎn)動的角速度映射到球體的角速度。解算之后的逆雅可比矩陣和雅可比矩陣定義如下：

(12)

式中，rb和rw分別表示滾球輪與全向輪的驅(qū)動半徑。由于仰角θ與天頂角φ之和是90°，即φ+θ=90°，由圖2可見，同時由此即可得到：sinθ=cosφ，cosθ=sinφ。故解算之后的逆雅可比矩陣和雅可比矩陣又可以改寫為：

(13)

故球體的角速度矢量Ω可由下式表出：

(14)

-rbΩ2i+rbΩ1j+0k=Vxi+Vyj+Vzk

(15)

故，對于機(jī)器人的速度V可表示如下：V=Vxi+Vyj+Vzk；其中3個分量：Vx,Vy,Vz在笛卡爾坐標(biāo)系下可表示如下：

(16)

由式(16)可得三個速度分量的關(guān)系：

(17)

利用式(17)對時間的導(dǎo)數(shù)，可得到基于IASMP模型的運(yùn)動學(xué)和動力學(xué)方程：

(18)

2 球形全向移動自平衡控制系統(tǒng)的設(shè)計(jì)

考慮到球形全向移動自平衡機(jī)器人與四軸飛控系統(tǒng)具有相似的特點(diǎn)，即非線性、強(qiáng)耦合、多變量等等[9]。因此，基于第1節(jié)對球形全向移動自平衡機(jī)器人的運(yùn)動學(xué)分析，將四軸飛控的控制思路應(yīng)用于機(jī)器人。

機(jī)器人控制部分，參考國外Pixhawk開源飛控，其旋翼控制部分主要采用串級PID控制技術(shù)，實(shí)現(xiàn)諸如誤差校正、姿態(tài)解算、姿態(tài)控制、位置控制等等，并取得了較好的控制效果。

四旋翼通過4個電動機(jī)實(shí)現(xiàn)空間6自由度的運(yùn)動，只需克服空氣阻力(可以忽略不計(jì))即可迅速改變機(jī)身姿態(tài)；而球形全向移動自平衡機(jī)器人則是通過3個直流無刷減速電動機(jī)，配合全向輪，通過無滑動的摩擦作用驅(qū)動球形輪，以實(shí)現(xiàn)機(jī)器人在平面上3自由度的運(yùn)動。相比之下，四旋翼對于控制器的實(shí)時性、精確性要求更加嚴(yán)格；而對于該機(jī)器人，由于其控制輸出需要通過靜摩擦進(jìn)行傳遞，期間必然存在一定的滯后性，對系統(tǒng)的實(shí)時性要求相對較低。基于以上對比分析，考慮通過串級PID技術(shù)，設(shè)計(jì)球形全向移動自平衡控制系統(tǒng)。

2.1 控制系統(tǒng)整體設(shè)計(jì)思路

基于對上一章機(jī)器人運(yùn)動原理的分析，機(jī)器人控制可通過XOZ與YOZ兩個正交平面實(shí)現(xiàn)。依據(jù)MPU6050運(yùn)動傳感器的軸向，以其水平X、Y軸對兩個平面建立兩套串級PID控制器，控制機(jī)器人的X、Y方向。根據(jù)運(yùn)動傳感器MPU6050的6軸數(shù)據(jù)，使用4元素法，通過卡爾曼濾波與互補(bǔ)融合，解算出3軸歐拉角輸入X、Y方向的PID控制器的輸出，依據(jù)式(10)，將兩個方向的PID控制器輸出，映射到機(jī)器人3個電機(jī)上，以驅(qū)動球形輪實(shí)現(xiàn)機(jī)器人直立控制與全向移動。

2.2 機(jī)器人姿態(tài)控制方案

串級PID控制技術(shù)作為對傳統(tǒng)適用于線性時不變系統(tǒng)的單級PID控制技術(shù)的改進(jìn)，通過內(nèi)外兩環(huán)并聯(lián)調(diào)節(jié)，大大提高了系統(tǒng)的抗干擾能力[10]。參考飛控控制思路，確定以球形全向移動自平衡機(jī)器人的角度環(huán)作為主控制回路，主回路設(shè)計(jì)角度PID調(diào)節(jié)器作為主調(diào)節(jié)器；角速度環(huán)作為副回路，設(shè)計(jì)角速度PID調(diào)節(jié)器作為副調(diào)節(jié)器。主回路使用由MPU6050運(yùn)動傳感器的6軸數(shù)據(jù)解算得到的3軸姿態(tài)角作為反饋信號，與設(shè)定值做差后作為主回路PID的輸入。主回路的角度PID輸出與角速度融合后直接作為副回路的輸入，副回路PID調(diào)節(jié)器輸出作用到執(zhí)行機(jī)構(gòu)[11]。

球形全向移動機(jī)器人串級PID控制系統(tǒng)框圖如3所示。由圖3可知，角度/角速度串級控制系統(tǒng)執(zhí)行順序是先執(zhí)行角度控制，再執(zhí)行角速度控制。為避免內(nèi)外環(huán)產(chǎn)生共振，也考慮到進(jìn)一步提高反應(yīng)速度和工作頻率，將系統(tǒng)角速度控制周期設(shè)為角度控制周期的一半，平均每5 ms角速度PID控制器進(jìn)行一次控制更新，每10 ms角度PID控制器進(jìn)行一次控制更新。如此設(shè)置，一來可以使得內(nèi)環(huán)角速度對干擾能夠進(jìn)行快速而有效抑制，二來也將外環(huán)的誤差對內(nèi)環(huán)的影響降到最低，進(jìn)一步增強(qiáng)了機(jī)器人系統(tǒng)的抗擾性。

2.3 機(jī)器人全向移動控制方案

對于運(yùn)動控制，就四旋翼控制系統(tǒng)而言主要包括4個量：油門與3軸姿態(tài)角。由于空中運(yùn)動的空間優(yōu)勢，使得其具有較大的可控制裕量，通過設(shè)定其姿態(tài)角，產(chǎn)生姿態(tài)偏差，在姿態(tài)控制器的調(diào)節(jié)下趨使其向某個特定方向運(yùn)動[12]。而對于Ballbot機(jī)器人，其偏角的角度需要考慮驅(qū)動輪的驅(qū)動力，驅(qū)動力受重力分力的限制，若不外加支撐，其可調(diào)裕度非常有限，稍微控制不好就會導(dǎo)致控制失效。另外由于機(jī)器人的速度是根據(jù)3個電動機(jī)編碼器解算得出的，該機(jī)器人又屬于動穩(wěn)定機(jī)器人，故其解算數(shù)值必然存在較大噪聲[13]。但由于機(jī)器人對于控制的實(shí)時性要求較低，故本文通過中值濾波與均值濾波相互配合實(shí)現(xiàn)對機(jī)器人運(yùn)動速度的測算。

圖3 角度/角速度串級PID原理框圖

基于此本文提出對已有的角度/角速度串級PID進(jìn)行改進(jìn)，在角度環(huán)外再加入速度環(huán)，將速度環(huán)的輸出進(jìn)行量程轉(zhuǎn)換與當(dāng)前反饋的姿態(tài)角進(jìn)行疊加，共同作為角度環(huán)的輸入，構(gòu)成3閉環(huán)控制系統(tǒng)。即將速度控制轉(zhuǎn)換成角度控制，通過直立環(huán)調(diào)節(jié)速度響應(yīng)，使得控制作用更加平滑[14]。

機(jī)器人的運(yùn)動控制框圖如圖4所示。

圖4 機(jī)器人運(yùn)動控制框圖

3 機(jī)器人控制系統(tǒng)仿真模型的設(shè)計(jì)

機(jī)器人控制主要由X、Y方向的兩個串級PID控制器組成，由于機(jī)器人結(jié)構(gòu)的對稱性，兩個控制器設(shè)計(jì)方法基本一致，僅3個電動機(jī)對應(yīng)X、Y兩個方向的轉(zhuǎn)向以及組合輸出的比例有所不同。故僅從X方向控制器仿真模型的設(shè)計(jì)出發(fā)展開介紹。

機(jī)器人控制系統(tǒng)仿真模型的設(shè)計(jì)主要包括兩部分：基于Matlab的SimMechanics工具箱設(shè)計(jì)機(jī)器人動力學(xué)仿真模型；機(jī)器人串級PID控制器Simulink仿真模型。

3.1 機(jī)器人動力學(xué)仿真模型

Matlab SimMechanics工具箱[15-16]主要用于多體動力機(jī)械系統(tǒng)及其控制的建模仿真。使用工具箱按照系統(tǒng)的運(yùn)動機(jī)理，設(shè)計(jì)出對應(yīng)的剛體、鉸鏈、約束以及外力，以此搭建系統(tǒng)的剛體仿真模型，在仿真時，使用SimMechanics可視化工具，可將機(jī)構(gòu)系統(tǒng)及其運(yùn)動轉(zhuǎn)化為直觀的動畫顯示。區(qū)別于其他機(jī)械系統(tǒng)仿真軟件，SimMechanics可以與Simulink結(jié)合，為控制系統(tǒng)的仿真設(shè)計(jì)提供了方便。

本文為簡化模型，將3個全向輪直接轉(zhuǎn)換為X、Y兩個方向的作用力，對球形全向移動機(jī)器人建立SimMechanics模型如圖5所示。

3.2 機(jī)器人控制系統(tǒng)仿真模型

機(jī)器人控制系統(tǒng)使用串級PID控制器進(jìn)行設(shè)計(jì)，控制器由速度、角度、角速度組成3閉環(huán)控系統(tǒng)，控制器輸出直接作用到機(jī)器人的動力學(xué)模型，得到各狀態(tài)變量的反饋值，并送往控制器反饋端進(jìn)行調(diào)節(jié)。機(jī)器人控制系統(tǒng)的Simulink仿真模型如圖6所示。

3.3 機(jī)器人運(yùn)動控制仿真

為驗(yàn)證機(jī)器人運(yùn)動控制的有效性，設(shè)計(jì)驗(yàn)證試驗(yàn)，設(shè)置機(jī)器人初始參數(shù)：[0,0,0,0,0]，即3個姿態(tài)角控制量均為0°，機(jī)器人兩個控制方向的速度給定值為0。隨后，給定X方向的速度設(shè)定值設(shè)定單個方波信號，觀察機(jī)器人姿態(tài)角的變化。

由仿真結(jié)果可知(見圖7),當(dāng)給定信號發(fā)出后，機(jī)器人速度環(huán)輸出引起機(jī)器人姿態(tài)變化，在姿態(tài)環(huán)的控制下，驅(qū)使機(jī)器人朝著X方向運(yùn)動。當(dāng)控制信號撤除后機(jī)器人能夠及時矯正姿態(tài)，保持直立狀態(tài)。由仿真結(jié)果可以看出,控制器能夠完成設(shè)定任務(wù)。

4 實(shí)物驗(yàn)證

為進(jìn)一步驗(yàn)證控制系統(tǒng)設(shè)計(jì)的合理性與有效性，本文根據(jù)控制器仿真模型，對Ballbot機(jī)器人設(shè)計(jì)實(shí)物驗(yàn)證。設(shè)計(jì)的機(jī)器人如圖8所示。

圖5 SimMechanics動力學(xué)模型圖

圖6 機(jī)器人控制系統(tǒng)仿真模型

圖7 運(yùn)動控制器產(chǎn)生的姿態(tài)變化曲線

圖8 Ballbot機(jī)器人實(shí)物圖

本文所設(shè)計(jì)的實(shí)驗(yàn)分為兩個部分，實(shí)驗(yàn)檢驗(yàn)機(jī)器人姿態(tài)控制和機(jī)器人運(yùn)動控制能力。

實(shí)驗(yàn)主要通過機(jī)器人機(jī)身運(yùn)動傳感器的3個姿態(tài)角(俯仰角Ax、翻滾角Ay、偏航角Az)反映機(jī)器人在靜止?fàn)顟B(tài)下的平衡情況以及運(yùn)動過程中的方向偏差情況；通過機(jī)器人的X、Y兩個方向的速度(Vx、Vy)反映機(jī)器人定點(diǎn)直立、速度配合效果。機(jī)器人實(shí)物測試數(shù)據(jù)如圖9所示。

圖9 姿態(tài)控制測試結(jié)果圖

在姿態(tài)控制器的控制下，機(jī)器人上電初始化后，直接進(jìn)入定點(diǎn)直立模式，不需要對機(jī)器人進(jìn)行任何額外設(shè)定。通過機(jī)器人3個姿態(tài)角的變化曲線以及機(jī)器人速度變化曲線可看出：機(jī)器人一直在原地調(diào)整姿態(tài)，各姿態(tài)角度控制在3°以內(nèi)，機(jī)器人的運(yùn)動速度一直在原點(diǎn)附近波動，同時機(jī)器人X-Y方向上的速度也在x=0附近調(diào)整，保證機(jī)器人可以在原點(diǎn)保持直立。據(jù)此可判定機(jī)器人能夠完成直立狀態(tài)下的姿態(tài)控制。

機(jī)器人運(yùn)動控制，主要通過測試機(jī)器人進(jìn)行直線運(yùn)動與圓周運(yùn)動來檢驗(yàn)機(jī)器人運(yùn)動方向的準(zhǔn)確性與速度控制的穩(wěn)定性。機(jī)器人運(yùn)動速度通過圖10可以看出在進(jìn)行直線運(yùn)動與圓周運(yùn)動時，機(jī)器人均能夠平穩(wěn)的調(diào)節(jié)運(yùn)動速度以達(dá)到控制目的。

圖10 運(yùn)動控制速度曲線圖

由圖11所示，在機(jī)器人直線運(yùn)動測試中，機(jī)器人Yaw軸的航向角變化曲線可知，機(jī)器人在運(yùn)動過程中，其航向角變化在3°以內(nèi)，可認(rèn)為機(jī)器人運(yùn)動方向穩(wěn)定，體現(xiàn)出速度控制的有效性。

圖11 機(jī)器人運(yùn)動方向曲線圖

5 結(jié) 語

本文重點(diǎn)研究球形全向移動自平衡機(jī)器人控制系統(tǒng)的設(shè)計(jì)?？刂葡到y(tǒng)的控制對象是一種以運(yùn)動高效性、靈活性為特點(diǎn)，并以單個球形輪作為運(yùn)動輪的全向移動機(jī)器人(Ballbot機(jī)器人)。實(shí)現(xiàn)關(guān)鍵在于設(shè)計(jì)以串級PID技術(shù)作為控制方式的控制器，以實(shí)現(xiàn)機(jī)器人的直立和全向移動。

本文從機(jī)器人的IASMP模型出發(fā)，對機(jī)器人進(jìn)行運(yùn)動學(xué)和動力學(xué)分析，由此得出機(jī)器人3個無刷減速直流電動機(jī)的組合控制方案；并在SimMechanics工具箱設(shè)計(jì)機(jī)器人動力學(xué)仿真模型的基礎(chǔ)上，設(shè)計(jì)機(jī)器人控制系統(tǒng)的Simulink模型進(jìn)行仿真；同時按照仿真模型的設(shè)計(jì)方法，在實(shí)驗(yàn)室Ballbot機(jī)器人上設(shè)計(jì)軟件控制器，實(shí)現(xiàn)了機(jī)器人定點(diǎn)直立與全向移動控制。

球形全向移動自平衡機(jī)器人控制系統(tǒng)具有多自由度、多驅(qū)動量、多傳感器的特點(diǎn)，是一個極為復(fù)雜的MIMO非線性系統(tǒng)，再加上欠驅(qū)動的系統(tǒng)特性，可以為如今的先進(jìn)控制理論的驗(yàn)證和優(yōu)化提供理想實(shí)驗(yàn)平臺。對球形全向移動自平衡機(jī)器人的研究不僅對于提高機(jī)器人對復(fù)雜地形環(huán)境的適應(yīng)能力具有現(xiàn)實(shí)意義，也推動了先進(jìn)控制理論的研究應(yīng)用。