基于Codesys的兩輪機器人控制研究

管觀洋，陳廣鋒，席 偉

（東華大學 機械工程學院，上海 201620）

兩輪機器人是一種差動輪式移動機器人，其平衡是依靠兩個驅動輪的移動來實現的，故體積小、結構簡單、運動靈活，適于在狹小和危險的空間內工作[1]。兩輪機器人屬于非線性、時變、欠驅動、非完整約束系統，控制問題是其研究的關鍵[2]。

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

1 兩輪機器人數學模型

1.1 兩輪機器人硬件平臺

圖1為兩輪機器人實物圖，整體尺寸為長22 cm，寬 10 cm，高 22 cm。

圖1 兩輪機器人實物Fig.1 Two-wheel robot physical picture

兩輪機器人部件主要包括Raspberry Pi（樹莓派）、MPU6050、電源、降壓模塊、直流電機及驅動、編碼器和車輪。樹莓派作為機器人控制中心，它是基于ARM Cortex-A53的微型電腦，集成了WiFi模塊，有40個GPIO，可用于各種控制系統的開發。

1.2 電機模型

兩輪機器人建模時，采用牛頓法分別對機器人的電機、車輪、車身進行建模，再綜合三者建立系統的數學模型[4-7]。兩輪機器人采用直流電機作為驅動，其數學模型如下：

式中：ua為電機輸入電壓；ia為電機電流；R為電機等效電阻；e為電機反電動勢；Tm為電機電磁轉矩；Td為電機負載轉矩；Jm為電機軸等效轉動慣量；Km為電機轉矩系數；Ke為反電動勢系數；ωm為電機轉速。

1.3 車輪模型

圖2為車輪受力分析圖，車輪的運動可分解為直線運動和繞輪軸的轉動，以右輪為例，其動力學方程為

式中：FVR，FHR為車身對右輪作用力的豎直分量和水平分量；fR為地面摩擦力；TR為右電機電磁轉矩；ωR為右輪角速度；vR為右輪前進速度；m為車輪質量；J為電機和車輪機械旋轉部分的轉動慣量；r為車輪半徑。

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

1.4 車身模型

車身的運動亦可分解成直線運動和轉向，圖3為車身受力分析的右視圖和俯視圖。

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

車身的直線運動又可細分為質點的平移和繞質點旋轉，直線運動動力學方程為

式中：mp為車身質量；l為車身質心P與兩車輪中心的距離；Jp為車身繞車輪連線的轉動慣量；θ為車身傾角；T 為車輪施加在車身的扭矩；（xo，zo）、（xp，zp）為車輪連線中心和車身質心在全局坐標系中的坐標。

據圖3俯視圖，轉向動力學方程如下：

式中：Ip為車身繞Z軸的轉動慣量；ω為機器人轉向速度；D為兩車輪間距；v為機器人前進速度。

1.5 兩輪機器人系統模型

假定兩輪機器人在水平地面上運動且車輪純滾動、無滑動，此時vR=ωRr成立。由前面分析可知v=（vL+vR）/2，T=TR+TL，FH=FHL+FHR，FH=FHL+FHR。 結合式（1）～式（3）可得兩輪機器人系統模型如下：

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

2 兩輪機器人的多環PID控制

兩輪機器人的平衡運動控制可以細分為平衡控制環、運動控制環和差速控制環。平衡控制環即系統根據自身的傾角變化計算平衡控制量ub；運動控制環即根據機器人實際速度與期望速度的差計算運動控制量uv；差速控制環即根據兩輪速度差來重新分配左右電機電壓，保證機器人航向角精度。

2.1 平衡控制環

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

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

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

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

參數的選擇可據系統模型來選取。

2.2 運動控制環

兩輪機器人的運動是基于控制行進速度vψ和行進方向ψ實現的，它們與左輪轉速φ˙L，右輪轉速φ˙R存在以下約束關系：

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

若用ev表示真實前進速度vψ與期望前進速度vD的差，運動控制量uv為

2.3 差速控制環

兩輪機器人轉彎靠左右輪的速度差來控制的，其控制框圖如圖5所示。

圖5 機器人差動控制框圖Fig.5 Robot differential control block diagram

從圖中可知，平衡控制電壓ub與運動控制電壓uv相加后，分配到左右電機。分配情況由差動系數σψ決定。差動系數σψ由期望前進角度ψ或ψ˙計算得到，取值［0，1］。機器人實際運動時，受地面、系統本身的影響，會偏離期望的軌跡，故需要式（12）所示的PID反饋環節來進行調節。

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

3 兩輪機器人的Codesys軟件開發與試驗測試

兩輪機器人控制軟件是在Codesys平臺上的完成的。Codesys做為工業控制軟件支持國際標準IEC61131-3，擴展性和兼容性很強，可以極大提高用戶和廠商的開發效率。

3.1 機器人軟件開發

圖6為兩輪機器人運動執行框圖，機器人軟件開發需要在Codesys平臺上完成軟件功能板塊中的內容。

圖6 機器人運動執行框圖Fig.6 Robot motion block diagram

軟件完成后，通過WiFi方式將程序下載到樹莓派中運行。機器人的運行狀態可通過Codesys在線監控，也可由電腦或手機打開網頁（http：//樹莓派IP：8080webvisu.htm）監控，網頁將顯示軟件控制界面如圖7所示。

圖7 機器人軟件界面Fig.7 Robot software interface

3.2 機器人試驗測試

3.2.1 姿態平衡控制試驗

兩輪機器人平衡控制指其啟動時傾角不為0，啟動后需恢復直立并保持平衡。平衡控制試驗的傾角變化如圖8所示。

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

從圖中可看出機器人從15°恢復直立用時約為0.8 s，之后傾角幾乎能穩定在0°周圍，驗證了該機器人有很好的平衡控制性能。

3.2.2 抗干擾試驗

兩輪機器人抗干擾能力是其運動性能的重要指標，干擾試驗時傾角變化如圖9所示。

圖9 干擾試驗傾角曲線Fig.9 Angle curve of interference experiment

由上圖可看出，機器人受干擾后恢復了平衡，用時約1 s，恢復時間非常短，驗證了該機器人的抗干擾效果好。

3.2.3 速度控制試驗

設置期望速度為0.5 m/s，差動系數σψ為0.4，每隔2 s對左右輪測速得速度見表1。從表中可知機器人左右輪速度能夠保持在一定范圍內，且兩輪速度差變化也很小，驗證了運動控制器和差速控制環控制器的有效性。

4 結語

兩輪機器人是一個典型的欠驅動、靜態不穩定系統，它的平衡運動控制是輪式機器人研究中的熱點。本文就兩輪機器人平衡和運動控制展開研究，以樹莓派為控制中心，在Codesys平臺開發控制軟件，并進行了相關試驗測試，驗證了此控制方法的有效性。

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

參考文獻：

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

[2] 王曉宇.兩輪機器人的研究[D].哈爾濱：哈爾濱工業大學，2007.

[3] 阮曉鋼，蔡建羨，李欣源.兩輪機器人的研究與設計[M].北京：科學出版社，2012.

[4] 戴福全.多自由度兩輪機器人技術研究[D].北京：北京理工大學，2015.

[5] 袁澤睿.兩輪機器人控制算法的研究[D].哈爾濱：哈爾濱工業大學，2006.

[6] 段學超，袁俊，滿曰剛.兩輪機器人的動力學建模與模糊進化極點配置控制[J].信息與控制，2013，42（2）：189-195.

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