尚偉燕 邱法聚 楊超珍

1.寧波工程學院，寧波，315000 2.寧波市特種設備檢驗檢測研究院，寧波，315000

0 引言

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

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

1 探測機器人運動學分析

對探測機器人進行運動學分析，首先要討論探測機器人各相關桿件之間的運動關系，因此需在各連桿上附加坐標系以構建連桿之間的位姿變換矩陣。探測機器人的結構如圖1所示。

1.1 探測機器人的運動學建模

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

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

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

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

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

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

1.2 探測機器人運動學方程

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

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

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

式中，L為前后輪履中心點之間的距離；S為內外側輪間距；Rs為內側車輪的轉向半徑。

通過內外側輪前進速度關系求得轉向半徑及探測機器人各輪的滑轉角速度分別為

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

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

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

將可直接測量或求得的分量移到式（1）右側，未知各分量置于式（1）左側，則探測車右后輪與車體之間的運動關系可表達為

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

對探測機器人參考坐標系到右側前履帶輪承重輪1的運動鏈條進行分析，可求得探測機器人車體與左右前承重輪之間運動學方程的一般形式：

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

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

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

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

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

2 運動學方程求解與仿真試驗

2.1 運動學方程求解方法的選取

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

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

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

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

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

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

2.2 探測機器人運動學求解與仿真試驗

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

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

對比圖6、圖7中水平及豎直方向對應的分速度曲線，我們可以看出：車體質心的運動曲線與前承重輪運動曲線變化較一致，驗證了所建立運動學模型的正確性；車身運動曲線較承重輪運動曲線平緩；左右側車輪存在速度差，探測機器人存在繞Z軸的滑移速度

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

3 結論

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

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

（3）運動學仿真試驗驗證了理論模型的正確性，車身運動曲線相對于車輪運動曲線平緩，證明探測機器人懸架結構設計能夠提高車身運動的平穩(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］蔡則蘇，洪炳熔，呂德生，等.基于搖桿－轉向架的月球車運動學分析與仿真［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車運動學建模與仿真［J］.北京航空航天大學學報，2008，34（3）：348－352.