自由漂浮空間機器人視覺伺服系統研究

劉陽, 謝宗武, 王濱, 劉宏, 蔡鶴皋

(哈爾濱工業大學 機器人技術與系統國家重點實驗室, 黑龍江 哈爾濱 150001)

自由漂浮空間機器人視覺伺服系統研究

劉陽, 謝宗武, 王濱, 劉宏, 蔡鶴皋

(哈爾濱工業大學 機器人技術與系統國家重點實驗室, 黑龍江 哈爾濱 150001)

為了將動力學模塊引入傳統的視覺伺服控制算法，使其更加符合真實模型。本文以自由漂浮空間機器人視覺伺服為目標，分析了其系統組成與工作原理。采用廣義雅克比的方法完成其速度級的運動學建模，并在6D 空間下分析其動力學模型。機械臂采用PD與前饋控制完成笛卡爾空間點到點連續路徑規劃。借助雙目手眼相機完成非合作目標位姿的提取，進而完成視覺伺服系統的搭建。本文算法可將機械臂控制算法引入到空間機器人視覺伺服系統，使得機器人控制更加方便，具有結構簡單成本低等優點。通過搭建SimMechanics仿真模型，實現了對期望軌跡的跟蹤，驗證了視覺伺服算法的正確性。

空間機器人；視覺伺服；自由漂浮；動力學；雙目視覺

隨著科學技術水平的發展，每年發射進入太空的航天器數目在逐漸增大。UCS(union of concerned scientists)衛星數據顯示[1]，截止到2016年1月1日，太空中活動衛星的數量為1 381，其中493顆衛星運行于同步地球軌道。這些活動衛星中大約有68%的衛星處于不受控狀態，如廢棄衛星、火箭殘留物以及太空垃圾[2]。每年都有衛星由于發射失敗而無法正確進入軌道，從而造成大量的經濟損失。此類衛星經過在軌維護，大多可以繼續服務。

作為主要的在軌維護設備，空間機器人能夠勝任衛星維修、能源補充、貨物運輸等多項太空任務[3-8]。隨著太空任務的復雜化，宇航員的操作風險大大提高，利用空間機器人代替宇航員完成空間工作已成為當前空間探索的新趨勢。中國將在未來五年內建成我國獨立自主研發的空間站，空間站各艙段之間的組裝依賴的也是空間機器人。

空間機器人是一個強非線性系統，其運動學與動力學之間存在著動力學耦合[9]。空間微重力環境下對其進行實時控制存在很大困難，同時風險高、難度大。視覺伺服的引入使得空間機器人在執行空間任務時變得智能化，能夠根據不同的環境采取不同的控制策略，從而能夠勝任更為復雜與多種多樣的空間任務。視覺伺服首先通過視覺信息獲取機器人當前位姿與期望位姿的偏差，之后將其作為控制器的輸入來規劃各個關節的運動，進而實現對目標衛星的跟蹤捕獲任務。

研究自由漂浮空間機器人的視覺伺服系統既能使其具有感知空間環境的能力，同時也豐富了空間機器人系統建模方法。它能夠有效的提高機械臂的控制性能，使其適應復雜多變的太空環境。根據相機數量的不同，視覺伺服可分為單目視覺伺服、雙目視覺伺服與多目視覺伺服；根據相機安放位置的不同，視覺伺服可分為手眼相機系統與全局相機系統；根據偏差信號的選擇，視覺伺服又可分為基于位置的視覺伺服、基于圖像的視覺伺服及混合視覺伺服系統[10]。

本文以自由漂浮空間機器人視覺伺服為主要研究目標，選取基于位置的雙目手眼視覺伺服系統，無需合作靶標即可實現對非合作目標衛星的位姿重建，同時回避了目標遮擋、圖像雅克比奇異、相機回退等問題，控制器設計相對簡單。假定圖像處理模塊能夠精確獲取目標位姿信息，本文研究了自由漂浮空間機器人視覺伺服系統的組成，詳細分析了其進行運動學與動力學建模。通過在基于位置的傳統視覺伺服算法的基礎上添加空間機器人動力學模塊，建立了自由漂浮空間機器人視覺伺服閉環系統。

1 空間機器人模型

自由漂浮空間機器人的一般模型如圖1所示，整個系統可視為由衛星基座及n自由度機械臂組成。

