差速轉(zhuǎn)向復(fù)合式探測機(jī)器人運(yùn)動學(xué)分析

尚偉燕 邱法聚 楊超珍

1.寧波工程學(xué)院，寧波，315000 2.寧波市特種設(shè)備檢驗檢測研究院，寧波，315000

0 引言

救援機(jī)器人具有較好的環(huán)境適應(yīng)性，往往能代替人完成復(fù)雜環(huán)境下的探測任務(wù)，在災(zāi)場搜救等方面顯示出其優(yōu)勢［1－3］。輪履復(fù)合式探測車在執(zhí)行探測任務(wù)過程中，當(dāng)路面狀態(tài)復(fù)雜多變，遇到難于跨越的障礙物、溝壕及斜坡等地形時，需要探測車跟蹤路徑規(guī)劃器所規(guī)劃的路徑，靈活地進(jìn)行避障行駛，因此，探測車在執(zhí)行探測任務(wù)中的轉(zhuǎn)向較頻繁。筆者設(shè)計的差速轉(zhuǎn)向復(fù)合式探測機(jī)器人（以下簡稱為探測機(jī)器人）采用滑動轉(zhuǎn)向方式，四輪全驅(qū)動，通過控制內(nèi)外側(cè)輪驅(qū)動電機(jī)的轉(zhuǎn)速實現(xiàn)探測機(jī)器人轉(zhuǎn)向。該轉(zhuǎn)向方式可以實現(xiàn)探測機(jī)器人的原地轉(zhuǎn)向，即零半徑回轉(zhuǎn)，這樣在運(yùn)動環(huán)境比較狹窄的情況下，探測機(jī)器人也能行駛自如，順利完成作業(yè)任務(wù)［4－5］。

探測機(jī)器人在非結(jié)構(gòu)化的三維環(huán)境中移動時，需要隨時獲取相對準(zhǔn)確的自身運(yùn)動信息，對其自身運(yùn)動行為進(jìn)行控制，從而順利地完成轉(zhuǎn)向避障等運(yùn)動，為探測、救援任務(wù)的順利完成打下基礎(chǔ)。因此，進(jìn)行探測機(jī)器人的運(yùn)動學(xué)分析對提高其探測效率具有重要意義。

1 探測機(jī)器人運(yùn)動學(xué)分析

對探測機(jī)器人進(jìn)行運(yùn)動學(xué)分析，首先要討論探測機(jī)器人各相關(guān)桿件之間的運(yùn)動關(guān)系，因此需在各連桿上附加坐標(biāo)系以構(gòu)建連桿之間的位姿變換矩陣。探測機(jī)器人的結(jié)構(gòu)如圖1所示。

1.1 探測機(jī)器人的運(yùn)動學(xué)建模

探測機(jī)器人在惡劣地面條件下行駛時，其后輪與地面之間是高副接觸，并且由于履帶厚度僅為4mm（相對于承重輪半徑可以忽略不計），因此其前后承重輪與地面之間的接觸也為高副。多個運(yùn)動部件與地面相接觸，構(gòu)成多個封閉鏈。為此，采用Denavit－Hartenberg規(guī)則［6］對通過回轉(zhuǎn)副連接的連桿進(jìn)行附體坐標(biāo)系的選擇及參數(shù)的規(guī)定，并在此基礎(chǔ)上結(jié)合適于描述高副接觸的Sheth－Uicker方法［7］，建立探測機(jī)器人運(yùn)動系統(tǒng)的坐標(biāo)系，以消除多閉環(huán)系統(tǒng)坐標(biāo)系傳遞矩陣的歧義性。

執(zhí)行任務(wù)過程中，探測機(jī)器人坐標(biāo)系位置處在不斷變化中，采用Sheth－Uicker方法設(shè)置瞬時重合坐標(biāo)系來描述探測機(jī)器人各部件的運(yùn)動，可以簡化運(yùn)動模型的建立和求解，如圖2所示。

