基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法
【專利摘要】基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法，解決了漂浮基座下空間機(jī)器人視覺(jué)測(cè)量時(shí)延誤差的補(bǔ)償問(wèn)題。包括確定系統(tǒng)的視覺(jué)時(shí)延，建立帶時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)與物理真實(shí)相對(duì)位姿間的數(shù)學(xué)關(guān)系；根據(jù)帶時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)和機(jī)械臂的關(guān)節(jié)指令，估計(jì)當(dāng)前的空間機(jī)器人末端速度；設(shè)計(jì)誤差控制器，減小空間機(jī)器人末端速度的估計(jì)誤差；根據(jù)校正后空間機(jī)器人末端速度估值，對(duì)當(dāng)前帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)進(jìn)行補(bǔ)償，得到經(jīng)過(guò)補(bǔ)償?shù)囊曈X(jué)測(cè)量數(shù)據(jù)。本發(fā)明利用歷史測(cè)量數(shù)據(jù)融合關(guān)節(jié)角速度指令估計(jì)當(dāng)前空間機(jī)器人末端速度，設(shè)計(jì)估計(jì)誤差控制器以減小速度估計(jì)的誤差；實(shí)現(xiàn)漂浮基座下空間機(jī)器人精確的視覺(jué)時(shí)延補(bǔ)償，有利于實(shí)現(xiàn)空間機(jī)器人高精度地完成精細(xì)操作任務(wù)。
【專利說(shuō)明】基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種空間機(jī)器人視覺(jué)測(cè)量時(shí)延誤差補(bǔ)償方法。
【背景技術(shù)】
[0002]漂浮基座下的空間機(jī)器人(包括載體航天器和機(jī)械臂)執(zhí)行空間任務(wù)時(shí)，面臨的一個(gè)實(shí)際問(wèn)題是其機(jī)械臂上的視覺(jué)測(cè)量系統(tǒng)，進(jìn)行視覺(jué)信息處理時(shí)往往需要消耗較長(zhǎng)的時(shí)間，其中一個(gè)原因是由于光照條件復(fù)雜，空間機(jī)器人所采用的視覺(jué)信息處理方法較復(fù)雜，另一個(gè)原因是能夠用于空間機(jī)器人視覺(jué)信息處理的處理器計(jì)算能力往往很有限，由于視覺(jué)信息方法復(fù)雜程度和視覺(jué)信息處理設(shè)備計(jì)算能力之間的矛盾導(dǎo)致了視覺(jué)測(cè)量信息往往具有較大的時(shí)延，表現(xiàn)在視覺(jué)測(cè)量精度上就表現(xiàn)為具有較大的測(cè)量誤差。由于視覺(jué)測(cè)量誤差的存在，使空間機(jī)器人在執(zhí)行目標(biāo)捕獲等任務(wù)時(shí)，會(huì)影響其操作精度。為了使空間機(jī)器人能夠更快速、平穩(wěn)的完成精細(xì)的在軌服務(wù)任務(wù)，在空間機(jī)器人系統(tǒng)設(shè)計(jì)過(guò)程中需要考慮空間機(jī)器人視覺(jué)時(shí)延誤差的補(bǔ)償問(wèn)題。
[0003]目前已經(jīng)有一些可用于地面固定基座機(jī)器人視覺(jué)時(shí)延誤差處理的預(yù)測(cè)和估計(jì)方法。最小二乘估計(jì)是以誤差的平方和最小為準(zhǔn)則，根據(jù)觀測(cè)數(shù)據(jù)估計(jì)線性模型中未知參數(shù)的一種基本參數(shù)估計(jì)方法。它的基本思路是選擇估計(jì)量使模型輸出與實(shí)測(cè)輸出之差的平方和達(dá)到最小。這種求誤差平方和的方式可以避免正負(fù)誤差相抵，而且便于數(shù)學(xué)處理?？柭鼮V波是一種線性最小方差估計(jì)，可以估計(jì)信號(hào)的過(guò)去和當(dāng)前狀態(tài)，甚至能估計(jì)將來(lái)的狀態(tài)。卡爾曼濾波預(yù)測(cè)模型從與被提取信號(hào)有關(guān)的測(cè)量值中通過(guò)方法估計(jì)出所需信號(hào)。其中被估計(jì)信號(hào)是由白噪聲激勵(lì)引起的隨機(jī)響應(yīng)，激勵(lì)源與響應(yīng)之間的系統(tǒng)方程己知，測(cè)量量與被估計(jì)量之間的函數(shù)關(guān)系也己知。Smith預(yù)估器經(jīng)常應(yīng)用于地面機(jī)器人視覺(jué)時(shí)延處理。Smith預(yù)估器的作用原理是通過(guò)引入時(shí)延矯正環(huán)節(jié)，對(duì)帶有測(cè)量時(shí)延的視覺(jué)數(shù)據(jù)進(jìn)行補(bǔ)償，從而將系統(tǒng)中的純時(shí)延部分分離到整個(gè)閉環(huán)系統(tǒng)之外，從而提高整個(gè)控制系統(tǒng)的控制性能。從Smith預(yù)估器的作用原理可以看出，Smith預(yù)估器設(shè)計(jì)的基礎(chǔ)在于必須保證能夠準(zhǔn)確的預(yù)測(cè)出被控對(duì)象在給定輸入狀態(tài)下的動(dòng)態(tài)特性，才能夠補(bǔ)償?shù)魷y(cè)量信息中的時(shí)延部分，因此Smith預(yù)估器非常依賴于建立精確的數(shù)學(xué)模型。漂浮基座下的空間機(jī)器人與地面機(jī)器人不同，由于空間機(jī)器人沒(méi)有固定的基座，機(jī)械臂的運(yùn)動(dòng)與載體航天器的運(yùn)動(dòng)耦合在一起，任何空間機(jī)械臂的運(yùn)動(dòng)都會(huì)改變載體航天器的位置和姿態(tài)，而載體航天器位置姿態(tài)的改變也會(huì)反過(guò)來(lái)影響空間機(jī)械臂的定位，因此空間機(jī)器人的動(dòng)力學(xué)計(jì)算非常復(fù)雜。不僅如此，隨著載體航天器燃料的不斷消耗，載體航天器的質(zhì)量、質(zhì)心位置和慣量矩陣都不斷在變化，精確的數(shù)學(xué)模型很難獲得，因此經(jīng)典的預(yù)測(cè)和估計(jì)方法很難在空間機(jī)器人應(yīng)用中獲得很好的效果O

