本發(fā)明涉及視覺定位，尤其涉及一種基于epnp算法的單目視覺姿態(tài)估計(jì)方法及系統(tǒng)。

背景技術(shù)：

1、數(shù)字醫(yī)學(xué)技術(shù)的發(fā)展推進(jìn)了骨科手術(shù)機(jī)器人導(dǎo)航技術(shù)的進(jìn)步，現(xiàn)有的骨科手術(shù)機(jī)器人導(dǎo)航系統(tǒng)對(duì)于術(shù)中病人的移動(dòng)體位監(jiān)測(cè)大多以ndi光學(xué)導(dǎo)航定位追蹤技術(shù)實(shí)現(xiàn)，其先通過(guò)多個(gè)marker標(biāo)記物表示手術(shù)器械、病人痛點(diǎn)及定位標(biāo)尺的空間位姿信息，接著ndi將上述的marker標(biāo)記物統(tǒng)一到同一坐標(biāo)系下，并借助坐標(biāo)系提供的空間數(shù)據(jù)確定手術(shù)路徑規(guī)劃的起止點(diǎn)、機(jī)器人定位位置及實(shí)時(shí)跟蹤術(shù)中病人的移動(dòng)情況。因此，各marker標(biāo)記物的空間位姿信息精度直接影響到手術(shù)路徑規(guī)劃以及機(jī)器人的精準(zhǔn)度，marker標(biāo)記物數(shù)量越多，其累計(jì)的誤差越大，且計(jì)算量也就越大。為此，本發(fā)明提出了一種基于epnp算法的單目視覺姿態(tài)估計(jì)方法，將epnp非迭代算法與單目相機(jī)結(jié)合，通過(guò)在只有一個(gè)marker標(biāo)記物下，通過(guò)單目相機(jī)實(shí)現(xiàn)機(jī)械臂對(duì)病人移動(dòng)過(guò)程的跟蹤；

2、現(xiàn)有的光學(xué)導(dǎo)航定位追蹤技術(shù)借助ndi實(shí)時(shí)追蹤顯示多個(gè)marker標(biāo)記物位的位置，存在marker標(biāo)記物數(shù)量多，累計(jì)誤差大，計(jì)算量大，且成本高等問(wèn)題，本發(fā)明提出了一種基于epnp算法的單目視覺姿態(tài)估計(jì)方法，其將epnp非迭代算法與單目相機(jī)結(jié)合，且在只有一個(gè)marker標(biāo)記物情況下，解決了ndi相機(jī)自身的需要匹配多個(gè)marker來(lái)進(jìn)行術(shù)中定位所帶來(lái)的誤差累計(jì)及計(jì)算量大等缺點(diǎn)，該方法降低了配準(zhǔn)誤差，提高了配準(zhǔn)精度，減小了計(jì)算量且降低了成本，本發(fā)明的應(yīng)用至少能夠解決現(xiàn)有技術(shù)的部分問(wèn)題。

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

1、本發(fā)明實(shí)施例提供一種基于epnp算法的單目視覺姿態(tài)估計(jì)方法及系統(tǒng)，至少能夠解決現(xiàn)有技術(shù)中部分問(wèn)題。

2、本發(fā)明實(shí)施例的第一方面，

3、提供基于epnp算法的單目視覺姿態(tài)估計(jì)方法，包括：

4、通過(guò)相機(jī)標(biāo)定獲得單目相機(jī)內(nèi)參；確定所述單目相機(jī)的視野內(nèi)布置的空間參考標(biāo)志物上特征點(diǎn)的世界坐標(biāo),以及所述特征點(diǎn)在相機(jī)圖像上的像素坐標(biāo)；將所述特征點(diǎn)的世界坐標(biāo)表示為4個(gè)非共面虛擬控制點(diǎn)的線性組合,建立相機(jī)透視投影模型,形成關(guān)于所述虛擬控制點(diǎn)相機(jī)坐標(biāo)的線性方程組,確定所述虛擬控制點(diǎn)在相機(jī)坐標(biāo)系對(duì)應(yīng)的相機(jī)坐標(biāo)；其中，所述單目相機(jī)固定于機(jī)械臂末端，所述單目相機(jī)的光軸與所述機(jī)械臂的末端中心點(diǎn)重合；

