徐 崢，孫瑩瑩，張 敏

(1. 駐馬店職業(yè)技術學院 機電工程系，河南 駐馬店 463000；2. 國建電網(wǎng)駐馬店供電公司，河南 駐馬店 463000；3. 四川理工學院，四川 自貢 643000)

?

解耦球形腕擬人機械臂運動控制算法研究*

徐 崢1，孫瑩瑩2，張 敏3

(1. 駐馬店職業(yè)技術學院 機電工程系，河南 駐馬店 463000；2. 國建電網(wǎng)駐馬店供電公司，河南 駐馬店 463000；3. 四川理工學院，四川 自貢 643000)

為滿足極端環(huán)境對機械臂在集成度和性能方面的特殊要求，文章提出一種具有高集成度球形手腕機構的7自由度機械臂，對運動控制算法進行了深入研究。首先，分析了機械臂結構特點，其末端手腕的靈活性和緊湊性由雙萬向節(jié)和雙半球結構來保證；其次，建立了機械臂的運動學模型，從正逆運動學兩個角度對機器人的特征進行了描述，并以其位置與姿態(tài)解耦的特點為突破口開發(fā)了機械臂的運動學算法；最后通過MATLAB仿真工具，通過繪制機械臂軌跡曲線對正逆運動算法進行仿真，驗證算法的有效性以及其跟蹤軌跡時的精確性。

7自由度；解耦；球形手腕；運動學

0 引言

在航空航天、軍工以及核工業(yè)等領域，作業(yè)環(huán)境通常具有空間狹小、危險性大等特點。以機器人代替人工進行作業(yè)尤其適應此種場景。機械臂的手腕機構主要的功能是調節(jié)末端姿態(tài)，來實現(xiàn)對于特殊位置的抓取，手腕機構的緊湊度和靈活性直接決定了機械臂是否適合在狹小空間內作業(yè)，并影響機械臂的定位精度。手腕部分通常具有三個自由度，分別對應機械臂的滾動、俯仰和偏轉。本文所設計的機械臂末端手腕采用雙萬向節(jié)和雙半球結構組成，區(qū)別于垂直關節(jié)型手腕，其末段桿長可忽略，相當于僅使用一個關節(jié)的空間實現(xiàn)三個自由度，具有更加緊湊、靈活的特點。

近年來，學術界提出了多種類型機械臂結構，大致可歸為三類：①球形末端：末端手腕軸線交于空間一點，其姿態(tài)和位置是解耦的，逆運動學分析和解算較為簡單，結構較為緊湊，在特殊作業(yè)場景中使用較多；②傳統(tǒng)非球形末端：末端關節(jié)軸線相交于一點，符合Piper準則，運動學解算簡單，但由于連接處桿長的限制，其結構不夠緊湊，姿態(tài)的調節(jié)不夠靈活，空間占用較大；③并聯(lián)手腕末端：等價于小型并聯(lián)機構。

哈爾濱工業(yè)大學吳偉國提出一種基于雙環(huán)解耦方法機構[1]，解決了運動耦合干涉問題。Mark Guckert提出一種緊湊三自由度手腕機構，其表面驅動單元避免了內部的奇異位置的產(chǎn)生[2]。H. R. Choi與S. M. Ryew提出了一種雙主動萬向節(jié)兩自由度的擬人手腕關節(jié)機械臂[3]，其僅具有俯仰與偏轉兩個自由度，可以確保完全自由滾轉的柔順性，運動模式可模仿人類手臂。Raghavan M與Roth B通過對機械臂奇異位置的處理和迭代阻尼因子的調整，改善了阻尼最小二乘DLS算法，實現(xiàn)了機械臂實時運動學求解[4]。文獻中所述結構較為繁瑣，所用方法受限于阻尼因子等經(jīng)驗性較強的因素，不能穩(wěn)定獲取到結果。

本文提出一種具有緊湊球形手腕機構的擬人手臂，分析了其結構特點，并構建了運動學方程，據(jù)此設計了機械臂的正逆運動學算法。文獻中所述算法在迭代計算過程中收斂域較窄，且無法解決關節(jié)與方程數(shù)不相等的問題。本算法通過降低所求解運動學方程維度的方式解決了這些問題。算法的有效性通過MATLAB仿真工具得到驗證。

1 機構分析與運動學建模

如圖1所示，為擬人球形機械臂，該機械臂共有7個關節(jié)：前4個關節(jié)為一般旋轉關節(jié)，后3個關節(jié)構成球形手腕，由一個轉動關節(jié)和一對半球結構組成[5]。

圖1 擬人球形機械臂

