空間6R機(jī)械臂圓弧軌跡規(guī)劃及仿真

呂 燕，安 凱

（山東航天電子技術(shù)所，山東 煙臺(tái) 264003）

0 引言

傳統(tǒng)軌跡規(guī)劃分為關(guān)節(jié)空間的規(guī)劃和在笛卡爾空間的規(guī)劃兩種。其中：關(guān)節(jié)空間的軌跡規(guī)劃相對(duì)簡(jiǎn)單，且不會(huì)出現(xiàn)奇異位形，但末端的運(yùn)動(dòng)軌跡不直觀；笛卡爾空間的軌跡規(guī)劃較直觀，但在規(guī)劃過程中易產(chǎn)生奇異位形，同時(shí)其運(yùn)動(dòng)學(xué)逆解也相對(duì)復(fù)雜。圓弧運(yùn)動(dòng)是機(jī)器人完成點(diǎn)到點(diǎn)運(yùn)動(dòng)的常用運(yùn)動(dòng)方式，研究具有普遍意義。某6R機(jī)器人由6個(gè)回轉(zhuǎn)關(guān)節(jié)組成，機(jī)器人末端裝有機(jī)械手，用于完成操作任務(wù)，如安裝相機(jī)執(zhí)行拍攝任務(wù)，抓取物體，協(xié)助交會(huì)對(duì)接等。在機(jī)械臂末端執(zhí)行任務(wù)時(shí)，為有效避開障礙物，防止抓手與障礙物碰撞、發(fā)生抓手或者障礙物的損壞等，本文基于文獻(xiàn)［1］的機(jī)器人圓弧軌跡規(guī)劃方法，對(duì)在笛卡爾空間的圓弧運(yùn)動(dòng)軌跡規(guī)劃進(jìn)行了研究。

1 運(yùn)動(dòng)學(xué)分析

用D－H法對(duì)機(jī)器人建模，如圖1所示，桿件參數(shù)見表1［2］。其中：ai－1，αi－1，di，θi分別為D－H建模法中按運(yùn)動(dòng)順序，由本坐標(biāo)系變換至下一坐標(biāo)系的旋轉(zhuǎn)角度和平移長度。

圖1 6R機(jī)器人模型Fig.1 6R－robot model

運(yùn)動(dòng)支架各連桿的參數(shù)為：d1＝135mm，d2＝50mm，d4＝－50mm，a2＝1 050mm，a3＝800mm。

機(jī)器人正運(yùn)動(dòng)學(xué)解是已知各關(guān)節(jié)的角度值，由運(yùn)動(dòng)學(xué)算法計(jì)算機(jī)械臂末端的位置和姿態(tài)。只要機(jī)器人的關(guān)節(jié)變量已知，就能根據(jù)運(yùn)動(dòng)學(xué)方程確定機(jī)器人的位置。

表1 D－H參數(shù)表Tab.1 D－H parameters

運(yùn)動(dòng)學(xué)逆解是已知關(guān)節(jié)末端期望的位置和姿態(tài)，由變量分離法求出各關(guān)節(jié)的轉(zhuǎn)動(dòng)角度。本文模型中，因構(gòu)型特殊，求解較易。在多組解中，本文選取一組合適的解作為最終解。

2 圓弧運(yùn)動(dòng)軌跡規(guī)劃

2.1 位置插補(bǔ)

空間圓弧軌跡的參數(shù)方程的編寫難度極大，對(duì)空間圓弧軌跡方程進(jìn)行插補(bǔ)也非常復(fù)雜。為便于計(jì)算圓的軌跡方程，須先將圓的軌跡方程由固定坐標(biāo)系進(jìn)行變換，在新坐標(biāo)系中引入軌跡規(guī)劃插值后，再反變換到基準(zhǔn)坐標(biāo)系中［3］。其軌跡規(guī)劃流程如圖2所示。

圖2 機(jī)器人圓弧運(yùn)動(dòng)軌跡規(guī)劃Fig.2 Robot’s round track programming