5、引入平行透視投影模型，通過(guò)迭代的平行透視求解過(guò)程，獲得相機(jī)位姿初始估計(jì)，基于所述相機(jī)位姿初始估計(jì)，確定相機(jī)坐標(biāo)系下控制點(diǎn)坐標(biāo)，計(jì)算控制點(diǎn)齊次重心坐標(biāo)系數(shù)的初始值；以縮小控制點(diǎn)在相機(jī)坐標(biāo)系和世界坐標(biāo)系下的距離差為優(yōu)化目標(biāo),利用所述控制點(diǎn)齊次重心坐標(biāo)系數(shù)的初始值,通過(guò)迭代優(yōu)化算法求解，獲得空間參考標(biāo)志物相對(duì)相機(jī)的精確位姿；

6、將空間參考標(biāo)志物相對(duì)于所述機(jī)械臂末端的初始位姿作為基準(zhǔn)位姿；當(dāng)所述空間參考標(biāo)志物發(fā)生移動(dòng)時(shí),相機(jī)實(shí)時(shí)捕獲所述空間參考標(biāo)志物的新位姿，計(jì)算所述機(jī)械臂末端當(dāng)前位姿,結(jié)合所述基準(zhǔn)位姿和所述機(jī)械臂末端當(dāng)前位姿,實(shí)時(shí)計(jì)算機(jī)械臂末端的目標(biāo)位姿，根據(jù)所述目標(biāo)位置移動(dòng)機(jī)械臂。

7、在一種可選的實(shí)施例中，

8、通過(guò)相機(jī)標(biāo)定獲得單目相機(jī)內(nèi)參；確定所述單目相機(jī)的視野內(nèi)布置的空間參考標(biāo)志物上特征點(diǎn)的世界坐標(biāo),以及所述特征點(diǎn)在相機(jī)圖像上的像素坐標(biāo)；將所述特征點(diǎn)的世界坐標(biāo)表示為4個(gè)非共面虛擬控制點(diǎn)的線性組合,建立相機(jī)透視投影模型,形成關(guān)于所述虛擬控制點(diǎn)相機(jī)坐標(biāo)的線性方程組,確定所述虛擬控制點(diǎn)在相機(jī)坐標(biāo)系對(duì)應(yīng)的相機(jī)坐標(biāo)包括：

9、采用張正友標(biāo)定法對(duì)所述單目相機(jī)進(jìn)行標(biāo)定,獲得所述單目相機(jī)的內(nèi)參數(shù)矩陣，包括相機(jī)焦距、光心坐標(biāo)；

10、將相機(jī)視野內(nèi)的棋盤格作為平面參考標(biāo)志物,通過(guò)特征提取算法檢測(cè)所述平面參考標(biāo)志物上角點(diǎn)的像素坐標(biāo)，選取n個(gè)所述角點(diǎn)作為特征點(diǎn)，基于所述棋盤格的尺寸和所述角點(diǎn)的排列，計(jì)算所述特征點(diǎn)在所述棋盤格對(duì)應(yīng)的棋盤坐標(biāo)系下的世界坐標(biāo)；

11、在所述棋盤坐標(biāo)系中，選取4個(gè)互不共面的點(diǎn)作為虛擬控制點(diǎn)，建立所述棋盤格坐標(biāo)系到所述虛擬控制點(diǎn)對(duì)應(yīng)的虛擬控制點(diǎn)坐標(biāo)系的變換關(guān)系，將所述特征點(diǎn)的世界坐標(biāo)通過(guò)坐標(biāo)變換和齊次重心坐標(biāo)表示，得到所述特征點(diǎn)在所述虛擬控制點(diǎn)坐標(biāo)系下的坐標(biāo)表示；

12、基于所述內(nèi)參數(shù)矩陣，結(jié)合所述特征點(diǎn)的世界坐標(biāo)，以及所述特征點(diǎn)在虛擬控制點(diǎn)坐標(biāo)系下的坐標(biāo)表示,建立相機(jī)成像透視投影模型方程組；

13、在所述相機(jī)成像透視投影模型方程組中,引入虛擬控制點(diǎn)在相機(jī)坐標(biāo)系下的未知坐標(biāo)表示，將所述相機(jī)成像透視投影模型方程組轉(zhuǎn)化為控制點(diǎn)在相機(jī)坐標(biāo)系的線性方程組，通過(guò)奇異值分解數(shù)值優(yōu)化方法求解所述線性方程組,得到4個(gè)虛擬控制點(diǎn)在相機(jī)坐標(biāo)系中的坐標(biāo)。

14、在一種可選的實(shí)施例中，

15、還包括：

