多傳感信息融合機器人標定方法

張湧濤，金 爽，王 一

（華北理工大學電氣工程學院，河北 唐山 063009）

多傳感信息融合機器人標定方法

張湧濤，金 爽，王 一

（華北理工大學電氣工程學院，河北 唐山 063009）

該文提出一種基于多傳感信息融合技術進行機器人運動學標定的方法。首先通過對機器人的指數積（POE）正向運動學模型取微分的方式建立末端執行器的誤差模型，利用ROMER-RA7520絕對關節臂測量機和安裝在末端工具上的多個傳感器分別采集末端執行器的位置和姿態信息；隨后根據坐標統一原理將測量機的位置測量數據和多傳感器的姿態量測數據轉換到機器人基礎坐標系下，實現位姿數據的空間配準；接著運用自適應加權融合算法融合處理經過空間配準后的位姿數據，得到末端執行器位姿測量值；最后應用迭代最小二乘法求解出參數偏差。KR5arc機器人標定仿真實驗表明：該方法可大幅度提升機器人在任意位姿下的定位、定姿精度。

指數積；測量臂；空間配準；多傳感信息融合；參數標定

0 引 言

工業機器人由于受機構本身加工、裝配誤差以及日常使用過程中的磨損等因素的影響，其絕對定位精度僅達毫米級，無法達到一些工業生產中對定位精度的要求。因此，如何有效消除或減小這些誤差，成為機器人在精密加工領域應用所面臨的首要問題。采用高精度的生產設備來提高機器人零部件的加工精度是提高機器人定位精度的有效方法，但成本太高，而通過修改機器人的運動學參數來提升其絕對定位精度是一種行之有效的方法，并且比較容易實現[1-3]。

機器人標定按照是否需要建立運動學模型分為兩類：運動學模型標定和自標定。運動學模型標定方法一般由誤差建模、測量、辨識和補償4個環節組成[4]。國內外學者對于機器人運動學標定問題已經展開了研究工作，如從建模方法出發，旨在建立更為準確且參數連續的運動學模型；其中，D-H參數模型由于在計算過程中所需的參數個數最少，因而得到了廣泛應用，但D-H參數模型也有其局限性，表現在當相鄰關節的軸線近乎平行時模型存在奇異性問題。對此，一些學者提出了改進的D-H參數模型或其他的參數模型來解決該問題，但也帶來了建模過程復雜、缺少通用性等新問題[5-6]。Park針對串聯機器人提出了一種指數積模型，不僅實現了轉動關節與移動關節的統一描述，而且運動學參數關于關節軸的變化是連續、光滑的，避免了D-H參數法存在的奇異性問題[7]。也有研究將目光集中于位姿測量方法，如利用激光跟蹤儀、三坐標測量機等高精度測量設備獲得機器人實際位姿，該方法測量精度高，但只能測量給定點的位姿信息且成本較高。或通過將機器人工作空間離散化，建立空間網格精度模型的方法對位置和姿態分別進行標定，該方法只能確保機器人在被標定空間內具有較好的標定效果。

近年來，多傳感信息融合技術以其觀測范圍廣、提供的信息可信度高、可進行在線實時測量以及成本低等優點受到廣泛關注，并在狀態監測與故障檢測、空間目標識別與跟蹤、機器人避障與導航以及圖像處理等領域發揮了重要的作用[8]。本文提出將該項技術應用于機器人運動學參數標定，通過分析探究數據融合過程中涉及的量測數據預處理技術以及融合算法的選擇實現提升機器人定位精度的目的。

1 誤差模型線性化

n自由度串聯結構機器人正向運動學的指數積（POE）公式為

θi——第i個關節的關節變量；

gst（0）——各關節均處于零位時按基礎坐標系描述的末端執行器的位姿矩陣。

gst（0）還可以表示為某個常旋量st∈se（3）的指數形式，則式（1）可改寫為

假設機器人的各個關節均存在運動學參數誤差，并用δξ、δξst和δθ分別代表關節旋量坐標偏差、初始位姿變換旋量坐標偏差和關節變量零位偏差，對式（2）取微分可得誤差模型：

其中：

關節變量零位偏差可看作是旋量坐標偏差的一個誤差源，無需單獨辨識[7]。因此，誤差模型（3）可簡寫為

將式（2）代入誤差模型式（4）中，經過化簡可知：

將式（6）、式（7）代入式（5）可知（δg·g-1）∨的顯式表達式為