設(shè)已知起始點(diǎn)P1在基坐標(biāo)系O－XYZ中的位置為（x1，y1，z1），終點(diǎn)P3在O－XYZ系中的位置為（x3，y3，z3），指定的路經(jīng)點(diǎn)P2在O－XYZ系中的位置為（x2，y2，z2）。

此時(shí)，可由空間O－XYZ系中的P1，P2，P3三點(diǎn)確定一空間平面M，由線段P1P2的中點(diǎn)可確定與P1P2垂直的一平面T，由線段P2P3的中點(diǎn)可確定與線段P2P3垂直的一平面S，如圖3所示。

圖3 坐標(biāo)空間關(guān)系Fig.3 Relationship of two reference frames

平面M的方程可表示為

平面T的方程可表示為

平面S的方程可表示為

聯(lián)立式（1）～（3），用高斯消去法可求出圓心P0（x0，y0，z0），進(jìn)而可求得半徑

以P0為原點(diǎn)，將M平面法矢量方向作為新坐標(biāo)系的Z0軸方向，P0P1方向作為新坐標(biāo)系的X0軸方向，新坐標(biāo)系的Y0軸的方向余弦為Z0軸的方向余弦叉乘X0軸的方向余弦，建立新坐標(biāo)系P0－X0Y0Z0。只要求出P0－X0Y0Z0系在O－XYZ系中的變換矩陣A，就可通過坐標(biāo)變換將三維空間的圓弧變換到二維平面的圓弧，如圖4所示。圖中：分別為點(diǎn)P1，P2，P3在P0－X0Y0Z0系中的坐標(biāo)。

由式（1）可得平面M的法矢量方向數(shù)為

圖4 M平面上圓示圖Fig.4 Circle on M－reference－plane

以M平面法矢量方向作為新坐標(biāo)系的Z0軸方向，Z0軸的方向余弦為

以P0P1方向作為P0－X0Y0Z0系的P0X0軸方向，則P0X0軸的方向余弦為

P0－X0Y0Z0系的P0Y0軸的方向余弦為

從而可求出P0－X0Y0Z0系與在O－XYZ系的變換矩陣分別為

進(jìn)而可得點(diǎn)P1，P2，P3在P0－X0Y0Z0系中的坐標(biāo)為

式中：i＝1，2，3。在平面M上，已知Q1，Q2，Q3三點(diǎn)坐標(biāo)，易求得P0Q1、P0Q2間的夾角θ1，P0Q2、P0Q3間的夾角θ2，以及由點(diǎn)Q1經(jīng)點(diǎn)Q2到點(diǎn)Q3的弧s＝r（θ1＋θ2）。

在M平面上進(jìn)行圓弧插補(bǔ)計(jì)算：由設(shè)定的末端線速度v可得運(yùn)動(dòng)時(shí)間t＝s／v和運(yùn)行步數(shù)N＝tΔt。角速度ω＝（θ1＋θ2）／t。圓心角時(shí)間函數(shù)為θk＝ωΔtk。此處：Δt為時(shí)間步長；k＝1／N。

在每個(gè)采樣周期，按上述規(guī)劃可得軌跡點(diǎn)對(duì)應(yīng)的圓心角θk。設(shè)點(diǎn)Qi為圓弧Q1Q2Q3上一點(diǎn)，其對(duì)應(yīng)圓心角為θi，則第i個(gè)插補(bǔ)點(diǎn)在P0－X0Y0Z0系中的坐標(biāo)為

式中：圓弧Q1Q2Q3在P0－X0Y0Z0坐標(biāo)系中逆時(shí)針時(shí)ρ＝1，順時(shí)針時(shí)ρ＝－1。

由式（10）、（11）可知：插補(bǔ)點(diǎn)在O－XYZ系中的坐標(biāo)Pi（xi，yi，zi）滿足

2.2 姿態(tài)插補(bǔ)

姿態(tài)插補(bǔ)采用均勻插補(bǔ)。位置插補(bǔ)中，已算得圓弧運(yùn)動(dòng)的總步數(shù)N，根據(jù)初始和期望姿態(tài)，可求得初始和結(jié)束時(shí)的歐拉角α1，β1，γ1和α2，β2，γ2，并平均分配在N步中。若角度以弧度表示，該返回值在－π～π之間（不包括－π），且在該范圍內(nèi)角度值唯一。