16、所述單目相機(jī)的內(nèi)參數(shù)矩陣，其公式如下：

17、

18、其中，k表示內(nèi)參數(shù)矩陣，fx表示相機(jī)在x軸方向上的焦距，fy表示相機(jī)在y軸方向上的焦距，u0表示相機(jī)的光心橫坐標(biāo)，v0表示相機(jī)光心縱坐標(biāo)；

19、所述相機(jī)成像透視投影模型方程組，其公式如下：

20、

21、其中，j表示虛擬控制點(diǎn)索引，i表示特征點(diǎn)索引，αij表示第i個(gè)特征點(diǎn)在虛擬控制點(diǎn)坐標(biāo)系下的齊次重心坐標(biāo)，xjc表示第j個(gè)虛擬控制點(diǎn)在相機(jī)坐標(biāo)系下的x分量，yjc表示第j個(gè)虛擬控制點(diǎn)在相機(jī)坐標(biāo)系下的y分量，zjc表示第j個(gè)虛擬控制點(diǎn)在相機(jī)坐標(biāo)系下的z分量，ui表示第i個(gè)特征點(diǎn)在圖像平面上的橫坐標(biāo)，vi表示第i個(gè)特征點(diǎn)在圖像平面上的縱坐標(biāo)。

22、在一種可選的實(shí)施例中，

23、引入平行透視投影模型，通過(guò)迭代的平行透視求解過(guò)程，獲得相機(jī)位姿初始估計(jì)，基于所述相機(jī)位姿初始估計(jì)，確定相機(jī)坐標(biāo)系下控制點(diǎn)坐標(biāo)，計(jì)算控制點(diǎn)齊次重心坐標(biāo)系數(shù)的初始值；以縮小控制點(diǎn)在相機(jī)坐標(biāo)系和世界坐標(biāo)系下的距離差為優(yōu)化目標(biāo),利用所述控制點(diǎn)齊次重心坐標(biāo)系數(shù)的初始值,通過(guò)迭代優(yōu)化算法求解，獲得空間參考標(biāo)志物相對(duì)相機(jī)的精確位姿包括：

24、通過(guò)建立平行透視投影模型，將所述相機(jī)成像透視投影模型的方程組重構(gòu)成關(guān)于控制點(diǎn)坐標(biāo)和齊次重心坐標(biāo)系數(shù)的線性方程組；

25、通過(guò)迭代求解所述平行透視投影模型的方程組，獲得相機(jī)位姿的初始估計(jì),并根據(jù)所述初始估計(jì)，計(jì)算控制點(diǎn)在相機(jī)坐標(biāo)系下的初始坐標(biāo)，基于控制點(diǎn)在相機(jī)坐標(biāo)系下的初始坐標(biāo)，通過(guò)最小二乘法求解齊次重心坐標(biāo)方程組,得到控制點(diǎn)齊次重心坐標(biāo)系數(shù)的初始值；

26、基于相機(jī)位姿，以及所述控制點(diǎn)齊次重心坐標(biāo)系數(shù)的初始值,根據(jù)控制點(diǎn)在相機(jī)坐標(biāo)系和世界坐標(biāo)系下距離差為基礎(chǔ)，構(gòu)建目標(biāo)函數(shù)，使所述目標(biāo)函數(shù)最小化，得到最優(yōu)的相機(jī)位姿估計(jì)，獲得空間參考標(biāo)志物相對(duì)相機(jī)的精確位姿。

27、在一種可選的實(shí)施例中，

28、所述目標(biāo)函數(shù)包括：

29、其公式如下：

30、

31、其中，β表示優(yōu)化目標(biāo)，j、l表示虛擬控制點(diǎn)索引，cjc表示第j個(gè)虛擬控制點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)，clc表示第l個(gè)虛擬控制點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)，cjw表示第j個(gè)虛擬控制點(diǎn)在世界坐標(biāo)系下的坐標(biāo)，clw表示第l個(gè)虛擬控制點(diǎn)在世界坐標(biāo)系下的坐標(biāo)。

32、在一種可選的實(shí)施例中，

33、將空間參考標(biāo)志物相對(duì)于所述機(jī)械臂末端的初始位姿作為基準(zhǔn)位姿；當(dāng)所述空間參考標(biāo)志物發(fā)生移動(dòng)時(shí),相機(jī)實(shí)時(shí)捕獲所述空間參考標(biāo)志物的新位姿，計(jì)算所述機(jī)械臂末端當(dāng)前位姿,結(jié)合所述基準(zhǔn)位姿和所述機(jī)械臂末端當(dāng)前位姿,實(shí)時(shí)計(jì)算機(jī)械臂末端的目標(biāo)位姿，根據(jù)所述目標(biāo)位置移動(dòng)機(jī)械臂包括：

