基于阻抗控制的機械臂末端工具的柔順控制*

李 煥，王奉文，徐世杰，侯月陽，盧 山

1.北京航空航天大學，北京 100191；2. 上海航天控制技術研究所，上海 201109；3.上海市空間智能控制技術重點實驗室，上海 201109

0 引言

隨著航天事業(yè)的發(fā)展，航天任務的多樣化，空間機械臂已成為空間站建設、航天器對接、在軌服務等任務中不可缺少的重要組件.在機械臂與環(huán)境接觸時，由于控制、測量等誤差的存在，末端作動器和目標之間可能存在很大的接觸力.為確保接觸力在允許的范圍之內，需要通過在機械臂關節(jié)或手指等部位引入額外的力/力矩傳感器實現力感知，再借助相應的控制策略實現末端柔順控制[1-3].

迄今為止，科研工作者提出了兩種基本的柔順控制方法.第一種方法是“力/位混合控制”，由Raibert和Craig提出[4].在該方法中，任務空間被柔順選擇矩陣分為位置控制和力控制兩個正交子空間.但由于柔順選擇矩陣的選取依賴于具體任務環(huán)境，且位置和力控制對于剛度的要求矛盾，所以該方法的應用受到很大限制[5].為了解決這些問題，Hogan提出了阻抗控制，該方法將機械臂的機械阻抗調整到適合目標模型[6].

阻抗控制建立了末端位置和接觸力之間的動力學關系，激起了廣大科研工作者們的研究興趣.Seraji提出自適應阻抗控制，允許自動調整增益，在環(huán)境參數變化的情況下也能提供穩(wěn)定性能[7].Seul提出神經網絡力控制，用于補償機器人動力學和未知環(huán)境的所有不確定性[8].Nakanishi將阻抗控制用于空間操縱器，有效保持了機械臂末端和目標的接觸[9].N. Uyama將阻抗控制和PID控制結合，用于非合作衛(wèi)星的消旋[10].由于機器人接觸任務的迫切需要，國內對于阻抗控制的研究也紛紛展開.東南大學的楊振等人[11]對傳統(tǒng)的阻抗控制算法、自適應阻抗控制算法作了比較研究.哈爾濱工業(yè)大學張磊等人[12]綜合考慮對空間站系統(tǒng)的影響，利用阻抗控制完成輔助轉位對接.中國空間技術研究院的危清清等人[13]提出柔性機械臂輔助大負載空間艙段對接的阻抗控制，消除了大負載慣性力對測量的影響.

但由以上研究工作可以看出，以往研究者只考慮機械臂末端在空間中的位置關系，而未考慮精細的末端工具結構.這極大地限制了阻抗控制在實際工程上的應用.為了解決這個問題，本文建立了精確的末端工具模型，并將位置和姿態(tài)同時納入柔順控制器的設計中，實現了末端位置和姿態(tài)的綜合控制.將阻抗控制應用在精細的末端工具上，實現空間在軌精細化操作，這也是本文的最大創(chuàng)新點.

本文以六角沉頭螺釘的拆卸作為實際應用背景.控制過程通過以下步驟實現.首先，建立末端扳手工具并安裝在機械臂末端，以建立機械臂和末端工具綜合體的運動學和動力學模型.然后，設計位置姿態(tài)一體化控制的包含位置內環(huán)的阻抗控制器，使用關節(jié)位置/速度傳感器和腕力傳感器的測量來得到關節(jié)角、關節(jié)角速度和末端接觸力反饋；然后，通過機械臂逆動力學計算得到各關節(jié)的驅動力矩作為系統(tǒng)輸入.最后，通過Adams/Matlab聯合仿真驗證了在末端和目標之間的接觸力控制在合理范圍的情況下，末端扳手準確插入六角螺釘沉頭孔.本文旨在通過控制機械臂末端和環(huán)境目標之間的接觸力，實現工程應用中機械臂末端的精細柔順操作.

1 系統(tǒng)動力學建模

為了對空間機械臂進行控制仿真，我們首先對空間機械臂的運動學和動力學內容進行分析.

1.1 機械臂運動學與逆運動學

機械臂的運動學是指機械臂關節(jié)角度空間和工作空間中的末端位置之間的關系，需要根據機器臂結構、各關節(jié)的相關參數，運用機器人運動學的相關算法，建立機械臂齊次坐標變換關系，實現機器人運動學建模.

(1)

其中，θi指繞zi-1軸由xi-1轉向xi軸的關節(jié)角；di是從第(i-1)坐標系的原點到zi-1軸和xi軸的交點沿zi-1軸的距離；ai是從zi-1軸和xi軸的交點到第i坐標系原點沿xi-1軸的偏置距離(zi-1和zi兩軸間的最小距離)；αi為繞xi軸由zi-1軸轉向zi軸的偏角.(轉角均按按右手規(guī)則).

(2)

由此可以得到機械臂的正運動學.