對于式（4）的左側，令ga為測量得到的機器人末端執行器的實際位姿，gn為末端執行器名義位姿，則δgg-1反映了按基礎坐標系描述的從ga到gn的偏差，當兩者相差足夠小時存在如下關系：

2 多傳感信息融合

多傳感信息融合流程如圖1所示，通常由原始數據采集、量測數據預處理、數據關聯、數據決策以及信息融合5個階段構成[8]。假如傳感器的量測數據均來自于同一被測目標，則不必對其實施數據關聯和數據決策。

圖1 多傳感器信息融合流程圖

2.1 量測數據預處理和坐標統一

2.1.1 量測數據預處理

在本文設計的多傳感器信息融合系統中，各傳感器的初始采樣時刻、采樣頻率設置相同，并且假設通信網絡的傳輸延遲可忽略，則該融合系統的量測數據可看作是同步的，不必進行時間配準。此外，由于參與融合的各傳感器量測數據都是相對于自身坐標系描述的，因此需要將其轉化到機器人基礎坐標系中，即對其進行空間配準[11]。

多傳感器量測數據空間姿態配準實驗設計如下：設末端執行器在機器人基礎坐標系下和傳感器坐標系下的姿態角分別為ra和rb，則兩者之間的變換關系可表示為

其中 ra=（γa，βa，αa）T，rb=（γb，βb，αb）T，R 為傳感器坐標系與機器人基礎坐標系之間的姿態變換矩陣。

R的數學表達式確定原理為：若傳感器相對于機器人基礎坐標系的偏轉角、俯仰角和側傾角分別為Φ、η和φ，則傳感器坐標系與機器人基礎坐標系之間的姿態變換矩陣可表示為

該矩陣為正交矩陣，即滿足：

2.1.2 位置坐標統一

機器人的位置信息是通過ROMA-RA7520絕對關節臂測量機實現。位置坐標統一實驗設計如下：首先驅動機器人運動到工作空間中的一系列標定位姿，通過控制器讀取并記錄機器人處于各個位姿時所對應的末端執行器位置理論值；然后利用測量機探測不同位姿所對應的末端執行器位置測量值，由于測量臂具很高精度，因此可認為該測量值即為在測量臂坐標系下描述的末端執行器位置實際值；最后根據坐標同一原理建立測量臂坐標系與基礎坐標系之間的齊次變換矩陣，將測量臂坐標系下的量測數據轉化到機器人基礎坐標系下，即可得到相對于基礎坐標系描述的末端執行器位置實際值。

坐標統一原理為：假設空間中某點在直角坐標系OaXaYaZa下的坐標向量為 Pa=（xa，ya，za）T，在直角坐標系 ObXbYbZb下的坐標向量為 Pb=（xb，yb，zb）T，則兩者之間的坐標變換可表示為

Tab=[txtytz]T——描述位置的平移向量。

式（14）給出了6個正交約束方程，故式（13）提供3組或3組以上坐標向量即可確定位姿變換矩陣Hab。

位置坐標統一實驗設計如下：首先驅動機器人運動到工作空間中的一系列標定位姿，然后通過控制器讀取并記錄機器人處于各個位姿時所對應的末端執行器位置理論值，接著利用測量機探測不同位姿所對應的末端執行器位置測量值，由于測量臂是一種高精度的測量設備，因此可認為該測量值即為在測量臂坐標系下描述的末端執行器位置實際值，最后根據坐標同一原理建立測量臂坐標系與基礎坐標系之間的齊次變換矩陣，將測量臂坐標系下的量測數據轉化到機器人基礎坐標系下，至此，即可得到相對于基礎坐標系描述的末端執行器位置實際值。

2.2 信息融合

目前，信息融合算法主要包括加權平均法、卡爾曼濾波法、D-S證據理論、Bayes推理法、神經網絡等。本文在分析了現有融合算法特點的基礎上，提出采用自適應加權融合算法[12]對經過配準的傳感器量測數據進行融合處理。自適應加權融合算法模型如圖2所示。

圖2 自適應加權融合算法模型

圖中，X1，X2，…，Xn表示 n 個傳感器對于同一對象相同參數的量測數據，為加權融合后的值。

2.2.1 最優加權因子及其方差

假設測量系統由兩個傳感器組成，傳感器A和B的方差分別為σ12和σ22，且對應的加權因子分別為W1和W2，X1和X2為兩傳感器的量測數據，兩者相互獨立且都是真值X的無偏估計。由于待估真值X為常量，因此需通過傳感器歷史量測數據的均值來估計對象參數的真值，即存在如下關系：

