基于Codesys的兩輪機(jī)器人控制研究

管觀洋，陳廣鋒，席 偉

（東華大學(xué) 機(jī)械工程學(xué)院，上海 201620）

兩輪機(jī)器人是一種差動(dòng)輪式移動(dòng)機(jī)器人，其平衡是依靠?jī)蓚€(gè)驅(qū)動(dòng)輪的移動(dòng)來(lái)實(shí)現(xiàn)的，故體積小、結(jié)構(gòu)簡(jiǎn)單、運(yùn)動(dòng)靈活，適于在狹小和危險(xiǎn)的空間內(nèi)工作[1]。兩輪機(jī)器人屬于非線性、時(shí)變、欠驅(qū)動(dòng)、非完整約束系統(tǒng)，控制問(wèn)題是其研究的關(guān)鍵[2]。

兩輪機(jī)器人以樹(shù)莓派為系統(tǒng)控制中心，樹(shù)莓派的功能和擴(kuò)展性強(qiáng)，可很大程度上縮短開(kāi)發(fā)周期。采用牛頓法建立機(jī)器人數(shù)學(xué)模型，有利于其控制策略的研究。在離散的卡爾曼濾波基礎(chǔ)上，提出了利用3σ原則去除明顯的機(jī)器人傾角雜質(zhì)數(shù)據(jù)，獲得精準(zhǔn)度高的傾角值。提出了將機(jī)器人的控制細(xì)化為平衡控制、運(yùn)動(dòng)控制[3]和差速控制的方法，將線性的PD平衡控制器非線性化，提高機(jī)器人平衡效果。機(jī)器人控制代碼在Codesys上完成，Codesys是近年來(lái)新興的工控開(kāi)發(fā)平臺(tái)，它不受限于產(chǎn)品種類，兼容性強(qiáng)。

1 兩輪機(jī)器人數(shù)學(xué)模型

1.1 兩輪機(jī)器人硬件平臺(tái)

圖1為兩輪機(jī)器人實(shí)物圖，整體尺寸為長(zhǎng)22 cm，寬 10 cm，高 22 cm。

圖1 兩輪機(jī)器人實(shí)物Fig.1 Two-wheel robot physical picture

兩輪機(jī)器人部件主要包括Raspberry Pi（樹(shù)莓派）、MPU6050、電源、降壓模塊、直流電機(jī)及驅(qū)動(dòng)、編碼器和車輪。樹(shù)莓派作為機(jī)器人控制中心，它是基于ARM Cortex-A53的微型電腦，集成了WiFi模塊，有40個(gè)GPIO，可用于各種控制系統(tǒng)的開(kāi)發(fā)。

1.2 電機(jī)模型

兩輪機(jī)器人建模時(shí)，采用牛頓法分別對(duì)機(jī)器人的電機(jī)、車輪、車身進(jìn)行建模，再綜合三者建立系統(tǒng)的數(shù)學(xué)模型[4-7]。兩輪機(jī)器人采用直流電機(jī)作為驅(qū)動(dòng)，其數(shù)學(xué)模型如下：

式中：ua為電機(jī)輸入電壓；ia為電機(jī)電流；R為電機(jī)等效電阻；e為電機(jī)反電動(dòng)勢(shì)；Tm為電機(jī)電磁轉(zhuǎn)矩；Td為電機(jī)負(fù)載轉(zhuǎn)矩；Jm為電機(jī)軸等效轉(zhuǎn)動(dòng)慣量；Km為電機(jī)轉(zhuǎn)矩系數(shù)；Ke為反電動(dòng)勢(shì)系數(shù)；ωm為電機(jī)轉(zhuǎn)速。

1.3 車輪模型

圖2為車輪受力分析圖，車輪的運(yùn)動(dòng)可分解為直線運(yùn)動(dòng)和繞輪軸的轉(zhuǎn)動(dòng)，以右輪為例，其動(dòng)力學(xué)方程為

式中：FVR，F(xiàn)HR為車身對(duì)右輪作用力的豎直分量和水平分量；fR為地面摩擦力；TR為右電機(jī)電磁轉(zhuǎn)矩；ωR為右輪角速度；vR為右輪前進(jìn)速度；m為車輪質(zhì)量；J為電機(jī)和車輪機(jī)械旋轉(zhuǎn)部分的轉(zhuǎn)動(dòng)慣量；r為車輪半徑。

圖2 右輪受力分析Fig.2 Right wheel force analysis

1.4 車身模型

車身的運(yùn)動(dòng)亦可分解成直線運(yùn)動(dòng)和轉(zhuǎn)向，圖3為車身受力分析的右視圖和俯視圖。

圖3 車身受力分析Fig.3 Robot body force analysis

車身的直線運(yùn)動(dòng)又可細(xì)分為質(zhì)點(diǎn)的平移和繞質(zhì)點(diǎn)旋轉(zhuǎn)，直線運(yùn)動(dòng)動(dòng)力學(xué)方程為

