球形機器人動力學建模與運動控制設計

曾 鵬，張啟倫，張 超，鄭功倍，高丙團

（1.貴州電網有限責任公司 電力科學研究院，貴陽550002；2.貴州電網有限責任公司 興義安龍供電局，安龍552400；3.貴州創星電力科學研究院有限責任公司，貴陽550002；4.東南大學 電氣工程學院，南京210096）

電纜通道的工作環境惡劣，采用移動傳感網絡進行狀態監測可以有效地提供工作效率、保障人員安全。球形機器人是一款綜合了足式和輪式機器人特點的移動機器人，其具有“不倒翁”的特性，不會輕易發生傾覆，環境適應能力較強，且密閉的球形外殼可有效地保護球內的監測傳感元件，可以作為電纜通道移動傳感網絡移動節點[1-2]。

自20世紀90年代球形機器人問世以來[3]，球形機器人的機構和控制設計受到廣泛的關注和研究[4-5]。輪式驅動方式通過在球內放置輪式驅動的小車實現，其結構較為直觀，且內部車體便于搭載傳感設備，更適合于作為移動網絡的傳感節點。然而，由于單輪驅動運動軌跡單一，四輪驅動成本較高，故在此選擇雙輪驅動的球形機器人作為電纜通道移動網絡的機器人節點。

當前國內外對雙輪驅動移動機器人的研究鮮有報道，其動力學特性和運動控制有待研究。本文針對雙輪驅動的球形機器人，首先基于拉格朗日方程法建立其動力學模型；在此基礎上，分析球形機器人的轉向運動和直線運動特征，從而設計機器人的速度、位置PID 控制器，實現球形機器人的速度和位置閉環運動控制，以滿足傳感網絡節點的運動控制要求。

1 動力學建模

雙輪驅動球形機器人如圖1所示。圖中，均質球殼內部有一兩輪獨立驅動的小車，靜止狀態下小車的質心與球心距離為l。在小車上，可以安裝電纜通道監測需要的視覺傳感設備、 無線通信模塊等，通過左右兩輪的獨立驅動可以實現球形機器人的運動控制。

圖1 球形機器人示意圖Fig.1 Schematic diagram of spherical robot

球形機器人的相關參數變量設定如下：M 為均質球殼質量；m 為內部小車車體質量；mw為車輪質量；R 為球殼半徑；Rw為車輪半徑；h 為靜止狀態下車輪與球殼接觸點到球心的豎直距離；d 為左右車輪間距的1/2；θ 為車體質心與球心連線相對于垂直方向的傾角；θL和θR分別為左右車輪滾過的角度；φ為車體前進方向逆時針偏離x 軸正方向的角度；τL為左側電機輸入的驅動轉矩；τR為右側電機輸入的驅動轉矩。

球形機器人是一個多體系統，運動體包括球殼、內部車體、左右驅動輪，可采用拉格朗日方程建立系統的動力學模型。選擇左驅動輪轉角θL，右驅動輪轉角θR，內置車體傾角θ，球形機器人移動距離S作為廣義坐標。基于廣義坐標計算系統的動能和勢能，車體動能Ek1為

式中：Jm為內部質心到球心的等效轉動慣量；Jφm為轉向過程中車體相對于曲率圓心的轉動慣量。球形機器人轉向角速度與車輪驅動速度的關系為

車體勢能Ep1為（設球殼中心點為零勢能點）

2 個車輪動能Ek2為

式中：Jw為車輪相對于車輪軸的轉動慣量。2 個車輪的勢能Ep2為

球殼動能Ek3為

式中：JM為球殼相對球心的轉動慣量；JφM為轉向過程中球殼相對于曲率圓心的轉動慣量。球殼勢能Ep3=0。

由此可得到球形機器人的拉格朗日函數為

系統耗散能量主要由小車擺動過程中的黏性阻尼引起，則可定義為