式中：m——傳感器i的歷史量測數據的個數；

引進加權因子W1和W2后，兩傳感器量測數據的融合結果為

此時，傳感器系統總方差為

由于 X1和X2都是真值X 的無偏估計，因此1（m）和2（m）同樣滿足這一條件，則式（17）可簡寫為

2.2.2 最優加權因子的求取

由式（19）可知，求取最優加權因子的前提是先求得各傳感器的方差，而方差σi2可根據各傳感器的測量值，通過計算得到。設兩相互獨立傳感器的測量誤差分別為ei和ej，則測量值Xi和Xj可用下式表示：

其中ei和ej為零均值高斯白噪聲。

則兩傳感器的方差可表示為

由于兩傳感器相互獨立且ei和ej為零均值高斯白噪聲，故Xi和Xj的互協方差函數Rij可化簡為

Xi和Xj的自協方差函數Rii滿足：

由式（22）、式（23）可知式（21）改寫為

式中Rii和Rij可由各自的時間域估計值確定。

假設 Rii（k）和 Rij（k）分別代表 Rii和 Rij的時間域估計值，則有：

同理可知：

3 參數辨識

式（8）可寫成如下矩陣形式：

式中：

矩陣H中的各項具有如下形式：

驅動機器人運動到工作空間中的m個測量位姿，將產生m組形如式（27）的誤差方程，將此m組誤差方程合并即可得到標定方程組：

其中：

旋量坐標誤差可由迭代最小二乘法解得：

4 參數標定流程

控制機器人運動到工作空間中的m個標定位姿，通過控制器讀取各關節變量值，并代入式（1）計算末端執行器位姿名義值，通過融合處理多個傳感器提供的量測數據獲得末端執行器位姿測量值，參數迭代識別過程中，將旋量坐標名義值作為迭代計算初值，關節旋量坐標偏差δξi和初始位姿變換旋量坐標偏差δξst可根據式（29）確定，最后按照圖 3所示迭代識別過程完成旋量坐標辨識。為保證更新后的關節旋量坐標仍然滿足關節約束條件，需要在每次迭代計算過程中對旋量坐標進行一次正交化處理。

圖3 參數迭代識別過程

5 標定實驗

為了檢驗參數標定方法的有效性，通過KR5arc串聯機器人標定實驗進行驗證。KR5arc機器人由剛性連桿以及連接相鄰連桿的6個轉動關節和基座構成，機器人的初始關節角度 θ1～θ6依次為 0°，90°，-90°，0°，0°，0°，機器人連桿參數如圖4所示，由此可計算出各關節旋量坐標的理論值如表1所示。

首先驅動機器人運動到工作空間中的50個標定點，通過控制器讀取和記錄每個標定點所對應的關節角度值，并根據表2所示旋量坐標的理論值，按式（2）計算末端執行器的名義位姿，計算此50個標定點處所對應的多傳感器系統的融合輸出值，最后按照圖3所示迭代識別過程完成旋量坐標辨識，并根據辨識結果修正機器人的運動學參數。參數迭代識別過程中，以旋量坐標名義值為計算初值，辨識結果如表1所示。

圖4 KR5arc機器人連桿參數（單位：mm）

1）姿態校準效果驗證實驗。首先，在機器人的工作空間中隨機取30個標定點以外的點作為驗證點，利用航姿參考系統和雙軸傾角傳感器采集這30個點處的機器人姿態信息并對其進行融合處理，將融合結果作為機器人姿態角實際值；根據式（2）及表1所示旋量坐標理論值解算出這30個點在基礎坐標系下的姿態角理論值，然后解算出補償后的姿態角；最后分別將標定前和標定后機器人的姿態角與其實際值做差可得姿態校準效果圖，如圖5所示。

2）位置校準效果驗證實驗。①對上述30個驗證點進行編號，測量其在測量臂坐標系下的坐標，并按照編號順序依次計算相鄰兩點之間的距離，由于測量臂是一種高精度的測量設備，因此可將其測量結果作為真實值；②根據式（2）以及表1所示旋量坐標理論值計算這30個點在基礎坐標系下的位置理論值，并按照編號順序依次計算相鄰兩點之間的距離；③根據式（2）以及表1所示旋量坐標辨識結果計算補償后這30個點在基礎坐標系下的位置坐標，并按照編號順序依次計算相鄰兩點之間的距離；④將①與②計算得到的結果做差可知標定前機器人的距離誤差，將①與③計算得到的結果做差可知標定后機器人的距離誤差，結果如圖6所示。