機械臂的末端位置由前四個關節(jié)決定，球形手腕負責姿態(tài)的調節(jié)。相較于垂直關節(jié)型7自由度機械臂，本結構的手腕部更加緊湊，在空間上僅占用一個旋轉關節(jié)的空間，但可通過三個自由度對姿態(tài)進行調節(jié)[6]。

1.1 機械結構

腕部主要由動力源、傳動機構、執(zhí)行器和輸出端三部分組成。手腕機構的動力源由三個直流電機構成；減速機構是齒輪系替代了減速機，可實現(xiàn)機構的小型化；執(zhí)行器由上下兩個半球組成，其接觸面與末端軸線的空間垂面傾角為φ。上下半球的運動分別由直流電機驅動，其運動合成后可實現(xiàn)手腕的俯仰和偏轉兩個自由度。

外部萬向節(jié)控制手腕旋轉關節(jié)，由第三個電機經(jīng)減速機構驅動，可實現(xiàn)沿手臂末端軸線的滾動，作為末端的第三個自由度。運動傳遞流程如圖2所示。Ji代表手腕的第i個關節(jié)，θi代表其對應的電機轉角，Li代表連桿。其中J1、J3及J4為主動關節(jié)。上下半球圍繞球心運動，球心是俯仰與偏轉兩自由度的中心，以球心為參考點，θ2為由上下兩半球轉動在傾角為φ的斜面上形成的角度差，由于上下半球分別存在線性傳動關系，θ2可以表示為θ3和θ1的初等函數(shù)。上下半球旋轉運動合成后形成的工作區(qū)域是以界面為基準，傾角為2φ的圓錐形區(qū)域。

圖2 運動輸出流程

1.2 運動學正解

擬人機械臂的坐標系分布如圖3所示。

圖3 擬人機械臂坐標系分布

基坐標系可選位置并不唯一，按照上面的方式建立坐標系可以方便解析各個關節(jié)值與機器人位姿之間的幾何關系，相較于傳統(tǒng)的D-H法，在正解過程中可直觀地獲得機器人相鄰關節(jié)間的齊次變換矩陣，同時在一定程度上減小運動學逆向解析的計算量[7]。這種方法適用于在工程中實現(xiàn)快速建模，在無需考慮動力學的情況下，可簡化運動學關系的推導，便于獲取運動學方程，且結果更加直觀。手腕部分相鄰關節(jié)間的齊次變換矩陣如下，由于手臂的位置與姿態(tài)解耦，為方便逆解運算，將前四關節(jié)與手腕部分分開計算。

手腕部正解如下：

將上述矩陣相乘可得到手腕正解結果為：

前四軸正解如下：

前四軸總正解為：

(1)

1.3 運動學特征分析

由上述運動學方程可知，機械臂后三軸組成的手腕部分的位置向量始終為零，其對末端位置無影響，故可通過機器人總正解的位置向量建立的運動學方程來求解前四關節(jié)的逆解[8]；進而根據(jù)結果，獲得腕部關節(jié)獨立的姿態(tài)用于求解腕部關節(jié)的逆解。同時，分析由位置向量構建的方程組可得，位置與各軸間關系較復雜，很難直觀的對方程組進行消元以降低未知數(shù)的個數(shù)，且方程的個數(shù)3少于未知數(shù)的個數(shù)4。傳統(tǒng)方法解決此問題無法處理冗余軸帶來的多解問題[9]。

2 運動學逆解

根據(jù)機械臂的肩肘與手腕部關于位置和姿態(tài)解耦的特點，逆運動學求解可分開進行。通常情況下求解非線性方程組，方程數(shù)與未知數(shù)個數(shù)相等，對于非線性方程組：

fi(x0,x1,...,xn-1)=0i=0,1,…,n-1

記為：

fi(X)=0i=0,1,…,n-1

其中：

X=(x0,x1,…,xn-1)T

設：

為非線性方程組第k次迭代近似值。則計算第k+1次迭代值得迭代格式如下。

X(k+1)=X(k)-F(X(k))-1f(X(k))

(2)

其中，F(xiàn)(X(k))為機器人雅克比矩陣。

2.1 逆解前四軸

對于前四軸，可根據(jù)機器人末端位置向量p可以建立包含三個方程的方程組：

上式中方程的個數(shù)少于未知數(shù)的個數(shù)，其對應的雅克比矩陣不為方陣，傳統(tǒng)的迭代法或解析法均無法解決此問題，需要對迭代法進行改進。此問題發(fā)生的原因為機械臂共有7個自由度，屬于冗余關節(jié)機器人，這種機構的運動學逆解不唯一，且冗余軸不受到任何機構關系限制，運動學解算過程中，可以記錄該軸的角度值，也可通過數(shù)值迭代方法直接進行求解?？扇「鶕?jù)運動學方程組可求得機器人雅克比矩陣J為：

