曾 鵬，張啟倫，張 超，鄭功倍，高丙團(tuán)

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

電纜通道的工作環(huán)境惡劣，采用移動傳感網(wǎng)絡(luò)進(jìn)行狀態(tài)監(jiān)測可以有效地提供工作效率、保障人員安全。球形機(jī)器人是一款綜合了足式和輪式機(jī)器人特點(diǎn)的移動機(jī)器人，其具有“不倒翁”的特性，不會輕易發(fā)生傾覆，環(huán)境適應(yīng)能力較強(qiáng)，且密閉的球形外殼可有效地保護(hù)球內(nèi)的監(jiān)測傳感元件，可以作為電纜通道移動傳感網(wǎng)絡(luò)移動節(jié)點(diǎn)[1-2]。

自20世紀(jì)90年代球形機(jī)器人問世以來[3]，球形機(jī)器人的機(jī)構(gòu)和控制設(shè)計(jì)受到廣泛的關(guān)注和研究[4-5]。輪式驅(qū)動方式通過在球內(nèi)放置輪式驅(qū)動的小車實(shí)現(xiàn)，其結(jié)構(gòu)較為直觀，且內(nèi)部車體便于搭載傳感設(shè)備，更適合于作為移動網(wǎng)絡(luò)的傳感節(jié)點(diǎn)。然而，由于單輪驅(qū)動運(yùn)動軌跡單一，四輪驅(qū)動成本較高，故在此選擇雙輪驅(qū)動的球形機(jī)器人作為電纜通道移動網(wǎng)絡(luò)的機(jī)器人節(jié)點(diǎn)。

當(dāng)前國內(nèi)外對雙輪驅(qū)動移動機(jī)器人的研究鮮有報(bào)道，其動力學(xué)特性和運(yùn)動控制有待研究。本文針對雙輪驅(qū)動的球形機(jī)器人，首先基于拉格朗日方程法建立其動力學(xué)模型；在此基礎(chǔ)上，分析球形機(jī)器人的轉(zhuǎn)向運(yùn)動和直線運(yùn)動特征，從而設(shè)計(jì)機(jī)器人的速度、位置PID 控制器，實(shí)現(xiàn)球形機(jī)器人的速度和位置閉環(huán)運(yùn)動控制，以滿足傳感網(wǎng)絡(luò)節(jié)點(diǎn)的運(yùn)動控制要求。

1 動力學(xué)建模

雙輪驅(qū)動球形機(jī)器人如圖1所示。圖中，均質(zhì)球殼內(nèi)部有一兩輪獨(dú)立驅(qū)動的小車，靜止?fàn)顟B(tài)下小車的質(zhì)心與球心距離為l。在小車上，可以安裝電纜通道監(jiān)測需要的視覺傳感設(shè)備、 無線通信模塊等，通過左右兩輪的獨(dú)立驅(qū)動可以實(shí)現(xiàn)球形機(jī)器人的運(yùn)動控制。

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

球形機(jī)器人的相關(guān)參數(shù)變量設(shè)定如下：M 為均質(zhì)球殼質(zhì)量；m 為內(nèi)部小車車體質(zhì)量；mw為車輪質(zhì)量；R 為球殼半徑；Rw為車輪半徑；h 為靜止?fàn)顟B(tài)下車輪與球殼接觸點(diǎn)到球心的豎直距離；d 為左右車輪間距的1/2；θ 為車體質(zhì)心與球心連線相對于垂直方向的傾角；θL和θR分別為左右車輪滾過的角度；φ為車體前進(jìn)方向逆時針偏離x 軸正方向的角度；τL為左側(cè)電機(jī)輸入的驅(qū)動轉(zhuǎn)矩；τR為右側(cè)電機(jī)輸入的驅(qū)動轉(zhuǎn)矩。

球形機(jī)器人是一個多體系統(tǒng)，運(yùn)動體包括球殼、內(nèi)部車體、左右驅(qū)動輪，可采用拉格朗日方程建立系統(tǒng)的動力學(xué)模型。選擇左驅(qū)動輪轉(zhuǎn)角θL，右驅(qū)動輪轉(zhuǎn)角θR，內(nèi)置車體傾角θ，球形機(jī)器人移動距離S作為廣義坐標(biāo)?；趶V義坐標(biāo)計(jì)算系統(tǒng)的動能和勢能，車體動能Ek1為