圖2中的下標(biāo)含義如下：G代表地面全局坐標(biāo)系；B代表固聯(lián)在探測機(jī)器人車體中心處的坐標(biāo)系代表坐標(biāo)系B的瞬時重合坐標(biāo)系，相對于坐標(biāo)系G靜止；θ、φ、ψ分別為探測機(jī)器人在瞬時重合坐標(biāo)系中的俯仰角、側(cè)向翻滾角和轉(zhuǎn)向角，設(shè)探測機(jī)器人在慣性坐標(biāo)系中的位置坐標(biāo)為（x，y，z），則探測機(jī)器人車體相對于地面全局坐標(biāo)系的位姿變換矩陣為

基于此位姿變換矩陣可以方便直觀地進(jìn)行高副接觸條件下探測機(jī)器人運(yùn)動狀態(tài)的描述。為了便于建模，假設(shè)越障過程中探測機(jī)器人不產(chǎn)生轉(zhuǎn)向運(yùn)動，直線正向行駛。

探測機(jī)器人左右兩側(cè)對稱，結(jié)構(gòu)相同，基于上述運(yùn)動建模假設(shè)，在越障過程中僅對其一側(cè)建立圖3所示的坐標(biāo)系。各坐標(biāo)系描述如下：D表示差速齒輪坐標(biāo)系；R表示搖臂懸架坐標(biāo)系；Q表示固定于搖臂懸架上的坐標(biāo)系，坐標(biāo)原點(diǎn)位于阻尼彈簧與搖臂懸架鉸接處；H表示固聯(lián)于前履帶輪懸架上的坐標(biāo)系，坐標(biāo)原點(diǎn)位于搖臂懸架與前輪懸架鉸接點(diǎn)處；LF表示固聯(lián)于前履帶輪懸架上的坐標(biāo)系，坐標(biāo)原點(diǎn)位于前側(cè)桿與前輪懸架的鉸接處；KF表示固聯(lián)于前側(cè)桿上的坐標(biāo)系，坐標(biāo)原點(diǎn)位于前承重輪與前側(cè)桿鉸接處；LH表示固聯(lián)于后履帶輪懸架上的坐標(biāo)系，坐標(biāo)原點(diǎn)位于后側(cè)桿與前輪懸架的鉸接處；KH表示固聯(lián)于后側(cè)桿上的坐標(biāo)系，坐標(biāo)原點(diǎn)位于后承重輪與后側(cè)桿鉸接處；AF表示前承重輪與地面（履帶）接觸點(diǎn)坐標(biāo)系，位于承重輪上；AH表示后承重輪與地面（履帶）接觸點(diǎn)坐標(biāo)系，位于承重輪上；C表示彈簧阻尼與懸架的鉸接點(diǎn)坐標(biāo)系，坐標(biāo)原點(diǎn)位于懸架上表示前承重輪與地面（履帶）的瞬時重合坐標(biāo)系（圖3中未標(biāo)出）表示后承重輪與地面（履帶）的瞬時重合坐標(biāo)系（圖3中未標(biāo)出）；W表示固聯(lián)于搖臂懸架的坐標(biāo)系，坐標(biāo)原點(diǎn)位于后輪與搖臂懸架鉸接處；P表示后輪與地面接觸點(diǎn)坐標(biāo)系表示后輪與地面的瞬時重合坐標(biāo)系。

將只包含常數(shù)項的矩陣標(biāo)記為T，將包含變量項的矩陣標(biāo)記為Ω，對探測機(jī)器人的參考坐標(biāo)系到前履帶承重輪、后輪的運(yùn)動鏈條分別進(jìn)行分析。

1.2 探測機(jī)器人運(yùn)動學(xué)方程

為了估算探測機(jī)器人的位置和方向，我們通過坐標(biāo)的矩陣變換引入車輪的雅可比矩陣，從而由各個車輪的速度計算出車體的速度。最終求得探測機(jī)器人直線正向越障時車體運(yùn)動與右后輪運(yùn)動之間的關(guān)系表達(dá)式：

