基于旋量理論的Stanford機(jī)器人的逆運(yùn)動(dòng)學(xué)分析

王曉磊，李曉丹

(遼寧工業(yè)大學(xué)機(jī)械工程與自動(dòng)化學(xué)院，遼寧錦州 121001)

0 前言

機(jī)器人逆運(yùn)動(dòng)學(xué)也即是求機(jī)器人運(yùn)動(dòng)學(xué)的反解，是指在給定機(jī)器人末端執(zhí)行器的位置和姿態(tài)后，求解與該位置和姿態(tài)相關(guān)各關(guān)節(jié)的輸出變量，它在機(jī)器人的運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)及控制中起著非常重要的作用。它的求解速度和精度直接影響著該機(jī)器人運(yùn)動(dòng)的快速性和準(zhǔn)確性。傳統(tǒng)的串聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)方程多采用D-H變換矩陣，D-H變換矩陣法相對成熟，但會(huì)產(chǎn)生大量的矩陣相乘計(jì)算，計(jì)算復(fù)雜。運(yùn)用旋量理論可以把復(fù)雜的空間機(jī)構(gòu)問題變得十分簡單，它已經(jīng)廣泛地應(yīng)用到機(jī)器人領(lǐng)域中［1－5］。

1969年Victor Scheinman設(shè)計(jì)了Stanford機(jī)器人，目前已被廣泛地應(yīng)用于教學(xué)和工業(yè)生產(chǎn)中。董明曉［6］和趙軻［7］等均在D-H齊次變換矩陣的基礎(chǔ)上推導(dǎo)出逆運(yùn)動(dòng)問題算法。本文作者對Stanford機(jī)器人的逆運(yùn)動(dòng)分析采用旋量理論的方法，通過指數(shù)積公式建立機(jī)器人正運(yùn)動(dòng)學(xué)數(shù)學(xué)模型，根據(jù)該機(jī)器人的結(jié)構(gòu)特點(diǎn)，應(yīng)用Paden-Kahan子問題求解Stanford機(jī)器人的運(yùn)動(dòng)學(xué)逆解。

1 旋量基礎(chǔ)

任何剛體從一個(gè)位形到另一位形的運(yùn)動(dòng)都可以通過繞空間某直線旋轉(zhuǎn)θ角加上沿該直線的平移距離d合成，此復(fù)合運(yùn)動(dòng)稱為旋量運(yùn)動(dòng)。而其無窮小量稱為運(yùn)動(dòng)旋量，文獻(xiàn)［8］詳細(xì)推導(dǎo)了用運(yùn)動(dòng)旋量的矩陣來表示剛體的運(yùn)動(dòng)，如公式(1)所示。

式中:^ξ表示剛體的瞬時(shí)速度在SE(3)中的元素，ξ表示關(guān)于該軸線的單位旋量坐標(biāo)。

對于多自由度的串聯(lián)機(jī)器人，只需要2個(gè)坐標(biāo)系，即基座坐標(biāo)系{S}和末端執(zhí)行器固定的工具坐標(biāo)系{T}。定義機(jī)器人的初始位形為θ=0時(shí)位形，用gST(0)來表示。從初始位形時(shí){T}與{S}之間的變換可表示為

2 Paden-Kahan子問題簡介

從式(1)可以看出機(jī)器人的各關(guān)節(jié)變量具有強(qiáng)耦合的特點(diǎn)，若求其運(yùn)動(dòng)學(xué)逆解必有一定的困難，因此通常利用Paden-Kahan子問題來消除耦合變量，簡化求解的工作量。Paden-Kahan子問題主要有2個(gè):

(1)點(diǎn)a沿ξ軸旋轉(zhuǎn)到點(diǎn)b求解滿足條件的θ。

(2)一點(diǎn)繞兩個(gè)有序相關(guān)軸ξ1和ξ2的旋轉(zhuǎn)，a、b是空間兩點(diǎn)，其求解過程可轉(zhuǎn)化為兩個(gè)子問題分別求解θ1θ2。θ2為點(diǎn)a繞軸ξ2旋轉(zhuǎn)的角度，接著再繞軸ξ1旋轉(zhuǎn)的角度為θ1，最后點(diǎn)a與點(diǎn)b重合。

上述的2個(gè)子問題具體推導(dǎo)過程及求解結(jié)果見文獻(xiàn)［8］。

3 Stanford機(jī)器人的逆解分析