式中：ξ 為黏滯阻尼系數。

在計算廣義力的過程中，考慮左、右車輪與球殼之間的摩擦力FL和FR，球殼與地面之間的摩擦力Ff，分別定義為

式中：μ1和μ2為摩擦系數；sgn（·）為符號函數。則相對應的廣義力分別為

其中

式（12）～式（15）即為兩輪驅動球形機器人的動力學模型。由動力學模型可以看出系統是復雜非線性的，且只有2 個直接驅動控制輸入τL和τR，但要控制系統的廣義坐標構型變量有4 個，因此系統是欠驅動的。

2 運動控制設計

球形機器人在作為移動無線傳感器網絡移動節點時，根據不同控制需求（如單獨移動還是編隊移動）需要控制其速度或者位置。PID 控制具有結構簡單、穩定性好、魯棒性強等特點，在實踐中廣泛采用。PID 控制可根據需要，由比例P（proportion）、積分I（integration）、微分D（differentiation）這3 個單元組合成不同的控制器。在此采用PID 控制實現對球形機器人速度與位置的控制。

球形機器人的移動速度和驅動輪轉動速度之間的關系為

結合式（2），可得球形機器人運動軌跡的曲率半徑為

由式（17）可知，當2 個驅動輪的速度大小相等但方向相反時，轉向半徑ρ=0，說明雙輪驅動的球形機器人可實現零半徑轉向，即原地轉向；當2 個驅動輪的速度大小和方向均相同時，曲率半徑無窮大，即做直線運動。因此，針對不同方向上的速度或者位置控制要求，可分解為一系列的原地轉向和直線運動控制。鑒于此，在直行運動的情況下，設計的PID 控制方案如圖2所示，可實現對球形機器人運動速度的控制。

圖2 PID 控制系統框圖Fig.2 PID control system block diagram

在速度控制的過程主要關心速度控制的穩定性和控制精度，根據PID 控制設計經驗，可采用PI控制實現，將期望速度與實時速度的偏差項作為輸入，控制輸出設計為

式中：kp為比例系數；ki為積分系數。考慮到實際驅動電機提供的力矩存在上限，對控制器輸出的控制量進行限幅處理，并通過調節矩陣將控制輸出轉矩分為2 個同值分別驅動2 個驅動輪。

當球形機器人需要移動到某平面位置時，根據上述分析可知，可通過球形機器人原地轉向和直線運動組合完成。基于前述的速度PI 控制器設計，根據經典速度位置雙閉環設計思想[7-8]，類似的設計如式（18）所示的位置閉環PI 控制器。控制器設計與速度PID 控制器相似，輸入量為期望路程與實時路程的偏差項，輸出量同樣經過限幅處理后，通過調節矩陣得到球形機器人的控制轉矩，再輸入至系統。機器人原地轉向運動時，通過調節矩陣將控制輸出轉矩分為2 個相反數，分別驅動2 個驅動輪實現轉向控制。

3 仿真分析

3.1 開環動力學仿真

在MatLab/Simulink 2018B 中，根據式（12）～式（15）搭建球形機器人的模型，通過直線運動和轉彎運動2 組仿真試驗來建立的動力學模型進行驗證。

模型中所涉及的固定參數數值如下：M=0.05 kg，m=0.3 kg，mw=0.005 kg，R=0.08 m，Rw=0.02 m，l=0.06 m，h=0.048 m，d=0.064 m，g=9.8 m/s2。仿真中初始狀態均設為0。

首先，驗證球形機器人動力學模型的開環直行運動，設τL=τR=0.05 N·m，仿真時長8 s。仿真結果如圖3所示。由圖可見，球形機器人從初始位置開始，沿著φ=0 的方向直線運動，由于驅動輪質量較小且考慮了摩擦力，驅動輪在恒定力矩的作用下其速度在較短的時間內達到勻速，車體傾角在過渡過程結束后保持恒定，球形機器人能夠實現勻速的直線運動控制，與預期一致。