注：B0為空間機器人的衛星基座，Ji為關節i，ri∈R3為連桿i的質心位置矢量，pi∈R3為Ji的位置矢量，pe∈R3為機械臂末端的位置矢量，ai∈R3為第i個機械臂連體系與其質心系的相對位置矢量，bi∈R3為第i個機械臂質心系至第i+1個機械臂連體系的相對位置矢量圖1 空間機器人模型Fig.1 The general model of space robot

本文所研究的空間機器人虛擬樣機模型如圖2所示，采用后置DH坐標系對其進行建模分析，各坐標系方向如圖中所示，初始狀態下機械臂對應的DH參數表如表1所示。

圖2 空間機器人虛擬樣機模型Fig.2 Virtual prototyping of space robot

連桿iθi/(°)di/mmai-1/mmαi-1/(°)1000-902-45080003000-9040600090522500906010000

為分析方便，本文對空間機器人的相關動力學參數作理想化處理，同時假設初始狀態下，慣性系與基座系重合。

2 運動學建模

自由漂浮空間機器人的末端位姿受關節角與基座姿態的共同影響。非完整約束特性又使得基座的姿態與關節角的運動歷史相關，故對于自由漂浮空間機器人，無法通過解析法得到位置級的運動學方程。下面將分別從正、逆運動學兩方面對自由漂浮空間機器人進行運動學建模。

2.1 正向運動學建模

正向運動學是指已知空間機器人相關參數及關節角速度，求解機械臂末端的速度參數。本文采用Whitney矢量積法完成正向運動學建模[11]。

自由漂浮空間機器人的正向運動學方程可表示為

(1)

式中：

在自由漂浮狀態下，空間機器人系統的線動量與角動量守恒。考慮整個系統相對于其本體質心的角動量，并假設其初值均為零，能夠得到：

(2)

從而基座速度與關節角速度之間存在以下變換關系：

(3)

將式(3)代入式(1)，從而得到自由漂浮模式下的空間機器人正向運動學方程：

(4)

2.2 逆向運動學建模

逆向運動學是指將空間機器人相關速度參數由笛卡爾空間轉換至關節空間，本文通過對廣義雅克比矩陣求偽逆來計算其逆運動學。

由2.1節可知：

(5)

廣義雅可比矩陣Jg(Ψ0,Θ,mi,Ii)為6×n維矩陣。當n≠6時，無法直接對該矩陣求逆。考慮動力學奇異現象，對該矩陣的求逆操作變得更加復雜。

針對以上問題，本文首先對矩陣Jg進行奇異值分解：

Jg=UDVT

(6)

式中：U∈Rm×m、V∈Rn×n為正交矩陣，D∈Rm×n是以Jg的奇異值σ從大到小排列的對角矩陣，則廣義雅克比矩陣偽逆的表達式為

(7)

式中：

此時空間機器人逆運動學方程可表示為

(8)

當σi接近于0時，計算得到的關節角速度容易超限。針對該現象，定義廣義雅可比矩陣的阻尼最小方差逆：

(9)

式中:λ為阻尼系數，用來協調末端跟蹤精度及關節角速度。為減小末端跟蹤誤差，當機械臂遠離奇異構形時，應減小相應的阻尼系數。則自由漂浮空間機器人逆向運動學方程如下：

(10)

3 動力學建模

空間機器人動力學建模是其仿真實驗、路徑規劃的基礎。自由漂浮空間機器人處于失重狀態，是典型的無根樹系統。本文采用鉸接體算法完成自由漂浮空間機器人的動力學建模[12]。

鉸接體是指具有相對運動的剛體通過鉸鏈組成的組合體。通過引入了6D空間矢量，鉸接體算法將牛頓方程和歐拉方程結合到一起，既使得動力學方程變得簡潔、直觀，也提高了編程效率。

3.1 正向動力學建模

通過添加一個與慣性系相連的6自由度的虛擬鉸鏈，可將自由漂浮空間機器人等效為一個固定基座機械臂。

自由漂浮空間機器人以基座為初始端的鉸接體不受外力，則有下式成立：

(11)

進而求得衛星基座的加速度參數，由基座至末端機械臂逐步遞推，即可求得各關節的角加速度，算法流程圖如圖3所示。