式中：Jm為內(nèi)部質(zhì)心到球心的等效轉(zhuǎn)動慣量；Jφm為轉(zhuǎn)向過程中車體相對于曲率圓心的轉(zhuǎn)動慣量。球形機(jī)器人轉(zhuǎn)向角速度與車輪驅(qū)動速度的關(guān)系為

車體勢能Ep1為（設(shè)球殼中心點(diǎn)為零勢能點(diǎn)）

2 個車輪動能Ek2為

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

球殼動能Ek3為

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

由此可得到球形機(jī)器人的拉格朗日函數(shù)為

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

式中：ξ 為黏滯阻尼系數(shù)。

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

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

其中

式（12）～式（15）即為兩輪驅(qū)動球形機(jī)器人的動力學(xué)模型。由動力學(xué)模型可以看出系統(tǒng)是復(fù)雜非線性的，且只有2 個直接驅(qū)動控制輸入τL和τR，但要控制系統(tǒng)的廣義坐標(biāo)構(gòu)型變量有4 個，因此系統(tǒng)是欠驅(qū)動的。

2 運(yùn)動控制設(shè)計(jì)

球形機(jī)器人在作為移動無線傳感器網(wǎng)絡(luò)移動節(jié)點(diǎn)時，根據(jù)不同控制需求（如單獨(dú)移動還是編隊(duì)移動）需要控制其速度或者位置。PID 控制具有結(jié)構(gòu)簡單、穩(wěn)定性好、魯棒性強(qiáng)等特點(diǎn)，在實(shí)踐中廣泛采用。PID 控制可根據(jù)需要，由比例P（proportion）、積分I（integration）、微分D（differentiation）這3 個單元組合成不同的控制器。在此采用PID 控制實(shí)現(xiàn)對球形機(jī)器人速度與位置的控制。

球形機(jī)器人的移動速度和驅(qū)動輪轉(zhuǎn)動速度之間的關(guān)系為

結(jié)合式（2），可得球形機(jī)器人運(yùn)動軌跡的曲率半徑為

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

圖2 PID 控制系統(tǒng)框圖Fig.2 PID control system block diagram

在速度控制的過程主要關(guān)心速度控制的穩(wěn)定性和控制精度，根據(jù)PID 控制設(shè)計(jì)經(jīng)驗(yàn)，可采用PI控制實(shí)現(xiàn)，將期望速度與實(shí)時速度的偏差項(xiàng)作為輸入，控制輸出設(shè)計(jì)為

式中：kp為比例系數(shù)；ki為積分系數(shù)?？紤]到實(shí)際驅(qū)動電機(jī)提供的力矩存在上限，對控制器輸出的控制量進(jìn)行限幅處理，并通過調(diào)節(jié)矩陣將控制輸出轉(zhuǎn)矩分為2 個同值分別驅(qū)動2 個驅(qū)動輪。

當(dāng)球形機(jī)器人需要移動到某平面位置時，根據(jù)上述分析可知，可通過球形機(jī)器人原地轉(zhuǎn)向和直線運(yùn)動組合完成。基于前述的速度PI 控制器設(shè)計(jì)，根據(jù)經(jīng)典速度位置雙閉環(huán)設(shè)計(jì)思想[7-8]，類似的設(shè)計(jì)如式（18）所示的位置閉環(huán)PI 控制器?？刂破髟O(shè)計(jì)與速度PID 控制器相似，輸入量為期望路程與實(shí)時路程的偏差項(xiàng)，輸出量同樣經(jīng)過限幅處理后，通過調(diào)節(jié)矩陣得到球形機(jī)器人的控制轉(zhuǎn)矩，再輸入至系統(tǒng)。機(jī)器人原地轉(zhuǎn)向運(yùn)動時，通過調(diào)節(jié)矩陣將控制輸出轉(zhuǎn)矩分為2 個相反數(shù)，分別驅(qū)動2 個驅(qū)動輪實(shí)現(xiàn)轉(zhuǎn)向控制。

3 仿真分析

3.1 開環(huán)動力學(xué)仿真

在MatLab/Simulink 2018B 中，根據(jù)式（12）～式（15）搭建球形機(jī)器人的模型，通過直線運(yùn)動和轉(zhuǎn)彎運(yùn)動2 組仿真試驗(yàn)來建立的動力學(xué)模型進(jìn)行驗(yàn)證。