經(jīng)計(jì)算，初始位置的歐拉角為

期望位置的歐拉角為

則，第i步的歐拉角為

由此，可得第i步時(shí)的姿態(tài)陣

式中：a11＝cosαicosβicosγi－sinαisinγi；a12＝－cosαisinβicosγi－sinαicosβi；a13＝cosαisinβi；a21＝sinαicosβicosγl＋cosαisinβi；a22＝－sinαisinβicosγi＋cosαicosβi；a23＝sinαisinγi；a31＝－cosβisinγi；a32＝sinβisinγi；a33＝cosαi。

這樣，就獲得了每個(gè)插補(bǔ)周期內(nèi)機(jī)械臂末端的姿態(tài)陣。聯(lián)合位置規(guī)劃結(jié)果，由運(yùn)動(dòng)學(xué)反解可求得各關(guān)節(jié)值送下位機(jī)伺服。

3 圓弧軌跡規(guī)劃仿真

運(yùn)動(dòng)學(xué)反解中，對(duì)同一位姿陣，存在多組解。本文中，從第一關(guān)節(jié)角開始，各關(guān)節(jié)角的選取原則為與初始關(guān)節(jié)角之差的絕對(duì)值最小，6關(guān)節(jié)初始位置均按0°計(jì)算。

設(shè)實(shí)驗(yàn)數(shù)據(jù)中：起始和終點(diǎn)位姿陣分別為

中間點(diǎn)位置P2為（700，800，400）；末端速度v＝50，T＝0.5s。由過曲線軌跡規(guī)劃和軌跡姿態(tài)插補(bǔ)可得機(jī)械手末端起點(diǎn)和機(jī)械手末端終點(diǎn)間的運(yùn)動(dòng)軌跡如圖5所示，差補(bǔ)所得軌跡上的離散點(diǎn)坐標(biāo)見表2。

圖5 空間中規(guī)劃的末端三維軌跡Fig.5 Track of robot’s tip

表2 空間圓弧軌跡上的插值點(diǎn)坐標(biāo)Tab.2 ？？？of space arc track

在此組數(shù)據(jù)中，解算出的各關(guān)節(jié)的角度序列如下：

a）關(guān) 節(jié)1：0.862 17，0.874 65，0.884 19，0.890 74，0.894 29，0.894 91，0.892 72，0.887 86，0.880 52，0.870 89，0.859 20，0.845 68，0.830 56，0.814 09，0.796 50，0.778 02，0.758 90，0.739 35，0.719 60，0.699 83，0.680 26，0.661 07，0.642 43，0.624 52，0.607 48，0.591 46，0.576 62，0.563 08，0.550 97，0.540 42。

b）關(guān) 節(jié)2：－0.776 84，－0.795 52，－0.813 87，－0.831 53，－0.848 14，－0.863 45，－0.877 20，－0.889 25，－0.899 45，－0.907 74，－0.914 10，－0.918 51，－0.921 04，－0.921 73，－0.920 68，－0.917 98，－0.913 77，－0.908 15，－0.901 26，－0.893 24，－0.884 21，－0.874 32，－0.863 69，－0.852 44，－0.840 69，－0.828 56，－0.816 13，－0.803 51，－0.790 79，－0.778 05。

c）關(guān)節(jié)3：2.129 5，2.117 7，2.103 6，2.087 4，2.069 5，2.050 1，2.029 5，2.007 9，1.985 8，1.963 4，1.940 9，1.918 8，1.897 1，1.876 4，1.856 6，1.838 2，1.821 3，1.806 1，1.792 9，1.781 7，1.772 7，1.765 9，1.761 6，1.759 7，1.760 3，1.763 3，1.768 7，1.776 5，1.786 5，1.798 6。

d）關(guān) 節(jié)4：－2.414 7，－2.375 6，－2.333 7，－2.289 7，－2.244 1，－2.197 5，－2.150 6，－2.103 8，－2.057 6，－2.012 6，－1.969 2，－1.927 7，－1.888 6，－1.852 2，－1.818 7，－1.788 3，－1.761 2，－1.737 7，－1.717 6，－1.701 2，－1.688 4，－1.679 3，－1.673 7，－1.671 5，－1.672 7，－1.677 2，－1.684 6，－1.694 9，－1.707 7，－1.722 9。