式中，ω1為搖臂懸架轉(zhuǎn)動角速度為后輪相對于搖臂懸架在Z軸方向的運(yùn)動速度；分別為右后輪與地面接觸點(diǎn)的坐標(biāo)系在右后輪與地面瞬時重合坐標(biāo)系中的3個方向上的速度分量；ω為探測機(jī)器人右后輪滑轉(zhuǎn)角速度。

根據(jù)幾何關(guān)系可以求得側(cè)向滑移速度

式中，L為前后輪履中心點(diǎn)之間的距離；S為內(nèi)外側(cè)輪間距；Rs為內(nèi)側(cè)車輪的轉(zhuǎn)向半徑。

通過內(nèi)外側(cè)輪前進(jìn)速度關(guān)系求得轉(zhuǎn)向半徑及探測機(jī)器人各輪的滑轉(zhuǎn)角速度分別為

假設(shè)運(yùn)動過程中各輪的側(cè)向滑移情況相同，則以上分析過程建立了探測機(jī)器人各輪的側(cè)向滑轉(zhuǎn)角速度與探測車內(nèi)外側(cè)輪速度、車體結(jié)構(gòu)參數(shù)之間的關(guān)系，該關(guān)系式適用于實現(xiàn)差速轉(zhuǎn)向的探測車。

探測機(jī)器人行進(jìn)過程中，左右側(cè)搖臂轉(zhuǎn)向角速度ω2、ω1均可以通過傳感器獲得，因此車體繞Y軸的俯仰角速度可以表示為

其中，M、N、R、H 分別為與車體參數(shù)及可測得量相關(guān)的表達(dá)式。

將可直接測量或求得的分量移到式（1）右側(cè)，未知各分量置于式（1）左側(cè)，則探測車右后輪與車體之間的運(yùn)動關(guān)系可表達(dá)為

從上式可以看出，探測車車體速度矢量可以由車輪的速度矢量和各關(guān)節(jié)點(diǎn)的變化關(guān)系來決定，由于左側(cè)后輪與右側(cè)后輪在結(jié)構(gòu)上關(guān)于車體左右對稱，所以左側(cè)后輪與右側(cè)后輪和探測車車體間的變換關(guān)系矩陣具有相同的形式。因此，探測機(jī)器人車體與左右后輪運(yùn)動學(xué)方程的一般形式為

對探測機(jī)器人參考坐標(biāo)系到右側(cè)前履帶輪承重輪1的運(yùn)動鏈條進(jìn)行分析，可求得探測機(jī)器人車體與左右前承重輪之間運(yùn)動學(xué)方程的一般形式：

其中，角度θ01、θ02分別如圖3所示，且θ·01＝ω01，θ·02＝ω02。同理，求得探測機(jī)器人車體與左右后承重輪之間運(yùn)動學(xué)方程的一般形式：

在平行四邊形機(jī)構(gòu)作用下，探測機(jī)器人同一側(cè)連接前后承重輪的前側(cè)桿與后側(cè)桿在運(yùn)動過程中具有相同的轉(zhuǎn)角，因此存在JWj＝JWk，式（2）～式（4）中，i，j，k＝1時代表右側(cè)輪；i，j，k＝2時代表左側(cè)輪。

將式（2）～式（4）合并，可以得到6個輪子形成的復(fù)合式運(yùn)動方程：

將［E5×5Ji］、［E5×5JAj］、［E5×5JHk］中各行向量分別與相應(yīng)的零向量組合，構(gòu)成式（5）中30×11的參數(shù)矩陣A0，將JWi、JWj、JWk中的相應(yīng)行向量與零向量組合，構(gòu)成30×21的矩陣J，未知矢量矩陣為

已知量或可測得量矩陣包含21個分量：

2 運(yùn)動學(xué)方程求解與仿真試驗

2.1 運(yùn)動學(xué)方程求解方法的選取

