本發(fā)明涉及空間站交會(huì)對(duì)接、航天器之間的交會(huì)測(cè)量、空間飛行器放飛回收測(cè)量及其他合作目標(biāo)的相對(duì)測(cè)量領(lǐng)域，具體地，涉及一種基于單目視覺的空間合作目標(biāo)位姿解算方法、系統(tǒng)、終端及介質(zhì)。

背景技術(shù)：

1、基于單目視覺的空間合作目標(biāo)位姿解算方法，是通過單個(gè)相機(jī)獲取合作靶標(biāo)圖像特征，并由該圖像特征解算目標(biāo)與相機(jī)之間的相對(duì)位姿關(guān)系的方法。對(duì)于合作點(diǎn)目標(biāo)，通常采用基于點(diǎn)對(duì)應(yīng)的方法。最早是由fischler提出，通過控制目標(biāo)點(diǎn)位置關(guān)系的約束來求解目標(biāo)與相機(jī)之間的位姿，又稱為pnp(perspective?n?points)問題。然而，實(shí)際單目測(cè)量系統(tǒng)在采用pnp方法直接進(jìn)行位姿求解時(shí)，通常不可避免地會(huì)引入諸多誤差，如特征點(diǎn)質(zhì)心提取誤差、外參標(biāo)定誤差、共面點(diǎn)平面度誤差、相機(jī)內(nèi)參標(biāo)定誤差等，會(huì)導(dǎo)致解算結(jié)果誤差較大，甚至造成點(diǎn)的誤匹配，這就會(huì)導(dǎo)致后續(xù)迭代方法很難正常工作，從而直接影響了位姿信息的精度和魯棒性。

技術(shù)實(shí)現(xiàn)思路

1、針對(duì)現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種基于單目視覺的空間合作目標(biāo)位姿解算方法、系統(tǒng)、終端及介質(zhì)。

2、根據(jù)本發(fā)明的一個(gè)方面，提供一種適用于空間合作目標(biāo)的相對(duì)位姿解算方法，包括：

3、基于相機(jī)內(nèi)參，構(gòu)建矩陣并求解逆矩陣；

4、基于所述逆矩陣，獲得至少4個(gè)靶標(biāo)點(diǎn)在相平面上的理想像點(diǎn)位置；

5、基于所述理想像點(diǎn)位置，構(gòu)建共線投影方程；

6、結(jié)合靶標(biāo)點(diǎn)的外參，獲得所述共線投影方程的解；

7、利用所述共線投影方程的解計(jì)算旋轉(zhuǎn)軸和旋轉(zhuǎn)角；

8、將所述旋轉(zhuǎn)軸和所述旋轉(zhuǎn)角轉(zhuǎn)換成旋轉(zhuǎn)矩陣，獲得合作目標(biāo)的相對(duì)位姿。

9、優(yōu)選地，所述基于相機(jī)內(nèi)參，構(gòu)建矩陣并求解逆矩陣，包括：

10、根據(jù)相機(jī)內(nèi)參中的鏡頭焦距f，建立矩陣k：

11、基于所述矩陣k，獲得逆矩陣k-1：k-1＝inverse(k)，inverse為矩陣求逆函數(shù)。

12、優(yōu)選地，所述基于所述逆矩陣，獲得4個(gè)靶標(biāo)點(diǎn)在相平面上的理想像點(diǎn)位置，包括：

13、將4個(gè)靶標(biāo)點(diǎn)進(jìn)行相平面投影點(diǎn)解算：

14、利用靶標(biāo)點(diǎn)在相平面的投影坐pxi、pyi，得到理想像點(diǎn)位置，即目標(biāo)矢量的廣義坐標(biāo)表示cpxi、cpyi。

15、優(yōu)選地，所述基于所述理想像點(diǎn)位置，構(gòu)建共線投影方程：

16、

17、所述結(jié)合靶標(biāo)點(diǎn)的外參{px1,py1,px2,py2,px3,py3,px4,py4}，獲得所述共線投影方程的解，包括：

18、求解所述共線投影方程，得到方程的解為：{sol1,sol2,sol3,sol4,sol5,sol6,sol7,sol8}；

19、對(duì)所述方程的解進(jìn)行歸一化，得到歸一化后的解{r0,r1,r2,r3,r4,r5,r6,r7}及平移量{tx,ty,tz}；

20、