34、在所述機(jī)械臂的末端對(duì)應(yīng)的工作空間內(nèi)，放置空間參考標(biāo)志物，所述空間參考標(biāo)志物由多個(gè)明確坐標(biāo)的控制點(diǎn)組成；

35、基于所述單目相機(jī)完整拍攝所述空間參考標(biāo)志物為基準(zhǔn),設(shè)置當(dāng)所述機(jī)械臂位于初始位置時(shí)，記錄所述機(jī)械臂的初始關(guān)節(jié)角度，計(jì)算所述空間參考標(biāo)志物相對(duì)于所述單目相機(jī)的初始位姿，并將所述初始位姿作為基準(zhǔn)位姿；

36、所述單目相機(jī)實(shí)時(shí)拍攝所述空間參考標(biāo)志物對(duì)應(yīng)的圖像,并計(jì)算所述空間參考標(biāo)志物相對(duì)于所述單目相機(jī)的當(dāng)前位姿；

37、根據(jù)所述基準(zhǔn)位姿和所述當(dāng)前位姿，計(jì)算所述空間參考標(biāo)志物的位姿變化量，根據(jù)所述位姿變化量和所述初始關(guān)節(jié)角度計(jì)算關(guān)節(jié)角度變化量,并將所述關(guān)節(jié)角度變化量補(bǔ)償?shù)剿鰴C(jī)械臂的當(dāng)前關(guān)節(jié)角度中，得到所述機(jī)械臂的末端的目標(biāo)關(guān)節(jié)角度，基于所述目標(biāo)關(guān)節(jié)角度，移動(dòng)所述機(jī)械臂。

38、在一種可選的實(shí)施例中，

39、所述單目相機(jī)實(shí)時(shí)拍攝所述空間參考標(biāo)志物對(duì)應(yīng)的圖像包括：

40、以4x4齊次變換矩陣來(lái)表示坐標(biāo)系間的相對(duì)位姿，齊次變換矩陣的含義是坐標(biāo)系j相對(duì)于坐標(biāo)系i的位姿。

41、在初始時(shí)，所述機(jī)械臂的末端與所述空間參考標(biāo)志物相對(duì)位置基準(zhǔn)，其公式如下：

42、

43、其中，表示相機(jī)坐標(biāo)系與目標(biāo)位姿坐標(biāo)系之間的常量齊次變換矩陣，表示相機(jī)坐標(biāo)系與機(jī)械臂基座坐標(biāo)系之間的初始齊次變換矩陣，表示機(jī)械臂基座坐標(biāo)系與目標(biāo)位姿坐標(biāo)系之間的初始齊次變換矩陣；

44、所述空間參考標(biāo)志物移動(dòng)后，使所述空間參考標(biāo)志物與機(jī)械臂的末端相對(duì)位姿保持不變，滿足如下公式：

45、

46、其中，表示相機(jī)坐標(biāo)系相對(duì)于機(jī)械臂基座坐標(biāo)系的齊次變換矩陣，表示目標(biāo)位姿坐標(biāo)系相對(duì)于機(jī)械臂基座坐標(biāo)系的齊次變換矩陣；

47、同時(shí)，在所述機(jī)械臂的基座坐標(biāo)系下的機(jī)械臂末端的位姿，滿足如下公式：

48、

49、其中，表示機(jī)械臂末端坐標(biāo)系相對(duì)于機(jī)械臂基座坐標(biāo)系的齊次變換矩陣，表示相機(jī)坐標(biāo)系相對(duì)于機(jī)械臂末端坐標(biāo)系的變換矩陣的齊次變換矩陣；

50、所述空間參考標(biāo)志物移動(dòng)到新位置，基于所述基座坐標(biāo)系，新位姿公式如下：

51、

52、其中，表示目標(biāo)位姿新的位置相對(duì)于相機(jī)坐標(biāo)系的齊次變換矩陣，表示目標(biāo)位姿新的位置相對(duì)于機(jī)械臂基座坐標(biāo)系的齊次變換矩陣，表示目標(biāo)位姿新的位置相對(duì)于相機(jī)坐標(biāo)系的齊次變換矩陣。

53、本發(fā)明實(shí)施例的第二方面，