【發(fā)明內(nèi)容】

[0004]本發(fā)明提供一種基于速度估計(jì)的漂浮基座下空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法，以減小空間機(jī)器人視覺(jué)系統(tǒng)因時(shí)延存在而產(chǎn)生的視覺(jué)測(cè)量誤差，有利于空間機(jī)器人基于補(bǔ)償后視覺(jué)信息完成精細(xì)的空間任務(wù)。
[0005]基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法由以下步驟完成:
[0006]步驟一、根據(jù)所采用的視覺(jué)處理算法和所應(yīng)用的硬件確定視覺(jué)系統(tǒng)的視覺(jué)時(shí)延，建立帶時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)與物理真實(shí)相對(duì)位姿(測(cè)量相機(jī)與目標(biāo))間的數(shù)學(xué)關(guān)系；步驟二、根據(jù)帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)和機(jī)械臂的關(guān)節(jié)指令，估計(jì)當(dāng)前空間機(jī)器人的末端速度(見(jiàn)式(26));步驟三、設(shè)計(jì)誤差控制器，減小空間機(jī)器人末端速度估值的誤差，得到校正后空間機(jī)器人末端速度估值(見(jiàn)式(30));步驟四、根據(jù)校正后空間機(jī)器人末端速度估值，對(duì)當(dāng)前帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)進(jìn)行補(bǔ)償，得到經(jīng)過(guò)補(bǔ)償?shù)囊曈X(jué)測(cè)量數(shù)據(jù)(見(jiàn)式(32))。
[0007]本發(fā)明具有以下有益效果:本發(fā)明方法通過(guò)空間機(jī)器人數(shù)學(xué)模型分析，給出了利用歷史測(cè)量數(shù)據(jù)融合關(guān)節(jié)角速度指令估計(jì)當(dāng)前空間機(jī)器人末端速度。本發(fā)明利用空間機(jī)器人視覺(jué)系統(tǒng)獲得的帶時(shí)延的測(cè)量數(shù)據(jù)和機(jī)械臂關(guān)節(jié)角速度指令，估計(jì)當(dāng)前空間機(jī)器人末端速度，同時(shí)設(shè)計(jì)估計(jì)誤差控制器，減小空間機(jī)器人末端速度估值的誤差，根據(jù)得到校正后空間機(jī)器人末端速度估值，可有效地補(bǔ)償由于時(shí)延引起的漂浮基座下空間機(jī)器人對(duì)目標(biāo)的視覺(jué)測(cè)量誤差，可保證空間機(jī)器人在軌高精度地完成精細(xì)操作任務(wù)。
[0008]本發(fā)明不需建立空間機(jī)器人精確的數(shù)學(xué)模型，利用數(shù)學(xué)模型分析得到的機(jī)械臂真實(shí)末端速度與帶時(shí)延末端速度的簡(jiǎn)化關(guān)系，進(jìn)行末端速度估計(jì)，并采用閉環(huán)控制的方法提高了空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償?shù)木?；該方法避免了?fù)雜的動(dòng)力學(xué)計(jì)算，計(jì)算簡(jiǎn)單，便于實(shí)現(xiàn)漂浮基座下空間機(jī)器人精確的視覺(jué)時(shí)延補(bǔ)償，有利于實(shí)現(xiàn)空間機(jī)器人高精度地完成精細(xì)操作任務(wù)，可用于空間機(jī)器人在軌維護(hù)、空間碎片清理、深空探測(cè)等空間應(yīng)用領(lǐng)域。
【專利附圖】

