尚偉燕 邱法聚 楊超珍

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

0 引言

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

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

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

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

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

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

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

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

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

探測(cè)機(jī)器人左右兩側(cè)對(duì)稱，結(jié)構(gòu)相同，基于上述運(yùn)動(dòng)建模假設(shè)，在越障過程中僅對(duì)其一側(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)位于懸架上表示前承重輪與地面（履帶）的瞬時(shí)重合坐標(biāo)系（圖3中未標(biāo)出）表示后承重輪與地面（履帶）的瞬時(shí)重合坐標(biāo)系（圖3中未標(biāo)出）；W表示固聯(lián)于搖臂懸架的坐標(biāo)系，坐標(biāo)原點(diǎn)位于后輪與搖臂懸架鉸接處；P表示后輪與地面接觸點(diǎn)坐標(biāo)系表示后輪與地面的瞬時(shí)重合坐標(biāo)系。

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

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

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

式中，ω1為搖臂懸架轉(zhuǎn)動(dòng)角速度為后輪相對(duì)于搖臂懸架在Z軸方向的運(yùn)動(dòng)速度；分別為右后輪與地面接觸點(diǎn)的坐標(biāo)系在右后輪與地面瞬時(shí)重合坐標(biāo)系中的3個(gè)方向上的速度分量；ω為探測(cè)機(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)向半徑及探測(cè)機(jī)器人各輪的滑轉(zhuǎn)角速度分別為

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

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

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

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

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

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

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

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

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

將［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，未知矢量矩陣為

已知量或可測(cè)得量矩陣包含21個(gè)分量：

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

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

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

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

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

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

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

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

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

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

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

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

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

3 結(jié)論

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

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

（3）運(yùn)動(dòng)學(xué)仿真試驗(yàn)驗(yàn)證了理論模型的正確性，車身運(yùn)動(dòng)曲線相對(duì)于車輪運(yùn)動(dòng)曲線平緩，證明探測(cè)機(jī)器人懸架結(jié)構(gòu)設(shè)計(jì)能夠提高車身運(yùn)動(dòng)的平穩(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)動(dòng)學(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(cè)車運(yùn)動(dòng)學(xué)建模與仿真［J］.北京航空航天大學(xué)學(xué)報(bào)，2008，34（3）：348－352.