表1 旋量坐標值

6 結束語

本文基于指數積公式建立了機器人的正向運動學模型，該模型避免了D-H參數法存在的奇異性問題，實現了轉動關節與移動關節的統一描述。首先通過測量機和安裝在末端工具上的多個傳感器分別采集末端執行器的位置和姿態信息，然后根據坐標統一原理將測量機的位置測量數據和多傳感器的姿態量測數據轉換到機器人基礎坐標系下，實現量測數據的空間配準，接著根據自適應加權融合算法對經過配準后的量測數據進行融合處理，該算法可確保融合后傳感器系統的總方差小于組成測量系統的各傳感器的方差。最后通過KR5arc串聯機器人標定仿真實驗驗證了標定方法的有效性。

圖5 姿態角校準效果圖

圖6 KR5arc機器人距離校準效果圖

[1]高文斌，王洪光，姜勇，等.基于距離誤差的機器人運動學參數標定方法[J].機器人，2013，35（5）：600-606.

[2]陳鋼，賈慶軒，李彤，等.基于誤差模型的機器人運動學參數標定方法與實驗[J].機器人，2012，34（6）：680-688.

[3]李睿，曲興華.工業機器人運動學參數標定誤差不確定度研究[J].儀器儀表學報，2014，35（10）：2192-2199.

[4]XUAN J Q，XU S H.Review on kinematics calibration technology of serial robots[J].International Journal of Precision Engineering and Manufacturing，2014，15 （8）：1759-1774.

[5]VEITSCHEGGER W K，WU C H.Robot accuracy analysis based on kinematics[J].IEEE Journal on Robotics and Automation，1986，2（3）：171-179.

[6]ZHUANG H， ROTH Z S， HAMANO F.A complete and parametrically continuous kinematic model for robot manipulators[J].IEEE Transactions on Robotics and Automation，1992，8（4）：451-463.

[7]高文斌，王洪光，姜勇.一種基于指數積的串聯機器人標定方法[J].機器人，2013，35（2）：156-161.

[8]蔣雅娜，王幫峰，李迎，等.多傳感器信息融合技術及其在智能材料結構中的應用[J].中國測試技術，2003，29（5）：22-23.

[9]HE R B， ZHAO Y J， YANG S N.Kinematic-parameter identification for serial-robot calibration based on POE formula[J].IEEE Transactions on Robotics，2010，26（3）：411-423.

[10]LOU Y J， CHEN T N， WU Y Q， et al.Improved and modified geometric formulation of POE based kinematic calibration of serial robots[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems Piscataway NJ USA： IEEE，2009：5261-5266.

[11]葉宏.多傳感器系統配準算法研究[D].綿陽：中國工程物理研究院，2014.

[12]丁輝，仲躍，張俊，等.基于相關性函數的多傳感器自適應加權融合算法 [J].重慶理工大學學報 （自然科學版），2016，30（2）：114-118.

（編輯：莫婕）

Robot calibration method based on multi-sensor information fusion

ZHANG Yongtao，JIN Shuang，WANG Yi
（College of Electrical Engineering，North China University of Science and Technology，Tangshan 063009，China）

A method of robot kinematics calibration based on multi-sensor information fusion technology is proposed.Firstly，an error model of end effector is established by differentiating the POE forward kinematics model of robot，and then ROMER-RA7520 absolute articulated arm measuring machine and many sensors mounted at the end tool are used to collect the end effector position and attitude information.After that，the position measurement data of the measuring machine and the attitude measurement data of multi-sensor will be converted to the robot base coordinate system according to unity principle of the coordinate to realize the special alignment of position and attitude data.And then，the adaptive weighted fusion algorithm is used to have fusion processing of the position and attitude data after special alignment and get the measured position and attitude data of end effector.Finally， iterative least square method is used to get the parameter deviation.KR5arc robot calibration simulation experiment shows that the method can greatly improve the positioning and attitude determination accuracy of the robot under any position and attitude.

product of exponential； measuring arm； space alignment； multi-sensor information fusion；parameter calibration

A

1674-5124（2017）04-0056-07

10.11857/j.issn.1674-5124.2017.04.013

2016-09-18；

2016-10-25

國家自然科學基金青年科學基金項目（51505125）

張湧濤（1958-），男，河北唐山市人，教授，主要從事檢測技術及智能裝置、控制理論在過程控制中的應用研究。