【附圖說(shuō)明】
[0009]圖1為本發(fā)明的視覺(jué)時(shí)延誤差控制系統(tǒng)框圖；圖2為本發(fā)明的流程圖；圖3為本發(fā)明空間機(jī)器人系統(tǒng)和各坐標(biāo)系示意圖；圖4為補(bǔ)償后視覺(jué)測(cè)量的估計(jì)數(shù)據(jù)與原始測(cè)量數(shù)據(jù)和真實(shí)相對(duì)位置對(duì)比圖，其中:圖4a為補(bǔ)償后視覺(jué)測(cè)量的估計(jì)數(shù)據(jù)與原始測(cè)量數(shù)據(jù)和真實(shí)相對(duì)位置在X軸方向的對(duì)比圖，圖4b為補(bǔ)償后視覺(jué)測(cè)量的估計(jì)數(shù)據(jù)與原始測(cè)量數(shù)據(jù)和真實(shí)相對(duì)位置在y軸方向的對(duì)比圖，圖4c為為補(bǔ)償后視覺(jué)測(cè)量的估計(jì)數(shù)據(jù)與原始測(cè)量數(shù)據(jù)和真實(shí)相對(duì)位置在z軸方向的對(duì)比圖；
[0010]圖5為補(bǔ)償后視覺(jué)測(cè)量數(shù)據(jù)估計(jì)誤差與未補(bǔ)償前視覺(jué)測(cè)量數(shù)據(jù)時(shí)延誤差的對(duì)比圖，其中:圖5a為補(bǔ)償后視覺(jué)測(cè)量數(shù)據(jù)估計(jì)誤差與未補(bǔ)償前視覺(jué)測(cè)量數(shù)據(jù)時(shí)延誤差在X軸方向的對(duì)比圖，圖5b為視覺(jué)測(cè)量數(shù)據(jù)估計(jì)誤差與未補(bǔ)償前視覺(jué)測(cè)量數(shù)據(jù)時(shí)延誤差在y軸方向的對(duì)比圖，圖5c為視覺(jué)測(cè)量數(shù)據(jù)估計(jì)誤差與未補(bǔ)償前視覺(jué)測(cè)量數(shù)據(jù)時(shí)延誤差在z軸方向的對(duì)比圖。
【具體實(shí)施方式】
[0011]【具體實(shí)施方式】一:結(jié)合圖1、圖2和圖3說(shuō)明本實(shí)施方式，本實(shí)施方式由以下步驟完成:一、根據(jù)所采用的視覺(jué)處理算法和所應(yīng)用的硬件確定視覺(jué)系統(tǒng)的時(shí)延，建立帶時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)與物理真實(shí)相對(duì)位姿(測(cè)量相機(jī)與目標(biāo))間的數(shù)學(xué)關(guān)系；二、根據(jù)帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)和機(jī)械臂的關(guān)節(jié)指令，估計(jì)當(dāng)前空間機(jī)器人的末端速度；三、設(shè)計(jì)誤差控制器，減小空間機(jī)器人末端速度估值的誤差，得到校正后空間機(jī)器人末端速度估值；四、根據(jù)校正后空間機(jī)器人末端速度估值，對(duì)當(dāng)前帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)進(jìn)行補(bǔ)償，得到經(jīng)過(guò)補(bǔ)償?shù)囊曈X(jué)測(cè)量數(shù)據(jù)。
[0012]【具體實(shí)施方式】二:本實(shí)施方式與【具體實(shí)施方式】一的不同點(diǎn)是:本實(shí)施方式在步驟一所述的根據(jù)所采用的視覺(jué)處理算法和所應(yīng)用的硬件確定視覺(jué)系統(tǒng)的時(shí)延，建立帶時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)與物理真實(shí)相對(duì)位姿(測(cè)量相機(jī)與目標(biāo))間的數(shù)學(xué)關(guān)系為:根據(jù)所采用的視覺(jué)信息處理方法和所應(yīng)用的硬件平臺(tái)確定整個(gè)視覺(jué)測(cè)量環(huán)節(jié)引起的時(shí)延為m個(gè)周期，若系統(tǒng)每個(gè)周期的時(shí)長(zhǎng)為ts，則空間機(jī)器人系統(tǒng)的視覺(jué)時(shí)延Td為:
[0013]Td=HiXts(I)
[0014]定義空間機(jī)器人k時(shí)刻的視覺(jué)測(cè)量信息Dv(k)和真實(shí)距離信息DJk)為
[0015]Dv (k) = [xv (k) yv (k) zv (k) a v(k) β v(k) Yv (k) ]τ(2)
[0016]Dr (k) = [xr (k) yr (k) zr (k) a r(k) β r(k) Yr (k) ]τ(3)
[0017]其中k為通用時(shí)刻，x(k), y(k), z(k)為相對(duì)位置信息，a (k), β (k), y (k)為描述相對(duì)位姿的歐拉角，空間機(jī)器人視覺(jué)測(cè)量信息Dv(k)和真實(shí)距離信息比(10間的關(guān)系為
[0018]Dr (k) =Dv(k+m)(4)
[0019]k表示任意時(shí)刻，m表示時(shí)延。
[0020]若當(dāng)前時(shí)刻為N，視覺(jué)測(cè)量信息為化㈨，則可知道化^!!!)之前的真實(shí)距離信息都可由過(guò)去時(shí)刻的視覺(jué)測(cè)量 數(shù)據(jù)直接得到。定義真實(shí)速度信息VJk)為
[0021]Vr(k) = [K(^) Λ(λ.) 1.(A-)牟(A.) $r(k) Yr(k)J(5)
[0022]可以由Dr (k+1)和Dr (k)計(jì)算得到
【權(quán)利要求】
1.一種基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法，其特征在于:所述方法由以下步驟完成: 步驟一、根據(jù)所采用的視覺(jué)處理算法和所應(yīng)用的硬件確定視覺(jué)系統(tǒng)的視覺(jué)時(shí)延，建立帶時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)與物理真實(shí)相對(duì)位姿間的數(shù)學(xué)關(guān)系； 步驟二、根據(jù)帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)和機(jī)械臂的關(guān)節(jié)指令，估計(jì)當(dāng)前空間機(jī)器人的末端速度； 步驟三、設(shè)計(jì)誤差控制器，減小空間機(jī)器人末端速度估值的誤差，得到校正后空間機(jī)器人末端速度估值； 步驟四、根據(jù)校正后空間機(jī)器人末端速度估值，對(duì)當(dāng)前帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)進(jìn)行補(bǔ)償，得到經(jīng)過(guò)補(bǔ)償?shù)囊曈X(jué)測(cè)量數(shù)據(jù)。
2.根據(jù)權(quán)利要求1所述的一種基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法，其特征在于: 在步驟一中，根據(jù)所采用的視覺(jué)處理算法和所應(yīng)用的硬件確定視覺(jué)系統(tǒng)的時(shí)延，建立帶時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)與物理真實(shí)相對(duì)位姿(測(cè)量相機(jī)與目標(biāo))間的數(shù)學(xué)關(guān)系，其過(guò)程為:根據(jù)所采用的視覺(jué)信息處理方法和所應(yīng)用的硬件平臺(tái)確定整個(gè)視覺(jué)測(cè)量環(huán)節(jié)引起的時(shí)延為m個(gè)周期，若系統(tǒng)每個(gè)周期的時(shí)長(zhǎng)為ts，則空間機(jī)器人系統(tǒng)的視覺(jué)時(shí)延TdS:Td=mXts(I) 定義空間機(jī)器人k時(shí)刻的視覺(jué)測(cè)量信息Dv(k)和真實(shí)距離信息DJk)為:
Dv (k) = [xv (k) yv (k) zv (k) a v (k) β v(k) Yv (k) ]τ (2)
Dr (k) = [xr (k) yr (k) zr (k) a r (k) β r(k) Yr (k) ]T (3) 其中k為通用時(shí)刻，x(k), y(k), z(k)為相對(duì)位置信息，a (k), β (k), y (k)為描述相對(duì)位姿的歐拉角，空間機(jī)器人視覺(jué)測(cè)量信息Dv(k)和真實(shí)距離信息比(10間的關(guān)系為:
Dr (k) =Dv (k+m) (4)
k表示任意時(shí)刻，m表示時(shí)延； 若當(dāng)前時(shí)刻為N，視覺(jué)測(cè)量信息為Dv(N)，則可知道比(N-m)之前的真實(shí)距離信息都可由過(guò)去時(shí)刻的視覺(jué)測(cè)量數(shù)據(jù)直接得到。定義真實(shí)速度信息VJk)為: yr(k) A⑷ tK(k) A⑷ t(k)J(5) 由Dr(k+1)和Dr(k)計(jì)算得到
3.根據(jù)權(quán)利要求1或2所述的一種基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法，其特征在于:在步驟二中，所述的根據(jù)帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)和機(jī)械臂的關(guān)節(jié)指令，估計(jì)當(dāng)前空間機(jī)器人的末端速度，其過(guò)程為:利用帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)，得到真實(shí)速度序列
4.根據(jù)權(quán)利要求3所述的一種基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法，其特征在于:在步驟三中，所述的誤差控制器設(shè)計(jì)為: 將步驟二所述的估計(jì)速度序列的過(guò)程看作一個(gè)帶有誤差的超前環(huán)節(jié):
5.根據(jù)權(quán)利要求4所述的一種基于速度估計(jì)的空間機(jī)器人視覺(jué)時(shí)延誤差補(bǔ)償方法，其特征在于:在步驟四中，所述的根據(jù)校正后空間機(jī)器人末端速度估值，對(duì)當(dāng)前帶有時(shí)延的視覺(jué)測(cè)量數(shù)據(jù)進(jìn)行補(bǔ)償，經(jīng)過(guò)補(bǔ)償?shù)囊曈X(jué)測(cè)量數(shù)據(jù)氙(N)計(jì)算如下:
【文檔編號(hào)】B25J9/16GK103878770SQ201410138351
【公開(kāi)日】2014年6月25日 申請(qǐng)日期:2014年4月8日 優(yōu)先權(quán)日:2014年4月8日
【發(fā)明者】王濱, 李振宇, 劉宏, 趙京東, 李志奇, 王志超 申請(qǐng)人:哈爾濱工業(yè)大學(xué)