對于n自由度非冗余機械臂(n≤6)，在非奇異的情況下，關節(jié)角速度可由雅可比矩陣求出

(3)

積分得到關節(jié)角.至此完成非冗余機械臂的逆解.

而對于冗余機械臂(n>6)，因其自變量(關節(jié)變量)的個數多于約束的個數，雅可比逆陣不存在，因此逆運動學是個不確定性問題.

一般求解冗余機械臂逆運動學的方法有梯度投影法[15]，固定關節(jié)角法等.其中，梯度投影法的關鍵是確定自運動放大系數k，而這通常要經過反復的計算機仿真才能確定.由于逆運動學問題不是本文研究的主要問題，由于簡便性，本文采用固定關節(jié)角法，固定第三個關節(jié)角為180°，求解得到其他六個關節(jié)角的初始值，即可得到七自由度機械臂的逆運動學解析解.

1.2 機械臂動力學方程

機械臂與環(huán)境接觸時，關節(jié)空間中的動力學方程為

(4)

本文使用的空間機械臂不考慮重力和摩擦力分量，故而動力學方程可簡化為

公式(5)反映了關節(jié)力矩和末端接觸力矩與關節(jié)角、關節(jié)角速度和關節(jié)角加速度之間的函數關系.

2 末端柔順控制器設計

本研究采用阻抗算法設計機械臂末端柔順控制器，將末端接觸力和位移納入統(tǒng)一控制體系.下面對控制律的導出和控制算法流程做詳細闡述.

2.1 基于位置的阻抗控制

阻抗控制通過調整機械臂的機械阻抗以保持末端執(zhí)行器的位姿和末端執(zhí)行器與環(huán)境之間的接觸力的理想動態(tài)關系.實際上，機械臂物理上內在的阻抗不變，阻抗控制的目的就是通過選用合適的主動控制參數來實現理想的目標阻抗.

常用到的二階線性阻抗控制表達式為

其中，M，D，K分別代表質量、阻尼、剛度矩陣.e表示期望末端位姿和實際末端位姿之間的差值.實際上，期望位姿即接觸目標在環(huán)境中的位置和姿態(tài).可以看出，阻抗控制下，末端執(zhí)行器表現得像一個質量-彈簧-阻尼系統(tǒng).如圖1[16]所示.

圖1 機械臂末端阻抗控制示意圖Fig.1 Schematic diagram of impedance controlof end-effector

阻抗控制可以分為傳統(tǒng)的阻抗控制和含有位置控制內環(huán)的阻抗控制.傳統(tǒng)的阻抗控制中，柔順控制和干擾抑制的要求矛盾，且大部分工業(yè)機器人都是以位置控制為內部控制結構，也就是說需要向機器人的控制器輸出位置信號，因此基于位置的阻抗控制策略在實際中獲得了廣泛的應用[17].因此本文采用基于位置的阻抗控制，流程示意圖如圖2所示.

2.2 末端柔順控制器設計

出于對末端位置和姿態(tài)的綜合考慮，本文用到的阻抗控制具體表達式為

(7)

圖2 基于位置的阻抗控制示意圖Fig.2 Schematic diagram of inner positionimpedance control

基于位置的阻抗控制不直接跟蹤輸入的期望位置，而是跟蹤阻抗控制計算得到的參考位置，這樣做的優(yōu)點在于，如果輸入的期望位置有誤差，若直接跟蹤則必將發(fā)生跟蹤不準確的情況，且產生較大接觸力；而如果先由阻抗控制計算得到一個參考位置，則已經把上一步產生的接觸力考慮到下一步位置跟蹤的調整中，機械臂對輸入誤差的容差能力便大大提高，柔順性也得到了更好的體現.

根據公式(7)便可計算得到參考位姿xc.根據圖2，接下來利用位置控制內環(huán)計算末端加速度，以控制機械臂末端實際位姿xe跟蹤參考位姿xc.本研究用的位置控制策略為PD控制，表達式為

(8)

式中，a為期望的機械臂末端加速度，Kp為比例系數，Kd為微分系數，一般取對角矩陣.實際末端位姿和速度均由下列正運動學計算

(9)

根據公式(8)計算得到期望的末端加速度.然后根據任務空間到關節(jié)空間的轉換，得到期望的關節(jié)角速度α

(10)

式中，J+(q)=JT(JJT)-1,為Jacobian矩陣的偽逆矩陣.

至此，我們便可以利用逆動力學計算各關節(jié)的驅動力矩作為機械臂輸入

(11)

3 仿真驗證

本文以七自由度冗余空間機械臂為研究對象，每節(jié)機械臂相對其內接體均具有一個轉動自由度.根據動力學仿真要求，空間機械臂實體模型主要分為三個部分：本體、機械臂、目標螺釘.其中機械臂和目標螺釘固定在本體的不同位置上.在Adams軟件中建立實體模型如圖3所示.