式中：mp為車身質(zhì)量；l為車身質(zhì)心P與兩車輪中心的距離；Jp為車身繞車輪連線的轉(zhuǎn)動(dòng)慣量；θ為車身傾角；T 為車輪施加在車身的扭矩；（xo，zo）、（xp，zp）為車輪連線中心和車身質(zhì)心在全局坐標(biāo)系中的坐標(biāo)。

據(jù)圖3俯視圖，轉(zhuǎn)向動(dòng)力學(xué)方程如下：

式中：Ip為車身繞Z軸的轉(zhuǎn)動(dòng)慣量；ω為機(jī)器人轉(zhuǎn)向速度；D為兩車輪間距；v為機(jī)器人前進(jìn)速度。

1.5 兩輪機(jī)器人系統(tǒng)模型

假定兩輪機(jī)器人在水平地面上運(yùn)動(dòng)且車輪純滾動(dòng)、無(wú)滑動(dòng)，此時(shí)vR=ωRr成立。由前面分析可知v=（vL+vR）/2，T=TR+TL，F(xiàn)H=FHL+FHR，F(xiàn)H=FHL+FHR。 結(jié)合式（1）～式（3）可得兩輪機(jī)器人系統(tǒng)模型如下：

式中：a=Jp/（mpl）+l；b=m+J/r2+mp/2；c=m+J/r2+mp/2。

2 兩輪機(jī)器人的多環(huán)PID控制

兩輪機(jī)器人的平衡運(yùn)動(dòng)控制可以細(xì)分為平衡控制環(huán)、運(yùn)動(dòng)控制環(huán)和差速控制環(huán)。平衡控制環(huán)即系統(tǒng)根據(jù)自身的傾角變化計(jì)算平衡控制量ub；運(yùn)動(dòng)控制環(huán)即根據(jù)機(jī)器人實(shí)際速度與期望速度的差計(jì)算運(yùn)動(dòng)控制量uv；差速控制環(huán)即根據(jù)兩輪速度差來(lái)重新分配左右電機(jī)電壓，保證機(jī)器人航向角精度。

2.1 平衡控制環(huán)

兩輪機(jī)器人平衡控制前需獲取其姿態(tài)信息，本文采用集成了加速度計(jì)和陀螺儀的MPU6050芯片。由于加速度計(jì)存在累積誤差，陀螺儀存在零點(diǎn)漂移，本文使用Kalman濾波將兩者數(shù)據(jù)融合處理，使兩者數(shù)據(jù)互補(bǔ)，而獲得準(zhǔn)確度高的傾角值。

考慮到測(cè)量干擾的影響，對(duì)Kalman增益Kt進(jìn)行如式（8）的處理，可去除傾角數(shù)據(jù)中明顯的雜質(zhì)數(shù)據(jù)。圖4為傾角均值濾波和Kalman濾波比較圖。

圖4 傾角均值濾波和卡爾曼濾波后比較Fig.4 Comparison of mean filter and Kalman filter of angle

得到傾角后，采用PD算法計(jì)算平衡控制量ub，但傳統(tǒng)線性PD控制器裕量小，魯棒性低，在平衡點(diǎn)處會(huì)產(chǎn)生自激振蕩，將其非線性化以克服上述不足：

參數(shù)的選擇可據(jù)系統(tǒng)模型來(lái)選取。

2.2 運(yùn)動(dòng)控制環(huán)

兩輪機(jī)器人的運(yùn)動(dòng)是基于控制行進(jìn)速度vψ和行進(jìn)方向ψ實(shí)現(xiàn)的，它們與左輪轉(zhuǎn)速φ˙L，右輪轉(zhuǎn)速φ˙R存在以下約束關(guān)系：

式中：Dm為兩車輪間距；Rw為車輪半徑。

若用ev表示真實(shí)前進(jìn)速度vψ與期望前進(jìn)速度vD的差，運(yùn)動(dòng)控制量uv為

2.3 差速控制環(huán)

兩輪機(jī)器人轉(zhuǎn)彎靠左右輪的速度差來(lái)控制的，其控制框圖如圖5所示。

圖5 機(jī)器人差動(dòng)控制框圖Fig.5 Robot differential control block diagram

從圖中可知，平衡控制電壓ub與運(yùn)動(dòng)控制電壓uv相加后，分配到左右電機(jī)。分配情況由差動(dòng)系數(shù)σψ決定。差動(dòng)系數(shù)σψ由期望前進(jìn)角度ψ或ψ˙計(jì)算得到，取值［0，1］。機(jī)器人實(shí)際運(yùn)動(dòng)時(shí)，受地面、系統(tǒng)本身的影響，會(huì)偏離期望的軌跡，故需要式（12）所示的PID反饋環(huán)節(jié)來(lái)進(jìn)行調(diào)節(jié)。

式中：vd=（1-σψ）vL-σψvR。