圖3 開環直線運動仿真結果Fig.3 Simulation result of open loop control for linear motion

進一步驗證球形機器人的轉向運動試驗。設置3 組不同的輸入轉矩：τL1=-0.04 N·m，τR1=0.04 N·m；τL2=0 N·m，τR2=0.04 N·m；τL3=0.04 N·m，τR3=-0.02 N·m。各狀態量的初值均為0，仿真結果如圖4所示。

圖4 開環轉向運動仿真結果Fig.4 Simulation result of open loop control for steering motion

由圖4所示軌跡可見，在原地靜止狀態下，開環輸入左、右驅動輪不同的轉矩得到不同轉向半徑的運動軌跡，輸入轉矩左右輪差值的正負產生不同的轉向方向，差值的大小產生不同的轉向半徑。當左右輪輸入轉矩大小相同方向相反時，原地轉向，軌跡為一個點，即初始位置點。這符合上述分析和實際的直觀經驗，表明了球形機器人動力學模型的合理性和正確性。

3.2 閉環運動控制仿真

3.2.1 閉環的速度控制仿真

根據動力學模型的仿真結果，雖然開環控制可以實現球形機器人的勻速直線運動，但控制轉矩與速度的關系不明確，且開環系統魯棒性較差。

仿真中，設置各狀態量的初值均為0，運動速度的期望值S˙d=0.2 m/s，比例系數kp=0.14，積分系數ki=0.5，控制器輸出量的幅值限制為0.07 N·m。將限幅處理后的輸出量經調解矩陣得到球形機器人的控制轉矩，輸出至系統并進行仿真，試驗結果如圖5所示。

圖5 速度控制仿真結果Fig.5 Simulation of speed control

由圖可見，約t=2 s 時速度以達到期望值，車體姿態角在速度穩定后還存在一定的振蕩，在恒輸出轉矩作用下由黏滯摩擦的作用自動實現車體姿態角的穩定調節；在t=3 s 時系統達到穩定。此后，球形機器人保持穩定狀態的勻速直線運動，證明了所設計速度PI 控制的有效性。

3.2.2 目標位置閉環控制仿真

進行球形機器人的目標位置閉環控制仿真，在初始狀態均為零的情況下，設定向φ=π/4 方向行走S=0.3 m，角度控制PI 參數分別為0.15 和0.3，仿真結果如圖6所示。

圖6 位置控制仿真結果Fig.6 Simulation of position control

運動控制過程分為2 個步驟，先進行原地轉向調節，再進行直線位置移動。由圖6所示仿真曲線可見，首先輸出左右驅動輪方向相反大小相同的控制轉矩進行原地轉向，經過約2.5 s 的時間球形機器人完成原地轉向控制，在2.5 s 之內小車位移為0 且保持不變。調整好方向后，小車運動方向角保持恒定的期望值，輸出方向和大小均相同的左右輪驅動轉矩開始直線移動，經過約3.5 s 的時間，路程達到期望值，此后機器人靜止，完成精確的位置控制，實現閉環位置控制的目標。

4 結語

結構簡單、成本低廉的雙輪驅動球形移動機器人，作為電纜通道等危險惡劣環境移動無線網絡監測系統節點的有力候選，需要首先掌握其動力學并進行運動控制設計。在此，采用拉格朗日方程法，建立了完整的雙輪小車驅動的球形機器人動力學模型；采用易于實現的PI 控制器，設計了系統的速度和位置閉環控制系統。通過將不同位置的運動控制分解為原地轉向和直線運動，進一步簡化了控制實現。仿真試驗分別驗證了所建立的動力學模型和設計的閉環運動控制方案，證明了動力學模型和控制方案的正確性和可行性。論文的研究成果可以為下一步組建電纜通道移動無線傳感監測網，奠定部分工作基礎。