球形機器人雙閉環(huán)PID控制設(shè)計與仿真

吳 晗，薛 磊，徐開蕓，朱 昊

（南京工程學(xué)院 自動化學(xué)院，江蘇 南京 211167）

0 引言

近年來，機器人已經(jīng)成為自動控制領(lǐng)域研究的熱點之一，具有廣泛的發(fā)展前景。球形機器人是一種欠驅(qū)動運動載體，其密封性良好。其獨特的運動方式使得其轉(zhuǎn)彎半徑為零，能實現(xiàn)任意時刻任意方向的移動，動作敏捷，高適航性。它適合在室內(nèi)導(dǎo)航、社會服務(wù)和軍事等狹窄、擁擠的環(huán)境中執(zhí)行任務(wù)，且能耗較低。本文研究球形機器人速度控制問題，為該系統(tǒng)設(shè)計雙閉環(huán)PID控制器，在Matlab環(huán)境下Simulink中仿真調(diào)試，實現(xiàn)球形機器人全方位的運動。

1 球形機器人結(jié)構(gòu)設(shè)計與工作原理

圖1 球形機器人的結(jié)構(gòu)圖Fig.1 Structure diagram of a spherical robot

球型機器人的結(jié)構(gòu)如圖1所示。包括透明球體外殼、控制模塊（主控芯片STM32F103）、運動模塊、通信模塊（HC-05藍(lán)牙模塊）以及電源模塊。其中，球形機器人的外殼由兩個半球組成，在中軸鏈接處安裝磁鐵將兩個半球合成一體。球體內(nèi)部放置自平衡雙輪小車，小車運動機構(gòu)由直流無刷電機、電機驅(qū)動模塊TB6612、陀螺儀MPU6050以及萬向輪組成。驅(qū)動雙輪小車會導(dǎo)致整個球體重心位置改變；陀螺儀采集球形機器人的姿態(tài)傳送給控制器，控制器運用控制算法實時調(diào)整球體姿態(tài)，使其重心變化保持在球形機器人期望運動的方向，從而實現(xiàn)控制球體的運動方向。

2 球形機器人系統(tǒng)建模

球形機器人系統(tǒng)是欠穩(wěn)定的非線性系統(tǒng)，是典型的倒立擺模型[1]。現(xiàn)采用一級倒立擺模型分析球形機器人的運動過程，應(yīng)用牛頓力學(xué)方法建立了一級倒立擺模型。模型中運動目標(biāo)為剛體系統(tǒng)，系統(tǒng)抽象成僅由小車和勻質(zhì)球體組成的運動系統(tǒng)。運動中忽略空氣阻力、靜摩擦力、雙輪小車旋轉(zhuǎn)時的摩擦力等次要因素。在以上前提條件下，球形機器人的受力分析如圖2所示，其中a為小車運動加速度，θ為擺桿與垂直向上方向的夾角，F(xiàn)為小車水平方向的力，m是球的質(zhì)量。

圖2 球型機器人的受力分析Fig.2 The force analysis of the spherical robot

為了保證球形機器人穩(wěn)定，通過控制車輪作加速運動，球身的慣性力與車輪的加速度方向相反，大小成正比，此時球的受力為：

由于θ很小，為此作線性化處理。假設(shè)負(fù)反饋控制是車輪加速度 a與偏角 θ成正比，比例為 k1，如果 k1＞g，那么力的方向與位移方向相反。

為了使得球形機器人能盡快地在垂直位置穩(wěn)定，需增加額外的阻力，此力與角速度成正比，比例為k2，方向相反。此時，球受到反作用力為：

由此，可以得到車輪的加速度為：

據(jù)此可以構(gòu)建球形機器人數(shù)學(xué)模型[2]，并建立速度的比例微分負(fù)反饋控制[3]。假設(shè)球形機器人的重心離地面水平高度為L，外力干擾引起球形機器人產(chǎn)生的角加速度為x（t），沿著垂直于球形機器人方向受力分析，可以得到球形機器人偏轉(zhuǎn)角與車輪運動加速度a（t）以及外力干擾加速度x（t）之間的運動方程為：

當(dāng)角度θ很小時，運動方程式（4）可簡化為：

當(dāng)球形機器人靜止時，其重心偏移會導(dǎo)致不穩(wěn)定，因此需要引入負(fù)反饋，得到相應(yīng)傳遞函數(shù)為：

3 球形機器人雙閉環(huán)PID控制設(shè)計

由于球形機器人系統(tǒng)的非線性特點，必須設(shè)計相應(yīng)的控制器，確保系統(tǒng)穩(wěn)定運行。

3.1 球形機器人速度分析

球形機器人的運行速度，由內(nèi)置雙車輪小車的轉(zhuǎn)動速度決定，相應(yīng)的運動軌跡示意如圖3所示。假設(shè)球形機器人外殼為圓C1，周長為S1，直徑D1=16cm；雙輪小車在球形機器人內(nèi)運動軌跡形成的圓為C2，周長為S2，直徑D2=14.5cm；雙輪小車的車輪為圓C3，周長為S3，直徑D3=4.5cm。