由此可得，牛頓拉夫遜迭代格式可擴展為：

(3)

其中，J(k)為雅克比矩陣的最小二乘廣義逆，由于雅克比矩陣J為行滿秩陣，有如下關系[10]。

J=JT(JJT)-1

(4)

逆解結果隨初值的選取可能會有多組解滿足，根據(jù)機器人各關節(jié)角度所在象限特征可篩選得出唯一解，采用迭代法求解可能遇到由于初值選取不當而導致迭代過程不收斂的問題，通常情況下迭代初值與真解較近時這種情況出現(xiàn)的概率較低。可采用下山迭代法來解決此問題。

X(k+1)=X(k)-kF(X(k))-1f(X(k))

其中k的選取原則為，若本迭代周期相較于上周期離真解較近，則k保持不變，若相較于上一周期距離真解更遠，則將k縮減為一半。采用本方法，可有效擴大可擴大迭代的收斂范圍，代價是收斂的速度相較于牛頓拉夫遜法有一定下降。

2.2 逆解第5到7軸

由運動學方程可得，機器人的第5到第7軸僅對姿態(tài)造成影響。根據(jù)此特征可解得手腕部三個關節(jié)。由下式

可得：

由第一行、第三列等量關系可得:

將上市右側代入，可求得：

θ5=atan2(c6s5,c5c6)

θ5的值受θ6的影響，如果θ6=0，則θ5為任意解，此時是機器人的奇異位置，驅動關節(jié)轉動所需的力矩較大，關節(jié)運動經(jīng)過此點時理論速度為無限大，在實際應用過程中應盡量避免此種情況。

由第二行、第一列的等量關系可得：

由第二行、第三列的等量關系可得：

將上式的右側代入可得：

θ7=atan2(c6s7,c6c7)

與第5軸情況類似，θ7的值受θ6的影響，如果θ6=0，則θ7為任意解。

在求解第5、7兩軸后，第6軸的值隨之確定，從第4到第6軸的旋轉矩陣如下。

令：

由第二行、第二列與第三列等量關系可得：

代入可得：

θ6=atan2(s6,c6)

由于正切函數(shù)的周期性，機器人關節(jié)翻轉后可得到第二組解：

(5)

角度篩選可依據(jù)各關節(jié)的象限特征進行，最終獲得唯一解。

3 仿真驗證

3.1 機械臂仿真

依據(jù)機械手連桿以及末端球形手腕的特征參數(shù)，通過MATLAB對正逆運動學算法的正確性進行仿真驗證。

通過規(guī)劃器生成機器人末端軌跡曲線，通過逆解得到關節(jié)值，再通過正解計算空間位置，將眾多位置點繪制成一條空間曲線，比較兩條曲線的重合度程度。

分別通過正弦軌跡及空間圓弧軌跡，套用運動學算法來進行軌跡跟蹤，如圖4、圖5所示。

圖4 正弦跟蹤曲線

圖5 圓弧跟蹤曲線

3.2 仿真分析

圖4、圖5中，藍色曲線代表規(guī)劃器生成的目標軌跡曲線，紅色代表經(jīng)一次逆解→正解后計算后形成實際曲線。從上述兩圖中可見，兩條曲線處于重合狀態(tài)。經(jīng)過計算，圖4、圖5曲線上各插值點對應偏差值的數(shù)學期望小于0.001mm，標準差接近于零，若提高迭代過程的容許誤差限，仍可進一步提升精度。易見本算法對于位置的跟蹤效果較好，結果穩(wěn)定，驗證了算法的有效性及精確性，采用擴展下山牛頓拉夫遜迭代方法降低了雅克比矩陣的維度，減小了迭代過程的誤差，同時，且增強了收斂速度。

4 總結

本文以擬人解耦球形腕機械臂為背景，首先法建立了機械臂的關節(jié)坐標系，通過運動學正解構建了其運動學矩陣方程組。以手腕機構與大臂部分位置與姿態(tài)解耦的特點為突破口，提出了一種改進牛頓拉夫遜法來處理逆解問題。通過廣義最小二乘逆取代一般方陣逆來實現(xiàn)以較少方程求解更多的未知數(shù)，成功解決了冗余軸機器人多解問題。對于手腕部分的求解主要依賴于對于機構特征的分析，根據(jù)姿態(tài)矩陣各行列間的等量關系求得兩組相互對稱的解，同時也證明了球形手腕機構的靈活性。通過對手腕部分關節(jié)取值的分析，闡述了手腕的主要奇異位置，在控制過程中，需要注意。