圖3 空間機械臂系統(tǒng)模型Fig.3 Model of space manipulator system

具體作業(yè)任務為機械臂末端裝載一個螺釘扳手，對目標衛(wèi)星本體上的螺釘進行拆卸操作.研究目標為，在拆卸過程中，通過柔順控制策略實現對機械臂末端與螺釘之間接觸力的控制.末端拆卸工具主要由適配筒、傳動滑竿、復位彈簧、電磁鐵、扳手組件以及其他結構共同組成.其中扳手組件是對螺釘進行旋擰操作的主要部件，其示意圖及目標螺釘的示意圖如圖4所示.可以看出，目標螺釘為六角沉頭螺釘，基于此要求，扳手設計為六角棱柱形狀，并且設計突出倒角，以便于兩者順利對接[18].

圖4 末端工具扳手組件(左)和目標螺釘(右)示意圖Fig.4 Schematic diagram of end-effector (left)and screw (right)

考慮到末端工具的精細化結構，采用Adams/Matlab聯合仿真方法.參數設置如表1.

表1 仿真參數Tab.1 Simulation parameters

初始時刻機械臂末端工具擺放在目標螺釘上方2 cm處，兩者之間的相對姿態(tài)未知，在阻抗控制器的作用下向螺釘沉頭凹槽運動.觀察末端工具和目標螺釘接觸過程中的動力學響應以及接觸力情況.

仿真結果如圖5～圖14所示.由圖5、圖6可以看出，由阻抗控制計算得到的參考位置和參考姿態(tài)會隨著末端與環(huán)境的接觸做出相應調整，以使機械臂末端表現出柔順性.從圖9和圖10可以看出，在第5 s左右，末端工具和目標螺釘第一次接觸，產生瞬間碰撞力，繼而機械臂末端被彈起；但由于阻抗控制使得末端表現為一個質量-彈簧-阻尼系統(tǒng)，故而末端工具彈起1.5 mm左右便又繼續(xù)向目標運動；連續(xù)彈起幾次，彈起幅度漸??；從圖11和圖12看出，由于阻抗控制的柔順效果，在碰撞時，末端接觸力控制在1 N之內，接觸力矩控制在0.01 N·m之內.這樣經過多次自身調整，在25 s左右末端工具和螺釘凹槽姿態(tài)一致，工具順利插入凹槽.之后，末端工具穩(wěn)定在目標螺釘凹槽處.末端接觸力和力矩變?yōu)?.

由正向運動學計算得到的末端位置(圖7)和姿態(tài)(圖8)與實際末端位置和姿態(tài)對比，也進一步驗證了系統(tǒng)動力學模型的正確性.

圖5 阻抗控制計算得到的參考末端位置Fig.5 The reference end-effector positions calculatedby impedance control

圖6 阻抗控制計算得到的參考末端姿態(tài)Fig.6 The reference end-effector attitude calculatedby impedance control

圖7 正向運動學計算得到的末端位置Fig.7 The end-effector position calculatedby direct kinematics

圖8 正向運動學計算得到的末端姿態(tài)Fig.8 The end-effector attitude calculatedby direct kinematics

圖9 實際末端位置Fig.9 Actual end-effector position

圖10 實際末端姿態(tài)Fig.10 Actual end-effector attitude

穩(wěn)定之后關節(jié)角位移之所以可以持續(xù)發(fā)生變化(圖13)得益于機械臂的冗余構型，使得在末端位姿固定的情況下，允許關節(jié)構型調整.

4 結 論

本文利用基于位置的阻抗控制律來控制機械臂末端精細的執(zhí)行工具，使其具有對環(huán)境的柔順能力.重點在于將六維的阻抗控制應用在末端柔順控制上，既實現了位置和姿態(tài)的綜合控制，又保證了接觸力和力矩保持在合適范圍內.首先，末端工具安裝在七自由度機械臂末端，基于此建立了該綜合體的動力學模型；然后，設計了具有位置控制內環(huán)的阻抗控制器，本文測量的位移和速度是六維向量，包括平移和旋轉，即位置和姿態(tài).末端工具根據反饋力和扭矩運動，在允許的接觸力范圍內實現與目標的柔順對接.最后設計了精細的末端工具和目標模型，仿真驗證可知，該控制器成功使扳手插入螺釘的六角沉頭孔，實現了機械臂在工程應用中的精細柔順操作.本文設計的基于位置內環(huán)的阻抗控制律允許參數調整，每個量可以根據特定任務而改變，可用于在軌維護，深空探測和空間站建設等方面，具有廣闊的應用前景.

圖11 末端接觸力Fig.11 Contact force between end-effector and target

圖12 末端接觸力矩Fig.12 Contact torque between end-effector and target

圖13 各關節(jié)角位移Fig.13 Angular displacement of seven joints

圖14 各關節(jié)驅動力矩Fig.14 Control torque of seven joints