Stanford機(jī)器人是一種結(jié)構(gòu)比較特殊的工業(yè)機(jī)器人，其中前2個(gè)關(guān)節(jié)軸線交于1點(diǎn)，而后三個(gè)關(guān)節(jié)軸線交于一點(diǎn)，結(jié)構(gòu)模型見圖1。利用Paden-Kahan子問題求解Stanford機(jī)器人的逆解可消除耦合變量，可簡化計(jì)算量。具體的求解過程如下:

(1)初始位形的選擇及指數(shù)積正解數(shù)學(xué)模型建立

取圖示位置為參考圖形;位置即θ=0時(shí)并建立工具坐標(biāo)系和慣性坐標(biāo)系。θ為0時(shí)慣性坐標(biāo)系與工具坐標(biāo)系的變換為

根據(jù)式(4)，可得

則:

圖1 Stanford機(jī)器人模型圖

(2)求前3個(gè)關(guān)節(jié)變量θ1、θ2、θ3

由于4，5，6軸交于一點(diǎn)Aω，所以位于3軸交點(diǎn)Aω?zé)o論后3軸如何旋轉(zhuǎn)其位置保持不變即

將式(7)兩邊同乘Aω，可得:

再取1，2軸的交點(diǎn)A1，同理得，

式(8)兩邊減去A1得:由距離不變原則:

解得θ3。

根據(jù)Paden-Kahan子問題2可解得θ1、θ2。

(3)求后3個(gè)關(guān)節(jié)變量θ4、θ5、θ6

取在6軸，但不在4，5軸一點(diǎn)B，可得

同理根據(jù)Paden-Kahan子問題2可解得θ4、θ5。

由已解得θ1～θ5，根據(jù)式(7)即可求出θ6。

4 實(shí)例驗(yàn)證

Stanford機(jī)器人的主要參數(shù)

l0=750，l2=300

首先給定角度依據(jù)機(jī)器人正解方程，求出機(jī)器人的末端位姿矩陣，應(yīng)用上述逆解的求解過程，求出相應(yīng)的角度。設(shè)給定角度

根據(jù)正解方程(7)求得末端位姿矩陣:

采用文中給出的逆解方法得出

再代入運(yùn)動(dòng)學(xué)正解位姿矩陣(7)，求得末端姿態(tài)得:

比較一下結(jié)果可以看出該算法精度較高，接近真實(shí)值。

5 結(jié)束語

根據(jù)Stanford機(jī)器人結(jié)構(gòu)特點(diǎn)，采用Paden-Kahan子問題將Stanford機(jī)器人運(yùn)動(dòng)學(xué)逆解問題劃分成若干子問題，使其逆解求解更快、更直接，提高了計(jì)算效率，有利于計(jì)算機(jī)對機(jī)器人的實(shí)時(shí)控制;同時(shí)通過具體實(shí)例驗(yàn)證可知該算法正確，精度較高，為求解Stanford機(jī)器人運(yùn)動(dòng)學(xué)逆解提供了一新途徑。

［1］趙杰，劉玉斌，蔡鶴皋．一種運(yùn)動(dòng)旋量逆解子問題的求解及其應(yīng)用［J］．機(jī)器人，2005，27(3):163－167．

［2］王小榮，楊晉，劉瀟瀟，等．仿人機(jī)器人基于旋量理論的運(yùn)動(dòng)學(xué)分析方法［J］．蘭州交通大學(xué)學(xué)報(bào)，2009，29(1):72－76．

［3］李君．基于旋量理論的Stanford臂的運(yùn)動(dòng)學(xué)分析［J］．天津科技大學(xué)學(xué)報(bào)，2010，25(4):72－75．

［4］王科．基于旋量和李群李代數(shù)的SCARA工業(yè)機(jī)器人研究［D］．杭州:浙江大學(xué)，2010．

［5］黃勇剛，杜力，黃茂林．基于旋量理論的機(jī)器人誤差建模方法［J］．哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2010，42(3):484－489．

［6］董明曉，張明勤，趙志超．Stanford機(jī)器人逆運(yùn)動(dòng)問題求解新方法［J］．機(jī)床與液壓，2001，12(2):52－53．

［7］趙軻．Stanford機(jī)器人逆解關(guān)節(jié)變量新方法［J］．茂名學(xué)院學(xué)報(bào)，2005，15(1):35－36．

［8］理查德摩雷，李澤湘，夏思卡薩思特里，等．機(jī)器人操作的數(shù)學(xué)導(dǎo)論［M］．北京:機(jī)械工業(yè)出版社，1998．