通過MATLAB仿真軟件對正逆解結果進行了驗證。仿真曲線中經(jīng)過逆解→正解得到的曲線與規(guī)劃曲線重合，證明了該算法的正確性，同時保證了解得精度。與其他的方程組數(shù)值解法和幾何求解法相比，通過擴展牛頓拉夫遜法，在未知數(shù)個數(shù)多于方程數(shù)的情況下直接獲得工作空間的合理解，并給出了較為簡潔的篩選方案。與傳統(tǒng)的解析法有著很明顯的不同，此方法僅需選取不同的初值進行求解，根據(jù)解得的結果結合角度象限特征進行篩選。對于算法的研究目前尚缺少迭代初值選取的最優(yōu)方法，今后的工作會補充此方面的研究。

[1] 吳偉國，鄧喜君，蔡鶴皋.基于直齒輪傳動和雙環(huán)解耦的柔性手腕原理與運動學分析[J]. 機器人,1998,20(6)：433-436.

[2]MarkLGuckert,MichaelDNaish.DesignofaNovel3DegreeofFreedomRoboticJoint[C].IEEE/RSJInt.Conf.IntelligentRob.Syst.,IROS,October11-15,2009,St.Louis,Mo,Unitedstates.USA:IEEE,2009:5146-5152.

[3]HRChoi,SMRyew.Anthropomorphicjointmechanismwithtwodegreesoffreedom[C].ProcIEEEIntConfRobotAutom,April24-28, 2000,SanFrancisco,CA,USA.UnitedStates:IEEE,2000: 1525-1530.

[4]RaghavanM,RothB.KinematicsAnalysisofthe6RManipulatorofGeneralGeometry[C].RoboticsResearchFifthInternationalSmposium, 1989:263-269.

[5] 林義忠, 廖繼芳, 劉國慶. 六自由度焊接機器人大臂模態(tài)分析及優(yōu)化[J].組合機床與自動化加工技術, 2016(2):38-40.

[6] 正方, 平雪良, 陳盛龍. 基于ROS的機器人模型構建方法研究[J]. 組合機床與自動化加工技術, 2015 (8):45-51.

[7]PerformanceImprovementofTrackingControlforaPlanarParallelRobotUsingSynchronizedControl[C].InternationalConferenceonIntelligentRobotsandSystems,2006.

[8]HonghaiLiu,SeniorMember,DavidJ.Brown,etal.FuzzyQualitativeRobotKinematics[J].FuzzySystems, 2008(6):808-822.

[9]GaoXS，HuangZ．Acharacteristicsetmethodforequationsolvinginfinitefields[J]．JournalofSymbolicComputation, 2012, 47(6):655-679．

[10]MouCQ，WangDM，LiXL．Decomposingpolynomialsetsintosimplesetsoverfinitefields:thepositive-dimensionalcase[J]．TheoreticalComputingScience，2013，468:102-113．

(編輯 李秀敏)

Research on Motion Control Algorithm of Decoupled Spherical Wrist Manipulator

XU Zheng1，SUN Ying-ying2，ZHANG Min3

(1.Department of Mechanical and Electronic Engineering, Zhumadian Vocational and Technical College, Zhumadian Henan 463000,China;2. State Grid Zhumadian Power Supply Company, Zhumadian Henan 463000,China)

In order to meet the special requirements on the integration and performance of robotic arm in extreme environment, a seven-degree-of-freedom manipulator with a highly integrated spherical wrist mechanism is proposed, and the motion control algorithm is studied in depth. Secondly, the kinematic model of the manipulator is established. From the two angles of kinematics and inverse kinematics, the kinematic model of the manipulator is established. The kinematic model of the manipulator is established. Finally, the forward and backward motion algorithm is simulated and validated by drawing the trajectory curve of the robot arm by means of the MATLAB simulation tool. The simulation results show that the proposed method is effective and reliable, and can be used to simulate the position of the manipulator and the attitude of the manipulator. The validity of the algorithm and the accuracy of its tracking trajectory.

7 DOF; decoupling; spherical wrist; kinematics

1001-2265(2017)06-0101-05

10.13462/j.cnki.mmtamt.2017.06.026

2017-01-23；

2017-02-18

國家自然科學基金項目(51275432)

徐崢(1960—)，男，河南信陽人，駐馬店職業(yè)技術學院副教授，研究方向為工業(yè)機器人與自動控制，(E-mail)xuzheng_1960@163.com。

TH165;TG659

A