21、優(yōu)選地，所述利用共線投影方程的解計(jì)算旋轉(zhuǎn)軸和旋轉(zhuǎn)角，包括：

22、首先，求解三元一次方程ax3+bx2+cx+d＝0，利用其實(shí)數(shù)解solar1，得到旋轉(zhuǎn)軸evector，其中

23、

24、

25、normalize為向量的歸一化處理函數(shù)；

26、接著，計(jì)算旋轉(zhuǎn)角，包括：

27、計(jì)算eanglecos，其中trace函數(shù)用以求矩陣的跡，

28、

29、計(jì)算eangle?sin?timesn，

30、eanglesintimesn＝rorigin-eanglecos*identitymatrix-(1-eanglecos)kroneckerproduct[evector，evector]

31、其中trace函數(shù)用以求矩陣的跡，identitymatrix為3*3的單位陣，kroneckerproduct為求向量外積的函數(shù)；

32、利用eanglesintimesn與evector的矩陣元計(jì)算eanglesin，

33、

34、獲得旋轉(zhuǎn)角：

35、

36、優(yōu)選地，所述將所述旋轉(zhuǎn)軸和所述旋轉(zhuǎn)角轉(zhuǎn)換成旋轉(zhuǎn)矩陣，獲得合作目標(biāo)的相對(duì)位姿，包括：

37、將所述旋轉(zhuǎn)軸和所述旋轉(zhuǎn)角轉(zhuǎn)換成旋轉(zhuǎn)矩陣r：

38、r＝cos(eangle)identitymatrix+(1-cos(eangle))kroneckerproduct(evector,evector)+sin(eangle)antisymetrymatrix(evector)

39、

40、其中antisymetrymatrix為反對(duì)稱矩陣函數(shù)；

41、利用旋轉(zhuǎn)矩陣r的矩陣元計(jì)算歐拉角{α，β，γ}

42、

43、獲得靶標(biāo)板到相機(jī)的位姿為{α，β，γ，tx，ty，tz}。

44、根據(jù)本發(fā)明的第二個(gè)方面，提供一種適用于空間合作目標(biāo)的相對(duì)位姿解算系統(tǒng)，包括：

45、逆矩陣模塊，該模塊基于相機(jī)內(nèi)參，構(gòu)建矩陣并求解逆矩陣；

46、位置模塊，該模塊基于所述逆矩陣，獲得4個(gè)靶標(biāo)點(diǎn)在相平面上的理想像點(diǎn)位置；

47、方程模塊，該模塊基于所述理想像點(diǎn)位置，構(gòu)建共線投影方程；

48、求解模塊，該模塊結(jié)合靶標(biāo)點(diǎn)的外參，獲得所述共線投影方程的解；

49、李代數(shù)模塊，該模塊利用所述共線投影方程的解計(jì)算旋轉(zhuǎn)軸和旋轉(zhuǎn)角；

50、位姿模塊，該模塊將所述旋轉(zhuǎn)軸和所述旋轉(zhuǎn)角轉(zhuǎn)換成旋轉(zhuǎn)矩陣，獲得合作目標(biāo)的相對(duì)位姿。

51、根據(jù)本發(fā)明的第三個(gè)方面，提供一種空間合作目標(biāo)的相對(duì)位姿的應(yīng)用，基于所述的適用于空間合作目標(biāo)的相對(duì)位姿解算方法或基于所述的適用于空間合作目標(biāo)的相對(duì)位姿解算系統(tǒng)，得到空間合作目標(biāo)的相對(duì)位姿，基于該相對(duì)位姿，進(jìn)行空間站交會(huì)對(duì)接、航天器之間的交會(huì)或空間飛行器放飛回收。

52、根據(jù)本發(fā)明的第四個(gè)方面，提供一種終端，包括存儲(chǔ)器、處理器及存儲(chǔ)在存儲(chǔ)器上并可在處理器上運(yùn)行的計(jì)算機(jī)程序，所述處理器執(zhí)行所述程序時(shí)可用于執(zhí)行所述的方法，或，運(yùn)行所述的系統(tǒng)。

53、根據(jù)本發(fā)明的第五個(gè)方面，提供一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有計(jì)算機(jī)程序，該程序被處理器執(zhí)行時(shí)可用于執(zhí)行所述的方法，或，運(yùn)行所述的系統(tǒng)。

54、與現(xiàn)有技術(shù)相比，本發(fā)明實(shí)施例至少具有如下的一種有益效果：