e）關(guān) 節(jié)5：－2.296 6，－2.306 8，－2.311 1，－2.309 4，－2.301 8，－2.288 7，－2.270 5，－2.247 7，－2.221 1，－2.191 3，－2.159 1，－2.125 0，－2.089 8，－2.053 9，－2.017 6，－1.981 4，－1.945 4，－1.909 6，－1.874 1，－1.838 8，－1.803 7，－1.768 5，－1.733 0，－1.697 1，－1.660 3，－1.622 4，－1.583 0，－1.541 6，－1.497 7，－1.450 6。

f）關(guān) 節(jié)6：0.875 94，0.872 86，0.870 15，0.867 93，0.866 33，0.865 42，0.865 26，0.865 89，0.867 31，0.869 51，0.872 45，0.876 09，0.880 34，0.885 14，0.890 39，0.895 99，0.901 82，0.907 78，0.913 75，0.919 61，0.925 23，0.930 50，0.935 29，0.939 47，0.942 93，0.945 52，0.947 12，0.947 61，0.946 84，0.944 69。

圓弧運(yùn)動(dòng)各關(guān)節(jié)的角度變化如圖6所示。

圖6 圓弧運(yùn)動(dòng)各關(guān)節(jié)的角度變化Fig.6 Angle change of joints when robot moving following a circle

由圖5、6可知：用本文中的控制方法，機(jī)械臂末端進(jìn)行點(diǎn)到點(diǎn)圓弧運(yùn)動(dòng)時(shí)，運(yùn)動(dòng)軌跡平滑，分解到各關(guān)節(jié)，各關(guān)節(jié)的角度變化平緩，不存在突變，保證了運(yùn)動(dòng)的穩(wěn)定性。

4 結(jié)論

本文對(duì)空間6R機(jī)械臂圓弧軌跡規(guī)劃方法進(jìn)行了研究。該插補(bǔ)方法可控制插補(bǔ)精度，如減小末端運(yùn)動(dòng)速度和插差值時(shí)間間隔，插值點(diǎn)就會(huì)更密集，運(yùn)動(dòng)軌跡會(huì)更接近于圓，關(guān)節(jié)角變化也會(huì)更平緩。因此，應(yīng)用時(shí)需在精度與運(yùn)動(dòng)速度間尋找平衡點(diǎn)。該軌跡規(guī)劃方法可用統(tǒng)一的實(shí)時(shí)插補(bǔ)遞推公式，使用方便，不僅可用于機(jī)械臂的圓弧軌跡規(guī)劃，而且可用于需空間圓弧功能的數(shù)控機(jī)床系統(tǒng)。此軌跡規(guī)劃方法滿足機(jī)械臂點(diǎn)到點(diǎn)圓弧運(yùn)動(dòng)的要求，使機(jī)械臂能更靈活地抓取物體，或當(dāng)機(jī)械臂在執(zhí)行點(diǎn)到點(diǎn)運(yùn)動(dòng)遇到障礙物時(shí)，如障礙物的大概位置、尺寸已知時(shí)，可使機(jī)械臂末端經(jīng)過指定空間中的第三點(diǎn)到達(dá)目標(biāo)位置，從而有效避開障礙物。此方法還可用于需精確跟蹤圓弧軌跡的場(chǎng)合。

［1］吳鎮(zhèn)煒，談大龍.機(jī)械手空間圓弧運(yùn)動(dòng)的一種有效軌跡規(guī)劃方法［J］.機(jī)器人，1999（1）：9－12，46.

［2］尼庫B（美）.機(jī)器人學(xué)導(dǎo)論——分析、系統(tǒng)及應(yīng)用［M］.孫富春，等（譯）.北京：電子工業(yè)出版社，2004.

［3］張 凱，劉成良，付 莊，等.6R機(jī)器人軌跡規(guī)劃及其在焊接中的應(yīng)用［J］.機(jī)械設(shè)計(jì)，2002，19（10）：20－23.