圖3 鉸接體算法流程圖Fig.3 The flow chart of articulated-body algorithm

算法可分為以下三部分，6D空間矢量的引入使得方程表達式更為簡潔，同時利用四元數來表示空間機器人基座的位姿參數以避免歐拉角奇異問題。

1)運動學外推。

由基座至末端逐步計算空間機器人各關節的速度與偏向力：

2)動力學內推。

由末端至基座逐步計算空間機器人各臂桿的慣量矩陣與鉸接體偏向力：

3)角加速度外推。

由基座至末端逐步計算空間機器人衛星基座的加速度與各關節的角加速度：

3.2 逆向動力學建模

逆向動力學建模是指已知機器人各關節的角加速度，求解關節力矩的方法。本文自由漂浮空間機器人逆向動力學建模采用的是牛頓-歐拉遞推算法，該算法是單處理器上計算效率最高的算法，其算法流程圖如圖4所示。

圖4 牛頓-歐拉算法流程圖Fig.4 The flow chart of Newton-Euler algorithm

算法可分為以下三部分：

1)運動學外推

由基座至末端逐步計算空間機器人各關節的速度、加速度與偏向力。

2)動力學內推

由末端至基座逐步計算空間機器人各桿件的慣量矩陣與鉸接體偏向力。

3)關節力矩外推

由基座至末端逐步計算空間機器人衛星基座的加速度與各關節的關節力矩。

4 空間機器人路徑規劃

本文主要研究基于逆運動學的空間機器人笛卡爾空間點到點直線路徑規劃。根據計算方向的不同，機器人路徑規劃可分為基于正運動的路徑規劃與基于逆運動學的路徑規劃。基于正運動學的路徑規劃首先利用多項式函數對關節軌跡進行擬合，之后通過正運動學得到關于末端軌跡的非線性方程組，進而采用牛頓迭代法、遺傳算法及粒子群算法等方法完成對關節軌跡的求解。基于逆運動學的路徑規劃算法通過求解機械臂末端速度，借助逆運動學直接求解得到關節角速度，具有計算量小、效率高等優點，可用于實時計算。

首先假設機械臂末端初始指向偏差為

(12)

式中：nt、ot、at為旋轉矩陣表示的末端姿態。

同時假設空間機器人機械臂末端位姿軌跡：

(13)

兩端微分后得到末端速度表達式：

(14)

聯立式(10)，則規劃后的關節角速度曲線為

(15)

5 關節控制器設計

在機械臂處于低速運動時，可忽略離心力、科氏力以及臂桿間的耦合力矩的影響，將空間機器人系統近似視為解耦的線性系統，此時可采用基于n個獨立關節的PD控制。而當各關節速度加快時，由于補償效果變差，獨立關節的PD控制的穩定性也變差。本文采用PD與前饋控制來設計關節控制器，前饋項由逆向動力學計算得到，其控制率如下：

(16)

式中：kip表示比例增益，它能夠提高系統的穩態精度，但同時會增大系統的震蕩次數。kid表示微分增益，它能夠減小系統的超調及調節時間，實現超前控制，但同時系統的動態性能變差。τid表示由逆動力學算法計算得到的各關節期望力矩。

PD與前饋控制框圖如圖5所示，基于空間機器人模型的計算力矩模塊獨立于伺服回路之外，能夠以較低速率運行，回路內部只需計算偏差信號帶來的增益，計算量小，能以較高的伺服速率運行。

圖5 PD與前饋控制框圖Fig.5 The PD and feedforward control

6 自由漂浮空間機器人視覺伺服系統

自由漂浮空間機器人視覺伺服系統的組成，其流程圖如圖6所示。

圖6 視覺伺服系統流程圖Fig.6 The visual servo system of space robot

各組成模塊分別實現以下功能：

1)成像系統。

雙目手眼相機實時采集非合作目標圖像，利用標定好的雙目相機參數完成對雙目圖像的立體校正，使其滿足對極幾何的要求。經過立體校正，圖像立體匹配時將由全平面的二維搜索變為沿極線方向的一維搜索，降低了計算量。

2)特征提取。