圖3 球形機器人內(nèi)置雙車輪小車的運動軌跡示意圖Fig.3 Schematic diagram of the trajectory of a dual wheel car in spherical robots

當(dāng)雙輪小車沿圓C2軌跡行走1圈時，由公式（7）可以推算出雙輪小車對應(yīng)在C3需轉(zhuǎn)3.22圈。當(dāng)雙輪小車沿圓C1轉(zhuǎn)動1圈時，由公式（8）可以推算出小車沿圓C2行走1.1圈，那么對應(yīng)的圓C3轉(zhuǎn)動3.45圈。

設(shè)圓 C1運動速度為 V1（r/min），圓 C2運動速度 V2（r/min），圓 C3對應(yīng)的速度為 V3（r/min），則：

由式（10）和（11）可得：

可見，雙輪小車運動速度為球殼運動速度的3.54倍。

3.2 球形機器人雙閉環(huán)PID控制器設(shè)計

由上述速度分析可知，通過控制雙輪小車速度就可以調(diào)節(jié)球形機器人的運行速度。這里球形機器人速度控制主要實現(xiàn)勻速、加速和減速。閉環(huán)控制的主要環(huán)節(jié)包括測量、比較和執(zhí)行。測量的關(guān)鍵是準(zhǔn)確測出被控變量的實際值；比較是被控變量的實際值與期望值相比較得到偏差；執(zhí)行是利用這個偏差調(diào)節(jié)控制對象，使其運動狀態(tài)接近希望狀態(tài)。這里采用工程實際中應(yīng)用最為廣泛的增量式PID控制算法為[4]：

式中：Kp—比例增益；Ki=KpT/Ti—積分增益；Kd=KpTd/T—微分增益；e（k）—第k次采樣時刻控制器的輸入值；u（k）—第 k 次采樣時刻控制器的輸出值；△u（k）—第k次采樣時刻控制增量值。

從式（13）可見，控制增量△u（k）僅與最近 3次的采樣值有關(guān)，所以增量式PID算法對控制量的計算影響較小，精度較高。

當(dāng)球形機器人運動時，球體自身姿態(tài)隨之改變。陀螺儀裝置測出球體姿態(tài)偏角作為測量值，與最近3次的采樣值進(jìn)行比較，得出的值代入式（13）中經(jīng)計算機加權(quán)處理獲得姿態(tài)偏角的變化量輸出，此變化量控制球形機器人運動方向。

雙閉環(huán)PID控制，即球體姿態(tài)偏角和轉(zhuǎn)速雙負(fù)反饋控制。負(fù)反饋的兩個調(diào)節(jié)器都采用上述增量式PID控制器，實時調(diào)節(jié)機器人偏角和速度，達(dá)到較好的穩(wěn)定控制效果。

球形機器人的雙閉環(huán)PID控制系統(tǒng)框圖如圖4所示，給定雙輪小車轉(zhuǎn)速信號與轉(zhuǎn)速反饋信號比較后送給速度控制器PID1，經(jīng)過PWM1控制電機轉(zhuǎn)速，構(gòu)成速度控制環(huán)。陀螺儀測得球體姿態(tài)偏角變化量送給角度控制器PID2控制球形機器人運動方向，是角度負(fù)反饋。從而構(gòu)成球形機器人的雙閉環(huán)PID控制系統(tǒng)。

圖4 球形機器人雙閉環(huán)PID系統(tǒng)框圖Fig.4 Block diagram of double closed loop PID system for spherical robot

4 球形機器人雙閉環(huán)PID控制仿真

在Matlab環(huán)境下Simulink中搭建球形機器人雙閉環(huán)PID控制系統(tǒng)仿真結(jié)構(gòu)圖如圖5所示。

圖5中在輸入Step階躍給定信號后，Scope示波器中顯示輸出結(jié)果波形如圖6所示，雙輪小車大約在0.6s后達(dá)到穩(wěn)定轉(zhuǎn)速100r/min；此時小車內(nèi)環(huán)的偏角也達(dá)到了穩(wěn)定狀態(tài)，球形機器人能夠穩(wěn)定運行?？梢?，雙閉環(huán)PID控制系統(tǒng)能夠有效地控制球形機器人的運動。

圖5 球形機器人雙閉環(huán)PID控制系統(tǒng)仿真結(jié)構(gòu)圖Fig.5 Simulation structure of double closed loop PID control system for spherical robot

圖6 球形機器人轉(zhuǎn)速和偏角仿真曲線Fig.6 Simulation curve of rotational speed and angle of spherical robot

5 結(jié)論

本文設(shè)計了一種基于自平衡雙輪小車的球形機器人結(jié)構(gòu)，基于球形機器人的運動狀態(tài)，分析了其運動速度特征，設(shè)計了雙閉環(huán)PID控制策略，解決了球形機器人的運動速度控制問題。使用仿真工具Simulink對系統(tǒng)進(jìn)行了仿真分析，結(jié)果表明數(shù)字PID控制方法能夠有效地對球形機器人進(jìn)行控制。在此基礎(chǔ)上，研制了試驗樣機，相應(yīng)的實測試驗也驗證了方案的可行性。