探測機(jī)器人運(yùn)動過程中，需要在一定采樣頻率下，不斷獲取6個輪子的運(yùn)動輸入信號，并通過運(yùn)動學(xué)方程估算出車體的位置及運(yùn)動狀況，因此，提高運(yùn)動學(xué)方程的求解精度和求解速度有利于快速準(zhǔn)確地把握探測機(jī)器人運(yùn)動信息并隨時對其運(yùn)動進(jìn)行控制。

式（5）是一個超靜定的線性系統(tǒng)，想求得精確解是不可能的，該系統(tǒng)可以用最小二乘法求解：

文獻(xiàn)［8-9］均采用將原超靜定方程轉(zhuǎn)化為正則方程的形式，進(jìn)而求得p·＝（AT0A0）－1AT0Jq·。該求解線性方程組的方法要通過矩陣求逆，與高斯消元求解相比，精度會降低，并且方程的系數(shù)矩陣比原方程的系數(shù)矩陣要病態(tài)很多。事實上，系數(shù)矩陣的條件數(shù)變成了原來的平方，即κ（AT0A0）＝κ（A20）。如果采用有限精度體系運(yùn)算，即使A0的列線性無關(guān)，轉(zhuǎn)換后方程的系數(shù)矩陣AT0A0仍可能奇異，從而使（AT0A0）－1不存在。文獻(xiàn)［10］在此基礎(chǔ)上，采用加權(quán)最小二乘法對探測機(jī)器人運(yùn)動學(xué)問題進(jìn)行求解，在一定程度上能夠提高求解精度，但是最小二乘法加權(quán)矩陣的確定上具有一定的主觀性。

在對各求解方法分析的基礎(chǔ)上，本文提出對探測機(jī)器人運(yùn)動學(xué)方程采用Householder反射來求解最小二乘的方法，其具體求解過程如下：首先構(gòu)造相應(yīng)的Householder反射矩陣，并將這一系列的Householder反射作用于A0的列，得到矩陣：

采用同樣的Householder反射作用于式（5）的右端，得到Rp·＝b，其中b＝H11…H2H1（Jq·）。通過以上變換，其求解結(jié)果表達(dá)通式如下：

由式（6）可以看出，通過Householder變換，前11個等式構(gòu)成上三角陣，可以通過回代法求得p·中各相應(yīng)分量的近似解。剩余19個方程的系數(shù)全為零，對應(yīng)位置b的分量構(gòu)成了殘差項。

2.2 探測機(jī)器人運(yùn)動學(xué)求解與仿真試驗

為驗證運(yùn)動學(xué)模型的正確性，將Recurdyn軟件下探測機(jī)器人在不平路面上前50s的仿真運(yùn)動作為研究對象，將測得的承重輪及后輪在3個方向上的運(yùn)動速度作為運(yùn)動輸入，運(yùn)動中左右輪存在速度差，且設(shè)置右側(cè)車輪速度是左側(cè)對應(yīng)車輪速度的2倍。在Recurdyn軟件仿真下，測得右側(cè)前后承重輪的運(yùn)動速度曲線，分別如圖5、圖6所示。圖5中，后承重輪上的速度變化曲線形狀與前承重輪相似，只是時間上滯后1s。由于搖臂懸架的存在，測得的右側(cè)后輪速度變化與前后兩承重輪的速度有一定差別，其具體變化曲線如圖6所示。

同樣，根據(jù)探測機(jī)器人車輪側(cè)向滑移速度與車輪轉(zhuǎn)向角速度之間的關(guān)系，可以求得探測機(jī)器人車輪側(cè)向滑移角速度ω的曲線。將測得的各組速度及角速度作為運(yùn)動輸入，設(shè)置采樣頻率為50kHz，將各輸入量代入運(yùn)動學(xué)方程，并在MATLAB環(huán)境下編程對運(yùn)動學(xué)方程進(jìn)行求解。經(jīng)過迭代，求得探測機(jī)器人車體在3個方向上的運(yùn)動速度，如圖7所示。

