空間6R機械臂圓弧軌跡規劃及仿真

呂 燕，安 凱

（山東航天電子技術所，山東 煙臺 264003）

0 引言

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

1 運動學分析

用D－H法對機器人建模，如圖1所示，桿件參數見表1［2］。其中：ai－1，αi－1，di，θi分別為D－H建模法中按運動順序，由本坐標系變換至下一坐標系的旋轉角度和平移長度。

圖1 6R機器人模型Fig.1 6R－robot model

運動支架各連桿的參數為：d1＝135mm，d2＝50mm，d4＝－50mm，a2＝1 050mm，a3＝800mm。

機器人正運動學解是已知各關節的角度值，由運動學算法計算機械臂末端的位置和姿態。只要機器人的關節變量已知，就能根據運動學方程確定機器人的位置。

表1 D－H參數表Tab.1 D－H parameters

運動學逆解是已知關節末端期望的位置和姿態，由變量分離法求出各關節的轉動角度。本文模型中，因構型特殊，求解較易。在多組解中，本文選取一組合適的解作為最終解。

2 圓弧運動軌跡規劃

2.1 位置插補

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

圖2 機器人圓弧運動軌跡規劃Fig.2 Robot’s round track programming

設已知起始點P1在基坐標系O－XYZ中的位置為（x1，y1，z1），終點P3在O－XYZ系中的位置為（x3，y3，z3），指定的路經點P2在O－XYZ系中的位置為（x2，y2，z2）。

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

圖3 坐標空間關系Fig.3 Relationship of two reference frames

平面M的方程可表示為

平面T的方程可表示為

平面S的方程可表示為

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

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

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

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

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

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

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

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

進而可得點P1，P2，P3在P0－X0Y0Z0系中的坐標為

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

在M平面上進行圓弧插補計算：由設定的末端線速度v可得運動時間t＝s／v和運行步數N＝tΔt。角速度ω＝（θ1＋θ2）／t。圓心角時間函數為θk＝ωΔtk。此處：Δt為時間步長；k＝1／N。

在每個采樣周期，按上述規劃可得軌跡點對應的圓心角θk。設點Qi為圓弧Q1Q2Q3上一點，其對應圓心角為θi，則第i個插補點在P0－X0Y0Z0系中的坐標為

式中：圓弧Q1Q2Q3在P0－X0Y0Z0坐標系中逆時針時ρ＝1，順時針時ρ＝－1。

由式（10）、（11）可知：插補點在O－XYZ系中的坐標Pi（xi，yi，zi）滿足

2.2 姿態插補

姿態插補采用均勻插補。位置插補中，已算得圓弧運動的總步數N，根據初始和期望姿態，可求得初始和結束時的歐拉角α1，β1，γ1和α2，β2，γ2，并平均分配在N步中。若角度以弧度表示，該返回值在－π～π之間（不包括－π），且在該范圍內角度值唯一。

經計算，初始位置的歐拉角為

期望位置的歐拉角為

則，第i步的歐拉角為

由此，可得第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。

這樣，就獲得了每個插補周期內機械臂末端的姿態陣。聯合位置規劃結果，由運動學反解可求得各關節值送下位機伺服。

3 圓弧軌跡規劃仿真

運動學反解中，對同一位姿陣，存在多組解。本文中，從第一關節角開始，各關節角的選取原則為與初始關節角之差的絕對值最小，6關節初始位置均按0°計算。

設實驗數據中：起始和終點位姿陣分別為

中間點位置P2為（700，800，400）；末端速度v＝50，T＝0.5s。由過曲線軌跡規劃和軌跡姿態插補可得機械手末端起點和機械手末端終點間的運動軌跡如圖5所示，差補所得軌跡上的離散點坐標見表2。

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

表2 空間圓弧軌跡上的插值點坐標Tab.2 ？？？of space arc track

在此組數據中，解算出的各關節的角度序列如下：

a）關 節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）關 節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）關節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）關 節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）關 節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）關 節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。

圓弧運動各關節的角度變化如圖6所示。

圖6 圓弧運動各關節的角度變化Fig.6 Angle change of joints when robot moving following a circle

由圖5、6可知：用本文中的控制方法，機械臂末端進行點到點圓弧運動時，運動軌跡平滑，分解到各關節，各關節的角度變化平緩，不存在突變，保證了運動的穩定性。

4 結論

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

［1］吳鎮煒，談大龍.機械手空間圓弧運動的一種有效軌跡規劃方法［J］.機器人，1999（1）：9－12，46.

［2］尼庫B（美）.機器人學導論——分析、系統及應用［M］.孫富春，等（譯）.北京：電子工業出版社，2004.

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