跡規(guī)劃是機(jī)器人控制問題的重要方面，根據(jù)作業(yè)要求通地軌跡序列控制點控制機(jī)器人位姿軌跡。Paul〔1〕首先利用齊次變換矩陣將手部在直角坐標(biāo)下的位置、速度和加速度變換成各關(guān)節(jié)的位移、速度和加速度，然后規(guī)劃成二次平滑函數(shù)。Paul方法的計算量非常大，Taylor〔2〕采用四元數(shù)表示法改進(jìn)了Paul方法。后來Lin和Luh〔3，4〕提出規(guī)劃軌跡的3次樣條函數(shù)方法，可得到優(yōu)化的關(guān)節(jié)運(yùn)動規(guī)律，但當(dāng)軌跡中間路徑點個數(shù)n較多時，此法所需計算量也較大，而且缺乏時姿態(tài)插補(bǔ)的考慮。在許多高精度應(yīng)用場合，如切割、弧焊等不僅要求機(jī)器人位置精確，還需要在該位置具有任意確定的姿態(tài)，對外部品質(zhì)的要求是很高的。因此，必須解決機(jī)器人姿態(tài)在插補(bǔ)結(jié)點處相應(yīng)的空間坐標(biāo)，以尋求更具一般意義的位姿軌跡生成的通用算法。
本文運(yùn)用旋量法來描述機(jī)器人末端夾持器在直角坐標(biāo)空間中的位置和姿態(tài)對時間函數(shù)所顯示的運(yùn)動軌跡，由于姿態(tài)旋量的直觀和簡便對描述瞬時姿態(tài)有獨特的優(yōu)點，且計算量也小。文中還利用速度矢量是雅可比矩陣列向量的線性組合關(guān)系，對廣義坐標(biāo)的速度量進(jìn)行線性規(guī)劃，免去了求解運(yùn)動學(xué)方程，并適合于具有冗余自由度的操作器。
1 機(jī)器人位姿軌跡
1.1 姿態(tài)旋量
機(jī)器人的位姿就是終端夾持器的位置和姿態(tài)。我們可以用角位移矢量Ω來描述機(jī)器人的姿態(tài)，設(shè)ψ為基坐標(biāo)系中繞瞬時軸加轉(zhuǎn)的等效旋轉(zhuǎn)角，K表示基系中瞬時轉(zhuǎn)軸的單位向量，則角位移矢量：
Ω＝ψK。
根據(jù)旋量定義，可以證明等效角位移矢量的姿態(tài)矢量是旋量，表示為
式中，OP為用位移矢量上給定的初始點位置，基系原點O為旋量參考點。
由對偶數(shù)理論可知：三維歐氏空間中直線與三維對偶空間中的點是一一對應(yīng)，于此可將直角坐標(biāo)空間中的姿態(tài)旋量映射到對偶空間，得到對應(yīng)點，位姿軌跡的規(guī)劃問題便轉(zhuǎn)化為對偶空間中由姿態(tài)旋量所映射的點運(yùn)動軌跡的選擇問題。
圖1 姿態(tài)旋量
1.2 位姿軌跡
設(shè)T為機(jī)器人由起始點到結(jié)束點完成運(yùn)動所需的總時間，t為分段軌跡算起的時間，令
若在時間間隔〔0，t〕內(nèi)，機(jī)器人完成一個給定的工作，整個工作軌跡上需計算的采樣點數(shù)：
N0＝Int(t／T)。
姿態(tài)旋量時應(yīng)的對偶空間中的點假設(shè)沿著一連續(xù)軌跡運(yùn)動
是λ(t)的對偶函數(shù)，寫成對偶坐標(biāo)形式。
(1)
式中Ωxi,Ωyi,Ωzi為姿態(tài)坐標(biāo)分量，的Plücker坐標(biāo)(Ωi，Soi，用坐標(biāo)分量的純量形式表示為(Ωxi,Ωyi,Ωzi,S0xi,S0yi,S0zi)
姿態(tài)矢量Ωi為瞬時轉(zhuǎn)動軸上的自由矢量，只有當(dāng)Pi點位置確定后，它才在軸線上唯一定位。Ωi在空間的定位可通過瞬時轉(zhuǎn)動軸線上Pi的位置矢量rip給定，于此S0i＝rip×Ωi〔5〕，將式(1)改寫成行列式形式的參數(shù)方程為
(2)
式中，xpi,ypi,zpi為夾持器姿態(tài)矢量Ωi在軸線上Pi點相對于基系的坐標(biāo)，式(2)就是機(jī)器人位姿的姿態(tài)旋量表示。由Ωxi，Ωyi,Ωzi確定機(jī)器人夾持器的姿態(tài)軌跡，由xpi,ypi,zpi導(dǎo)出其位置軌跡，設(shè)定理想位置及姿態(tài)軌跡為
(3)
(4)
代入式(2)便可確定機(jī)器人在對偶空間的姿態(tài)旋量。機(jī)器人在進(jìn)行焊接或切割工作，圓弧曲線軌跡運(yùn)動中姿態(tài)的變化，需要按式(2)求出每一采樣時刻的姿態(tài)角。
2 機(jī)器人運(yùn)動螺旋方程
設(shè)為終端速度旋量，為姿態(tài)角速度向量，vpi為終端位置速度，基旋量，
(5)
(6)
于端夾持器的瞬時運(yùn)動螺旋方程為
(7)
螺旋軸線Plücker坐標(biāo)為
3 關(guān)節(jié)運(yùn)動速度
設(shè)固聯(lián)于機(jī)器人各可動件上的附件參考系原點O′i放在運(yùn)動副關(guān)節(jié)處，相鄰運(yùn)動副軸線之間的合法線長度為a12，a23，……；相鄰兩桿之間的偏距分別為d1，d2，…；相鄰軸線之間的扭向角為v12，v23，…；運(yùn)動副相對回轉(zhuǎn)角為θ1，θ2，…。
定義函數(shù)
令
取第i關(guān)節(jié)的轉(zhuǎn)角θi，或滑移距離zi作為廣義坐標(biāo)，qi＝(1－μi)zi＋μiθi(i＝1，2，…，n)
將螺旋運(yùn)動旋量方程(7)作轉(zhuǎn)換后可得
(8)
或表示為
(9)
式中，J1，J2，J3是雅可比矩陣J的三個3×3子陣，這里注意到六關(guān)節(jié)機(jī)器人決定姿態(tài)的關(guān)節(jié)4、5、6的變量沒有影響vx,vy,vz的移動，可將式(9)分解寫成
(10)
(11)
由上式可知終端執(zhí)行器移動線速度和轉(zhuǎn)動角速度與各關(guān)節(jié)角速度的關(guān)系由雅可比矩陣聯(lián)系，它由機(jī)器人各桿件的位姿矩陣和旋轉(zhuǎn)矩陣組合給出。
根據(jù)工作過程的需要，規(guī)劃終端執(zhí)行器的位姿軌跡及速度必需與末端的實際測定的數(shù)值一致。然而，機(jī)器人各桿件的彈性變動，關(guān)節(jié)間隙，重力負(fù)載及桿件離心效應(yīng)等因素的影響致使機(jī)器人位姿動態(tài)精度形成誤差。設(shè)為期望軌跡上的速度旋量，為機(jī)器人末端測定的實際速度旋量，由傳感器可獲得實際位姿軌跡與期望作業(yè)偏差為
機(jī)器人的位置和姿態(tài)誤差分別小于給定誤差R及G的概率〔6〕。為使誤差收斂反回軌跡，以消除誤差的累積效果，需使位置及姿態(tài)誤差得到校正補(bǔ)償，式(10)，(11)改寫為
(12)
(13)
式(12)、(13)適用于J滿秩的情況，當(dāng)機(jī)器人具有冗余自由度時，對應(yīng)的有無窮多解，對此可取能量損失為最小，選取最優(yōu)解
(14)
為尋求滿足式(14)使損失函數(shù)N()，為最小，應(yīng)用拉格朗日算子解
(15)
W為n×n對稱正定矩陣，λ為Lagrange乘子，滿足最優(yōu)解的必要條件是
即
(16)
(17)
在式(16)，(17)中消去λ，得最優(yōu)解。
(18)
考慮到使誤差得到收斂，式(18)改寫成
(19)
其中均為正定陣。式(19)適用于有冗余自由度時的規(guī)劃。要求關(guān)節(jié)運(yùn)動速度不應(yīng)達(dá)到邊界位置極限速度，設(shè)M為允許的最大速度，必需使＜M，以適應(yīng)電機(jī)最大轉(zhuǎn)速的要求。
4 算  例
設(shè)斯坦福機(jī)械手在擬定軌跡中通過空間3個已知點P1(50，0，118)，P2(110.5，50，84)，P3(50.2，100，50)，并在三點保持姿態(tài)為Ω1(0，0，1.57)T，Ω2（0，－0.045，0)T，Ω3(0，0，1.57)T。P1，Ω1狀態(tài)相對應(yīng)的關(guān)節(jié)坐標(biāo)及其相應(yīng)的正弦和余弦值如表1，試規(guī)劃其運(yùn)動和位姿軌跡。
表1
關(guān)節(jié)坐標(biāo)
坐   標(biāo) 數(shù)   值 正   弦 余   弦
θ1 0° 0 1
θ2 90° 1 0
θ3   ／ ／
θ4 0° 0 1
θ5 90° 1 0
θ6 90° 1 0
解 設(shè)機(jī)械手終端以圓弧軌跡規(guī)劃，其位置坐標(biāo)函數(shù)及姿態(tài)坐標(biāo)函數(shù)為
xp＝f1〔λ(t)〕＝60.5sin(2.9966°t)＋50，
yp＝f2〔λ(t)〕＝－50.03cos(2.9966°t)＋50，
zp＝f3〔λ(t)〕＝34cos(2.9966°t)＋84，
Ωx＝ζ1〔λ(t)〕＝－0.05cos2(2.9966°t)＋0.05sin(2.9966°t)＋0.05，
Ωy＝ζ2〔λ(t)〕＝－0.065sin(2.9966°t)＋0.02cos2(2.9966°t)＋0.02，
Ωz＝ζ3〔λ(t)〕＝0.0012cos2(2.9966°t)－1.57sin(2.9966°t)＋1.569。
設(shè)運(yùn)動總時間為T＝60s，據(jù)式(2)當(dāng)t＝40s時終端夾持器的位置，姿態(tài)為
據(jù)式(5)、(6)可求得t＝40s終端的位姿速度值，
斯坦福機(jī)械手雅可比矩陣的三個子陣為
其中，
J11＝－d2〔C2(C4C5C6－S4S6)－S2S5C6〕＋S2d3(S4C5C6＋C4S6)，
J21＝－d2〔－C2(C4C5C6＋S4S6)＋S2S5S6〕＋S2d3(－S4C5S6＋C4C6)，
J31＝－d2(C2C4S5＋S2C5)＋S2d3(S4S5)，
J12＝d3(C4C5C6－S4S6)，J13＝－S5C6，
J22＝－d3(C4C5S6＋S4C6)，J23＝S5S6,
J32＝d3C4S5，J33＝C5。
d2＝－t6041S1＋t6042C1，
d3＝S2(t6041C1＋t6042S1)＋t6043C2，
Ci＝cosθi，Si＝sinθi，(i＝1，2，…，6)，
t6041＝102.5，t6042＝25.09，t6043＝67.07，
可得d2＝25.09，d3＝102.5。
據(jù)測定手部位姿誤差統(tǒng)計值為Δx＝0.08465，Δy＝0.1269，Δz＝0.1050，Δφx＝0.0022，Δφy＝0.0025，Δφz＝0.0041。取
據(jù)式(12)，(13)可得關(guān)節(jié)速度
5 結(jié)  論
1)本文用對偶映射原理來描述機(jī)器人的姿態(tài)旋量，用Plücker線坐標(biāo)表達(dá)機(jī)器人位姿。
2)在機(jī)器人軌跡規(guī)劃中，利用旋量方法時描述瞬時姿態(tài)具有直觀、簡便的獨特優(yōu)點，比較全面地表達(dá)了終端執(zhí)行器的位置和姿態(tài)的軌跡生成，且計算量較少。
3)根據(jù)實際工作軌跡進(jìn)行規(guī)劃，提高了操作器運(yùn)行精確性，并使非線性優(yōu)化問題化為線性優(yōu)化問題，利用速度矢量是雅可比矩陣列向量的線性組合關(guān)系，免去了求解逆運(yùn)動學(xué)方程，并適合于具有冗余自由度的操作器?！?
基金項目：福建省自然科學(xué)基金資助項目
作者單位：林瑞麟(華僑大學(xué)機(jī)電工程系，福建泉州362011)
參考文獻(xiàn)：
〔1〕Paul R P. Manipulator cartesian path contor〔J〕. IEEE Transaction on Systems,Man, and Cybernetics,1979,9(11)：702～711.
〔2〕Taylor R H. Planning and execution of straight line trajectories〔J〕. IBM Journal of Research and Development,1979,23：424～436.
〔3〕Lin C S, Chang P R, Luh JYS. Formulation and optimization of cubic polynomial joint trajectories for industrial robots〔J〕. IEEE Jransaction on Automatic Control,1983,28(12)：1066～1073.
〔4〕Luh J Y S, Lin C S. Approximate join trajectories for control of industrial robots along cartesian paths〔J〕. IEEE Trans System, Man and Cybernetico，1984，14(3)：444～450.
〔5〕林瑞麟，蔣少茵，林碧. 旋量法在機(jī)器人動力學(xué)分析中的應(yīng)用〔J〕.應(yīng)用數(shù)學(xué)和力學(xué)，1996，17(1)：75～80.
〔6〕徐衛(wèi)良，張啟先. 機(jī)器人誤差分析的蒙特卡洛方法〔J〕.機(jī)器人，1988，2(4)：1～5.
(湯任基推薦)