首先利用衛星噴管特征實現對非合作目標的粗定位，進而提取衛星基座邊緣的角點特征。為了計算非合作目標的相對位姿，應至少提取3組對應的角點特征。經過特征提取，圖像間的立體匹配變為了若干個像素點的匹配問題，大大減少了特征匹配時間，使得視覺伺服算法符合空間機器人實時控制的要求。

3)立體重建。

對特征提取得到的角點進行立體匹配，獲取相應的視差數據。根據三角測距原理，借助重投影矩陣完成特征點的三維坐標計算。非共線的3個空間點將確定唯一的圖像平面，進而求解衛星基座位姿。

4)PBVS控制器。

以上模塊借助雙目手眼相機完成了對非合作目標位姿的提取，從而可將其視為一個視覺位姿傳感器，能夠實時提供關于非合作目標衛星的當前位姿數據。PBVS控制器結合目標的期望位姿與當前位姿規劃空間機器人機械臂末端的運動，輸出期望的末端速度。

5)逆向運動學。

采用阻尼最小方差二乘法求解廣義雅克比矩陣的偽逆，進而將速度由笛卡爾空間轉換至關節空間，積分后得到相應的關節角。

6)關節控制器。

關節控制器采用PD與前饋控制算法。經由逆向動力學模塊計算期望的關節力矩，并將其作為控制器的前饋項。同時利用關節傳感器采集當前的關節角及角速度，對其采取PD控制，輸出實際的關節力矩。

7)正向動力學。

采用鉸接體算法計算實際的關節角加速度，積分后得到實際的關節角度與角速度。

8)正向運動學。

采用基于Whitney矢量積法的正向運動學求解自由漂浮空間機器人機械臂末端的實際速度，積分后得到當前位姿。

整個視覺伺服過程可等效為慣性系下由目標當前位姿到期望位姿的笛卡爾空間點到點的連續路徑規劃，其中當前位姿由雙目手眼相機計算得到。下面，將通過如圖7所示的SimMechanics仿真模型來驗證本文提出的自由漂浮空間機器人視覺伺服算法的有效性。SimMechanics是機器人建模過程中常用的仿真工具，通過搭建與系統等效的模塊框圖，使機器人的系統建模變得更加方便直觀。

圖7 空間機器人SimMechanics仿真模型Fig.7 The SimMechanics model of space robot

表2 視覺伺服系統參數表

圖8 空間機器人各關節角度跟蹤曲線Fig.8 Comparison in attitude angle of space robot

7 結論

本文主要研究了自由漂浮空間機器人視覺伺服系統的組成。采用PD與前饋控制分析了其笛卡爾空間的點到點連續路徑規劃，通過搭建SimMechanics仿真模型，驗證了本文算法的正確性。

1)本文提供了一種通用的自由漂浮空間機器人建模方法，但未考慮實際環境下視覺算法時延的影響；

2)在傳統的基于位置的視覺伺服算法的基礎上添加空間機器人動力學模塊，更貼合實際模型；

3)通過調整關節控制器，還可將各種機器人控制算法引入視覺伺服過程中去，具有結構簡單、算法靈活、成本低等優點。

此外，本文僅以點到點的路徑規劃為例分析了閉環系統的組成，后續工作中應分析不同路徑規劃以及機械臂奇異構型時對視覺伺服系統帶來的影響，進一步擴展其實際應用價值。

[1]XU Wenfu, LIANG Bin, LI Bing, et al. A universal on-orbit servicing system used in the geostationary orbit[J]. Advances in space research, 2011, 48(1): 95-119.

[2]CHEN Long, GUO Baolong, SUN Wei. Relative pose measurement algorithm of non-cooperative target based on stereo vision and RANSAC[J]. International journal of soft computing and software engineering, 2012, 2(4): 26-35.

[3]劉業超. 柔性關節機械臂控制策略的研究[D]. 哈爾濱: 哈爾濱工業大學, 2009: 3-13. LIU Yechao. Research on the control strategy for the flexible-joint robot arm[D]. Harbin: Harbin Institute of Technology, 2009: 3-13.

[4]劉陽. 面向非合作目標的空間機器人雙目視覺伺服研究[D]. 哈爾濱: 哈爾濱工業大學, 2015: 1-7. LIU Yang. Binocular vision system of space robot for non-cooperative target[D]. Harbin: Harbin Institute of Technology, 2015: 1-7.