3 兩輪機(jī)器人的Codesys軟件開(kāi)發(fā)與試驗(yàn)測(cè)試

兩輪機(jī)器人控制軟件是在Codesys平臺(tái)上的完成的。Codesys做為工業(yè)控制軟件支持國(guó)際標(biāo)準(zhǔn)IEC61131-3，擴(kuò)展性和兼容性很強(qiáng)，可以極大提高用戶和廠商的開(kāi)發(fā)效率。

3.1 機(jī)器人軟件開(kāi)發(fā)

圖6為兩輪機(jī)器人運(yùn)動(dòng)執(zhí)行框圖，機(jī)器人軟件開(kāi)發(fā)需要在Codesys平臺(tái)上完成軟件功能板塊中的內(nèi)容。

圖6 機(jī)器人運(yùn)動(dòng)執(zhí)行框圖Fig.6 Robot motion block diagram

軟件完成后，通過(guò)WiFi方式將程序下載到樹(shù)莓派中運(yùn)行。機(jī)器人的運(yùn)行狀態(tài)可通過(guò)Codesys在線監(jiān)控，也可由電腦或手機(jī)打開(kāi)網(wǎng)頁(yè)（http：//樹(shù)莓派IP：8080webvisu.htm）監(jiān)控，網(wǎng)頁(yè)將顯示軟件控制界面如圖7所示。

圖7 機(jī)器人軟件界面Fig.7 Robot software interface

3.2 機(jī)器人試驗(yàn)測(cè)試

3.2.1 姿態(tài)平衡控制試驗(yàn)

兩輪機(jī)器人平衡控制指其啟動(dòng)時(shí)傾角不為0，啟動(dòng)后需恢復(fù)直立并保持平衡。平衡控制試驗(yàn)的傾角變化如圖8所示。

圖8 平衡控制傾角曲線Fig.8 Angle curve of balance control

從圖中可看出機(jī)器人從15°恢復(fù)直立用時(shí)約為0.8 s，之后傾角幾乎能穩(wěn)定在0°周圍，驗(yàn)證了該機(jī)器人有很好的平衡控制性能。

3.2.2 抗干擾試驗(yàn)

兩輪機(jī)器人抗干擾能力是其運(yùn)動(dòng)性能的重要指標(biāo)，干擾試驗(yàn)時(shí)傾角變化如圖9所示。

圖9 干擾試驗(yàn)傾角曲線Fig.9 Angle curve of interference experiment

由上圖可看出，機(jī)器人受干擾后恢復(fù)了平衡，用時(shí)約1 s，恢復(fù)時(shí)間非常短，驗(yàn)證了該機(jī)器人的抗干擾效果好。

3.2.3 速度控制試驗(yàn)

設(shè)置期望速度為0.5 m/s，差動(dòng)系數(shù)σψ為0.4，每隔2 s對(duì)左右輪測(cè)速得速度見(jiàn)表1。從表中可知機(jī)器人左右輪速度能夠保持在一定范圍內(nèi)，且兩輪速度差變化也很小，驗(yàn)證了運(yùn)動(dòng)控制器和差速控制環(huán)控制器的有效性。

4 結(jié)語(yǔ)

兩輪機(jī)器人是一個(gè)典型的欠驅(qū)動(dòng)、靜態(tài)不穩(wěn)定系統(tǒng)，它的平衡運(yùn)動(dòng)控制是輪式機(jī)器人研究中的熱點(diǎn)。本文就兩輪機(jī)器人平衡和運(yùn)動(dòng)控制展開(kāi)研究，以樹(shù)莓派為控制中心，在Codesys平臺(tái)開(kāi)發(fā)控制軟件，并進(jìn)行了相關(guān)試驗(yàn)測(cè)試，驗(yàn)證了此控制方法的有效性。

表1 左右輪速度Tab.1 Speed of left wheel and right wheel

參考文獻(xiàn)：

[1] 楊國(guó)歡.基于復(fù)合控制器的兩輪機(jī)器人平衡控制研究[J].科技資訊，2014，12（10）：2-4.

[2] 王曉宇.兩輪機(jī)器人的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2007.

[3] 阮曉鋼，蔡建羨，李欣源.兩輪機(jī)器人的研究與設(shè)計(jì)[M].北京：科學(xué)出版社，2012.

[4] 戴福全.多自由度兩輪機(jī)器人技術(shù)研究[D].北京：北京理工大學(xué)，2015.

[5] 袁澤睿.兩輪機(jī)器人控制算法的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2006.

[6] 段學(xué)超，袁俊，滿曰剛.兩輪機(jī)器人的動(dòng)力學(xué)建模與模糊進(jìn)化極點(diǎn)配置控制[J].信息與控制，2013，42（2）：189-195.

[7] 李潮全.同軸兩輪機(jī)器人及其平衡控制技術(shù)研究[D].北京：北京理工大學(xué)，2011.