55、本發(fā)明實(shí)施例中的基于單目視覺的空間合作目標(biāo)位姿解算方法和系統(tǒng)，有效提高解算精度，并具有更高的誤差容忍度，顯著提升了產(chǎn)品的魯棒性，更適用于工程應(yīng)用。在空間合作目標(biāo)位姿解算方面具有重要的應(yīng)用價(jià)值。

技術(shù)特征：

1.一種適用于空間合作目標(biāo)的相對(duì)位姿解算方法，其特征在于，包括：

2.根據(jù)權(quán)利要求1所述的一種適用于空間合作目標(biāo)的相對(duì)位姿解算方法，其特征在于，所述基于相機(jī)內(nèi)參，構(gòu)建矩陣并求解逆矩陣，包括：

3.根據(jù)權(quán)利要求2所述的一種適用于空間合作目標(biāo)的相對(duì)位姿解算方法，其特征在于，所述基于所述逆矩陣，獲得4個(gè)靶標(biāo)點(diǎn)在相平面上的理想像點(diǎn)位置，包括：

4.根據(jù)權(quán)利要求3所述的一種適用于空間合作目標(biāo)的相對(duì)位姿解算方法，其特征在于，所述基于所述理想像點(diǎn)位置，構(gòu)建共線投影方程：

5.根據(jù)權(quán)利要求4所述的一種適用于空間合作目標(biāo)的相對(duì)位姿解算方法，其特征在于，所述利用共線投影方程的解計(jì)算旋轉(zhuǎn)軸和旋轉(zhuǎn)角，包括：

6.根據(jù)權(quán)利要求5所述的一種適用于空間合作目標(biāo)的相對(duì)位姿解算方法，其特征在于，所述將所述旋轉(zhuǎn)軸和所述旋轉(zhuǎn)角轉(zhuǎn)換成旋轉(zhuǎn)矩陣，獲得合作目標(biāo)的相對(duì)位姿，包括：

7.一種適用于空間合作目標(biāo)的相對(duì)位姿解算系統(tǒng)，其特征在于，包括：

8.一種空間合作目標(biāo)的相對(duì)位姿的應(yīng)用方法，其特征在于，包括：

9.一種終端，包括存儲(chǔ)器、處理器及存儲(chǔ)在存儲(chǔ)器上并可在處理器上運(yùn)行的計(jì)算機(jī)程序，其特征在于，所述處理器執(zhí)行所述程序時(shí)可用于執(zhí)行權(quán)利要求1-6中任一項(xiàng)所述的方法，或，運(yùn)行權(quán)利要求7中任一項(xiàng)所述的系統(tǒng)。

10.一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有計(jì)算機(jī)程序，其特征在于，該程序被處理器執(zhí)行時(shí)可用于執(zhí)行權(quán)利要求1-6中任一項(xiàng)所述的方法，或，運(yùn)行權(quán)利要求7中任一項(xiàng)所述的系統(tǒng)。

技術(shù)總結(jié)
本發(fā)明提供一種適用于空間合作目標(biāo)的相對(duì)位姿解算方法和系統(tǒng)，包括：基于相機(jī)內(nèi)參，構(gòu)建矩陣并求解逆矩陣；基于所述逆矩陣，獲得至少4個(gè)靶標(biāo)點(diǎn)在相平面上的理想像點(diǎn)位置；基于所述理想像點(diǎn)位置，構(gòu)建共線投影方程；結(jié)合靶標(biāo)點(diǎn)的外參，獲得所述共線投影方程的解；利用所述共線投影方程的解計(jì)算旋轉(zhuǎn)軸和旋轉(zhuǎn)角；將所述旋轉(zhuǎn)軸和所述旋轉(zhuǎn)角轉(zhuǎn)換成旋轉(zhuǎn)矩陣，獲得合作目標(biāo)的相對(duì)位姿。本發(fā)明能有效提高解算精度，并具有更高的誤差容忍度，顯著提升了產(chǎn)品的魯棒性，更適用于工程應(yīng)用。在空間合作目標(biāo)位姿解算方面具有重要的應(yīng)用價(jià)值。

技術(shù)研發(fā)人員：何峰,曲偉智,孫朔冬,姜麗輝,翟正一,楊逸峰,張徐瑋,鮑其蓮
受保護(hù)的技術(shù)使用者：上海交通大學(xué)
技術(shù)研發(fā)日：
技術(shù)公布日：2025/1/6