[5]DUBOWSKY S, PAPADOPOULOS E. The kinematics, dynamics, and control of free-flying and free-floating space robotic systems[J]. IEEE transactions on robotics and automation, 1993, 9(5): 531-543.

[6]VAFA Z, DUBOWSKY S. The kinematics and dynamics of space manipulators: the virtual manipulator approach[J]. International journal of robotics research, 1990, 9(4): 3-21.

[7]陳治科, 熊偉, 劉德生, 等. 在軌操作技術及國外發展分析[J]. 裝備學院學報, 2014, 25(6): 63-68. CHEN Zhike, XIONG Wei, LIU Desheng, et al. Analysis of on-orbit manipulation technology and its overseas development[J]. Journal of equipment academy, 2014, 25(6): 63-68.

[8]梁斌, 劉良棟, 李庚田, 等. 空間機器人的動力學等價機械臂[J]. 自動化學報, 1998, 24(6): 761-767. LIANG Bin, LIU Liangdong, LI Gengtian, et al. Dynamically equivalent manipulator for a space manipulator[J]. Acta automatica sinica, 1998, 24(6): 761-767.

[9]ZHANG Yu, SUN Kui, LIU Hong, et al. Pose measurement of nozzle based on laser range finders for capturing satellite[J]. Proceedings of the institution of mechanical engineers, 2016, 230(8): 1385-1396.

[10]蔡勇. 弧焊機器人焊縫跟蹤系統與位移運動學研究[D]. 西安: 西安理工大學, 2007: 8-10. CAI Yong. Welding seams tracing system on robot and its kinematics research[D]. Xi'an: Xi'an University of Technology, 2007: 8-10.

[11]曾岑. 在軌服務空間機械臂運動及任務規劃方法研究[D]. 大連: 大連理工大學, 2013: 33-38. CENG Cen. Research on motion and mission planning methods for on-orbit servicing space manipulator[D]. Dalian: Dalian University of Technology, 2013: 33-38.

[12]FEATHERSTONE R, ORIN D. Robot dynamics: equations and algorithms[C]//Proceedings of 2000 IEEE International Conference on Robotics and Automation. San Francisco, CA: IEEE, 2000, 1: 826-834.

Research on the visual servo system of a free-floating space robot

LIU Yang，XIE Zongwu，WANG Bin，LIU Hong，CAI Hegao

(State Key Laboratory of Robotics and System, Harbin Institute of Technology, Harbin 150001, China)

In order to add the dynamic model of the space robot to the traditional visual servo algorithm and enhance the authenticity of the simulation system, this paper analyzes the composition and principle of a free-floating space robot, focusing on its visual servo system. Generalized Jacobian matrix was used to complete kinematic model of the free-floating space robot, and the dynamics model was discussed under the condition of 6D spatial vector. Based on PD and feedforward control, the path planning of the robot was programmed in Cartesian space. A binocular hand-eye camera system was built to extract the pose of the non-cooperative satellite which further used to complete the visual servo system. Various robot control algorithms can be introduced to the visual servo system that makes it more convenient and takes advantage of simple mechanism with low cost. By building a SimMechanics model, the expected trajectory had been tracked successfully which proved the visual servo system right.

space robot; visual servo; free-floating; dynamics; binocular vision

2016-05-09.

日期： 2016-11-16.

國家重點基礎研究發展計劃(2013CB733105).

劉陽(1990-), 男, 博士研究生； 謝宗武(1973-), 男, 教授,博士生導師; 王濱(1973-), 男, 副教授; 劉宏(1966-),男,教授，博士生導師，“長江學者計劃”特聘教授; 蔡鶴皋(1934-), 男, 教授，博士生導師，中國工程院院士.

王濱，E-mail:wbhit@hit.edu.cn.

10.11990/jheu.201605027

http://www.cnki.net/kcms/detail/23.1390.u.20161116.1613.002.html

TP242.3

A

1006-7043(2017)02-0153-07

劉陽, 謝宗武, 王濱, 等. 自由漂浮空間機器人視覺伺服系統研究[J]. 哈爾濱工程大學學報， 2017, 38(2): 153-159. LIU Yang，XIE Zongwu，WANG Bin， et al. Research on the visual servo system of a free-floating space robot[J]. Journal of Harbin Engineering University, 2017, 38(2): 153-159.