對比圖6、圖7中水平及豎直方向?qū)?yīng)的分速度曲線，我們可以看出：車體質(zhì)心的運(yùn)動曲線與前承重輪運(yùn)動曲線變化較一致，驗證了所建立運(yùn)動學(xué)模型的正確性；車身運(yùn)動曲線較承重輪運(yùn)動曲線平緩；左右側(cè)車輪存在速度差，探測機(jī)器人存在繞Z軸的滑移速度

在采用上述Householder反射進(jìn)行最小二乘解計算的過程中，前50s時間內(nèi)要進(jìn)行2.5×105次采樣，計算量較大。為比較算法的精確程度，僅對前5s內(nèi)測得的25 000組采樣點(diǎn)進(jìn)行分析計算，最終求得殘差項的二范數(shù)為472.9405，而采用傳統(tǒng)方法求解時，所有殘差項的二范數(shù)為7017.7827。一般而言，殘差項越小，求解越為精確，因此，本文提出的采用Householder反射來求探測機(jī)器人運(yùn)動學(xué)方程最小二乘解的方法結(jié)果較為精確。每組采樣點(diǎn)循環(huán)代入式（6）求解時，均采用三角回代法計算前面11個等式，不僅求解時間較短，而且數(shù)值穩(wěn)定性好。

3 結(jié)論

（1）將承重輪、后輪的運(yùn)動作為輸入量，提供了一種求解探測機(jī)器人車體運(yùn)動的有效方法。為探測機(jī)器人的導(dǎo)航及運(yùn)動控制提供了理論依據(jù)。

（2）將Householder變換用于探測機(jī)器人超靜定復(fù)合運(yùn)動方程的求解后，求解精度和數(shù)值穩(wěn)定性得到了提高。

（3）運(yùn)動學(xué)仿真試驗驗證了理論模型的正確性，車身運(yùn)動曲線相對于車輪運(yùn)動曲線平緩，證明探測機(jī)器人懸架結(jié)構(gòu)設(shè)計能夠提高車身運(yùn)動的平穩(wěn)性。

［1］Wang Weidong，Du Zhijiang，Sun Lining.Obstacle Performance Analysis of Mine Research Robot Based on Terramechanics［C］／／Proceedings of the 2007IEEE International Conference on Mechatronics and Automation.Harbin，2007：1382－1387.

［2］Moosavian S A A，Semsarilar H，Kalantari A，et al.Design and Manufacturing of a Mobile Rescue Robot［C］／／2006IEEE／RSJ International Conference on Intelligent Robots and Systems.Beijing，2006：3982－3987.

［3］Matsuno F，Tadokoro S.Rescue Robots and Systems in Japan［C］／／2004IEEE International Conference on Robotics and Biomimetics.Shenyang，2005：12－20.

［4］Pazderski D，Kozlowski K.Trajectory Tracking of under Actuated Skid－steering Robot［C］／／American Control Conference.Washington DC，2008：3506－3511.

［5］Solc F，Sembera J.Kinetic Model of a Skid Steered Robot［C］／／Proceedings of the 7th WSEAS International Conference on Signal Processing，Robotics and Automation.Cambridge，UK，2008：61－65.

［6］Sheth P N，Uicker J J.A Generalized Symbolic Notation for Mechanisms［J］.Journal of Engineering for Industry，1971，93（1）：102－112.

［7］Shin Dong Hun，Park Kyung Hoon.Velocity Kinematic Modeling for Wheeled Mobile Robots［C］／／IEEE International Conference on Robotics and Automatiom.Seoul，2001：3516－3622.

［8］蔡則蘇，洪炳熔，呂德生，等.基于搖桿－轉(zhuǎn)向架的月球車運(yùn)動學(xué)分析與仿真［J］.機(jī)器人，2003，25（7）：636－651.

［9］Muir P F，Neuman C P.Kinematic Modeling of Wheeled Mobile Robot［J］.Robotic Systems，1987，4（2）：281－340.

［10］崔瑩，高峰?？勺冎睆捷喸虑蛱綔y車運(yùn)動學(xué)建模與仿真［J］.北京航空航天大學(xué)學(xué)報，2008，34（3）：348－352.