模型中所涉及的固定參數(shù)數(shù)值如下：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。仿真中初始狀態(tài)均設(shè)為0。

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

圖3 開環(huán)直線運(yùn)動仿真結(jié)果Fig.3 Simulation result of open loop control for linear motion

進(jìn)一步驗(yàn)證球形機(jī)器人的轉(zhuǎn)向運(yùn)動試驗(yàn)。設(shè)置3 組不同的輸入轉(zhuǎn)矩：τ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。各狀態(tài)量的初值均為0，仿真結(jié)果如圖4所示。

圖4 開環(huán)轉(zhuǎn)向運(yùn)動仿真結(jié)果Fig.4 Simulation result of open loop control for steering motion

由圖4所示軌跡可見，在原地靜止?fàn)顟B(tài)下，開環(huán)輸入左、右驅(qū)動輪不同的轉(zhuǎn)矩得到不同轉(zhuǎn)向半徑的運(yùn)動軌跡，輸入轉(zhuǎn)矩左右輪差值的正負(fù)產(chǎn)生不同的轉(zhuǎn)向方向，差值的大小產(chǎn)生不同的轉(zhuǎn)向半徑。當(dāng)左右輪輸入轉(zhuǎn)矩大小相同方向相反時，原地轉(zhuǎn)向，軌跡為一個點(diǎn)，即初始位置點(diǎn)。這符合上述分析和實(shí)際的直觀經(jīng)驗(yàn)，表明了球形機(jī)器人動力學(xué)模型的合理性和正確性。

3.2 閉環(huán)運(yùn)動控制仿真

3.2.1 閉環(huán)的速度控制仿真

根據(jù)動力學(xué)模型的仿真結(jié)果，雖然開環(huán)控制可以實(shí)現(xiàn)球形機(jī)器人的勻速直線運(yùn)動，但控制轉(zhuǎn)矩與速度的關(guān)系不明確，且開環(huán)系統(tǒng)魯棒性較差。

仿真中，設(shè)置各狀態(tài)量的初值均為0，運(yùn)動速度的期望值S˙d=0.2 m/s，比例系數(shù)kp=0.14，積分系數(shù)ki=0.5，控制器輸出量的幅值限制為0.07 N·m。將限幅處理后的輸出量經(jīng)調(diào)解矩陣得到球形機(jī)器人的控制轉(zhuǎn)矩，輸出至系統(tǒng)并進(jìn)行仿真，試驗(yàn)結(jié)果如圖5所示。

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

由圖可見，約t=2 s 時速度以達(dá)到期望值，車體姿態(tài)角在速度穩(wěn)定后還存在一定的振蕩，在恒輸出轉(zhuǎn)矩作用下由黏滯摩擦的作用自動實(shí)現(xiàn)車體姿態(tài)角的穩(wěn)定調(diào)節(jié)；在t=3 s 時系統(tǒng)達(dá)到穩(wěn)定。此后，球形機(jī)器人保持穩(wěn)定狀態(tài)的勻速直線運(yùn)動，證明了所設(shè)計(jì)速度PI 控制的有效性。

3.2.2 目標(biāo)位置閉環(huán)控制仿真

進(jìn)行球形機(jī)器人的目標(biāo)位置閉環(huán)控制仿真，在初始狀態(tài)均為零的情況下，設(shè)定向φ=π/4 方向行走S=0.3 m，角度控制PI 參數(shù)分別為0.15 和0.3，仿真結(jié)果如圖6所示。

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

運(yùn)動控制過程分為2 個步驟，先進(jìn)行原地轉(zhuǎn)向調(diào)節(jié)，再進(jìn)行直線位置移動。由圖6所示仿真曲線可見，首先輸出左右驅(qū)動輪方向相反大小相同的控制轉(zhuǎn)矩進(jìn)行原地轉(zhuǎn)向，經(jīng)過約2.5 s 的時間球形機(jī)器人完成原地轉(zhuǎn)向控制，在2.5 s 之內(nèi)小車位移為0 且保持不變。調(diào)整好方向后，小車運(yùn)動方向角保持恒定的期望值，輸出方向和大小均相同的左右輪驅(qū)動轉(zhuǎn)矩開始直線移動，經(jīng)過約3.5 s 的時間，路程達(dá)到期望值，此后機(jī)器人靜止，完成精確的位置控制，實(shí)現(xiàn)閉環(huán)位置控制的目標(biāo)。

4 結(jié)語

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