54、提供基于epnp算法的單目視覺姿態(tài)估計(jì)系統(tǒng)，包括：

55、第一單元，用于通過(guò)相機(jī)標(biāo)定獲得單目相機(jī)內(nèi)參；確定所述單目相機(jī)的視野內(nèi)布置的空間參考標(biāo)志物上特征點(diǎn)的世界坐標(biāo),以及所述特征點(diǎn)在相機(jī)圖像上的像素坐標(biāo)；將所述特征點(diǎn)的世界坐標(biāo)表示為4個(gè)非共面虛擬控制點(diǎn)的線性組合,建立相機(jī)透視投影模型,形成關(guān)于所述虛擬控制點(diǎn)相機(jī)坐標(biāo)的線性方程組,確定所述虛擬控制點(diǎn)在相機(jī)坐標(biāo)系對(duì)應(yīng)的相機(jī)坐標(biāo)；其中，所述單目相機(jī)固定于機(jī)械臂末端，所述單目相機(jī)的光軸與所述機(jī)械臂的末端中心點(diǎn)重合；

56、第二單元，用于引入平行透視投影模型，通過(guò)迭代的平行透視求解過(guò)程，獲得相機(jī)位姿初始估計(jì)，基于所述相機(jī)位姿初始估計(jì)，確定相機(jī)坐標(biāo)系下控制點(diǎn)坐標(biāo)，計(jì)算控制點(diǎn)齊次重心坐標(biāo)系數(shù)的初始值；以縮小控制點(diǎn)在相機(jī)坐標(biāo)系和世界坐標(biāo)系下的距離差為優(yōu)化目標(biāo),利用所述控制點(diǎn)齊次重心坐標(biāo)系數(shù)的初始值,通過(guò)迭代優(yōu)化算法求解，獲得空間參考標(biāo)志物相對(duì)相機(jī)的精確位姿；

57、第三單元，用于將空間參考標(biāo)志物相對(duì)于所述機(jī)械臂末端的初始位姿作為基準(zhǔn)位姿；當(dāng)所述空間參考標(biāo)志物發(fā)生移動(dòng)時(shí),相機(jī)實(shí)時(shí)捕獲所述空間參考標(biāo)志物的新位姿，計(jì)算所述機(jī)械臂末端當(dāng)前位姿,結(jié)合所述基準(zhǔn)位姿和所述機(jī)械臂末端當(dāng)前位姿,實(shí)時(shí)計(jì)算機(jī)械臂末端的目標(biāo)位姿，根據(jù)所述目標(biāo)位置移動(dòng)機(jī)械臂。

58、本發(fā)明實(shí)施例的第三方面，

59、提供一種電子設(shè)備，包括：

60、處理器；

61、用于存儲(chǔ)處理器可執(zhí)行指令的存儲(chǔ)器；

62、其中，所述處理器被配置為調(diào)用所述存儲(chǔ)器存儲(chǔ)的指令，以執(zhí)行前述所述的方法。

63、本發(fā)明實(shí)施例的第四方面，

64、提供一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有計(jì)算機(jī)程序指令，所述計(jì)算機(jī)程序指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)前述所述的方法。

65、本發(fā)明實(shí)施例中，能夠通過(guò)迭代優(yōu)化算法獲得空間參考標(biāo)志物相對(duì)相機(jī)的精確位姿，提高姿態(tài)估計(jì)的精確度；引入平行透視投影模型和通過(guò)迭代的平行透視求解過(guò)程，在沒有初始位姿的情況下，快速獲得相機(jī)位姿的初始估計(jì)，為進(jìn)一步的迭代優(yōu)化提供有效的起點(diǎn)，不僅提高算法的收斂速度，還提高最終姿態(tài)估計(jì)的精度；通過(guò)將單目相機(jī)固定于機(jī)械臂末端，并實(shí)時(shí)捕獲空間參考標(biāo)志物的新位姿，所述方法能夠?qū)崟r(shí)計(jì)算并調(diào)整機(jī)械臂末端的目標(biāo)位姿，以適應(yīng)空間參考標(biāo)志物的位置變化；實(shí)時(shí)位姿跟蹤和調(diào)整機(jī)制，為機(jī)械臂的精確操作和自動(dòng)化作業(yè)提供強(qiáng)有力的支持；由于不依賴于特定的空間參考標(biāo)志物形狀或尺寸，只需要在相機(jī)視野內(nèi)布置可識(shí)別的特征點(diǎn)，具有較高的靈活性。