本申請(qǐng)涉及位姿測(cè)量領(lǐng)域，尤其涉及一種位姿跟蹤測(cè)量方法、裝置、設(shè)備及存儲(chǔ)介質(zhì)。

背景技術(shù)：

1、位姿精度是評(píng)價(jià)工業(yè)機(jī)器人加工能力和部件對(duì)接裝配工藝的關(guān)鍵指標(biāo)之一，位姿精度影響了產(chǎn)品加工、裝配的效率和質(zhì)量，決定了產(chǎn)品的制造成本和整體質(zhì)量。基于工業(yè)化制造對(duì)高質(zhì)量、高效率發(fā)展的急切需求，需要提出更高精度的位姿測(cè)量技術(shù)和更便捷、更高容錯(cuò)的位姿測(cè)量手段。

2、現(xiàn)有的位姿測(cè)量技術(shù)是基于t系列的光學(xué)傳感器來反饋位姿信息。從單點(diǎn)測(cè)量方面來評(píng)價(jià)，t系列的光學(xué)傳感器(例如，t-probe和t-mac)的測(cè)量精度較低。

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

1、本申請(qǐng)的主要目的在于提供一種位姿跟蹤測(cè)量方法、裝置、設(shè)備及存儲(chǔ)介質(zhì)，旨在解決現(xiàn)有位姿測(cè)量技術(shù)存在的測(cè)量精度較低的技術(shù)問題。

2、為實(shí)現(xiàn)上述目的，本申請(qǐng)?zhí)峁┮环N位姿跟蹤測(cè)量方法，包括以下步驟：

3、建立被測(cè)對(duì)象的初始坐標(biāo)系，所述被測(cè)對(duì)象上設(shè)有反射鏡；

4、基于所述反射鏡在所述初始坐標(biāo)系中的點(diǎn)位坐標(biāo)，計(jì)算所述初始坐標(biāo)系相對(duì)于所述反射鏡的偏移矢量，并根據(jù)所述偏移矢量對(duì)所述初始坐標(biāo)系進(jìn)行平移，得到目標(biāo)坐標(biāo)系；

5、根據(jù)所述目標(biāo)坐標(biāo)系下反射鏡的點(diǎn)位坐標(biāo)，計(jì)算空間平面的法向量，并將所述法向量轉(zhuǎn)化為單位向量；

6、根據(jù)所述目標(biāo)坐標(biāo)系旋轉(zhuǎn)前后的關(guān)系，計(jì)算旋轉(zhuǎn)矩陣，根據(jù)所述旋轉(zhuǎn)矩陣和所述單位向量得到目標(biāo)跟蹤矢量；

7、根據(jù)被測(cè)對(duì)象的測(cè)量需求，對(duì)所述目標(biāo)坐標(biāo)系進(jìn)行關(guān)系匹配或擬合；

8、根據(jù)所述目標(biāo)跟蹤矢量，及所述關(guān)系匹配或擬合的結(jié)果，跟蹤反射鏡位置，進(jìn)行位姿跟蹤測(cè)量。

9、可選地，所述初始坐標(biāo)系至少包括初始工具坐標(biāo)系和初始偏移坐標(biāo)系中的一種；

10、所述建立被測(cè)對(duì)象的初始坐標(biāo)系，包括：

11、當(dāng)所述被測(cè)對(duì)象包括固定部件和移動(dòng)部件，選定所述移動(dòng)部件上的第一孔位，并基于所述第一孔位構(gòu)建所述初始偏移坐標(biāo)系。

12、可選地，所述反射鏡包括第一反射鏡、第二反射鏡和第三反射鏡；所述基于所述反射鏡的點(diǎn)位坐標(biāo)，計(jì)算所述初始坐標(biāo)系相對(duì)于所述反射鏡的偏移矢量，根據(jù)所述偏移矢量對(duì)初始坐標(biāo)系進(jìn)行平移，得到目標(biāo)坐標(biāo)系，包括步驟：

13、根據(jù)所述偏移矢量，將所述初始坐標(biāo)系的原點(diǎn)平移至所述第一反射鏡所在的點(diǎn)位，得到所述目標(biāo)坐標(biāo)系。

14、可選地，所述根據(jù)所述反射鏡的點(diǎn)位坐標(biāo)，計(jì)算空間平面的法向量，并將所述法向量轉(zhuǎn)化為單位向量，包括：

15、根據(jù)所述第一反射鏡、所述第二反射鏡和所述第三反射鏡的點(diǎn)位坐標(biāo)(xa,ya,za)、

16、(xb,yb,zb)和(xc,yc,zc)，計(jì)算空間平面的法向量計(jì)算公式為：

17、

18、a＝(yb-ya)(zc-za)-(zb-za)(yc-ya)

19、b＝(zb-za)(xc-xa)-(zc-za)(xb-xa)

20、c＝(xb-xa)(yc-ya)-(xc-xa)(yb-ya)；

21、將除以對(duì)應(yīng)的模長(zhǎng)得到單位向量

22、可選地，所述目標(biāo)坐標(biāo)系為o，旋轉(zhuǎn)后的坐標(biāo)系為a；所述根據(jù)目標(biāo)坐標(biāo)系旋轉(zhuǎn)前后的反射鏡坐標(biāo)系計(jì)算旋轉(zhuǎn)矩陣，根據(jù)所述旋轉(zhuǎn)矩陣和所述單位向量得到目標(biāo)跟蹤矢量，包括：

23、計(jì)算旋轉(zhuǎn)矩陣表達(dá)式為：

24、

25、式中：表示目標(biāo)坐標(biāo)系o中ox軸在旋轉(zhuǎn)后的坐標(biāo)系a中的投影；表示目標(biāo)坐標(biāo)系o中oy軸在旋轉(zhuǎn)后的坐標(biāo)系a中的投影；表示目標(biāo)坐標(biāo)系o中oz軸在旋轉(zhuǎn)后的坐標(biāo)系a中的投影；表示旋轉(zhuǎn)后坐標(biāo)系a中ax軸在目標(biāo)坐標(biāo)系o中投影的轉(zhuǎn)置；表示旋轉(zhuǎn)后坐標(biāo)系a中ay軸在目標(biāo)坐標(biāo)系o中投影的轉(zhuǎn)置；表示旋轉(zhuǎn)后坐標(biāo)系a中az軸在目標(biāo)坐標(biāo)系o中投影的轉(zhuǎn)置；

26、oz軸的單位向量的計(jì)算公式如下：

27、

28、所述為目標(biāo)跟蹤矢量。

29、可選地，所述根據(jù)被測(cè)對(duì)象的測(cè)量需求，對(duì)所述目標(biāo)坐標(biāo)系進(jìn)行關(guān)系匹配或擬合，包括步驟：

30、若所述被測(cè)對(duì)象包括固定部件和移動(dòng)部件，測(cè)量被測(cè)對(duì)象的對(duì)合狀態(tài)，則選定所述固定部件上的第二孔位；基于所述第二孔位構(gòu)建固定坐標(biāo)系，將目標(biāo)偏移坐標(biāo)系與固定坐標(biāo)系進(jìn)行關(guān)系匹配，得到目標(biāo)偏移坐標(biāo)系與固定坐標(biāo)系的相對(duì)關(guān)系；

31、若需要測(cè)量被測(cè)對(duì)象的絕對(duì)精度，則將所述目標(biāo)工具坐標(biāo)系與基坐標(biāo)系進(jìn)行擬合，建立統(tǒng)一坐標(biāo)系，所述基坐標(biāo)系為根據(jù)跟蹤設(shè)備建立的坐標(biāo)系。

32、基于同一發(fā)明構(gòu)思，本申請(qǐng)還提供一種位姿跟蹤測(cè)量裝置，包括：

33、初始坐標(biāo)系建立模塊，用于建立被測(cè)對(duì)象的初始坐標(biāo)系，所述被測(cè)對(duì)象上設(shè)有反射鏡；

34、目標(biāo)坐標(biāo)系建立模塊，用于基于所述反射鏡的點(diǎn)位坐標(biāo)，計(jì)算所述初始坐標(biāo)系相對(duì)于所述反射鏡的偏移矢量，根據(jù)所述偏移矢量對(duì)初始坐標(biāo)系進(jìn)行平移，得到目標(biāo)坐標(biāo)系；

35、空間平面計(jì)算模塊，用于根據(jù)所述反射鏡的點(diǎn)位坐標(biāo)，計(jì)算空間平面的法向，并將空間平面的法向量轉(zhuǎn)化為單位向量；

36、目標(biāo)跟蹤矢量解算模塊，用于根據(jù)所述目標(biāo)坐標(biāo)系旋轉(zhuǎn)前后的關(guān)系，計(jì)算旋轉(zhuǎn)矩陣，根據(jù)所述旋轉(zhuǎn)矩陣和所述單位向量得到目標(biāo)跟蹤矢量；

37、目標(biāo)坐標(biāo)系處理模塊，用于根據(jù)被測(cè)對(duì)象的測(cè)量需求，對(duì)所述目標(biāo)坐標(biāo)系進(jìn)行關(guān)系匹配或擬合；

38、跟蹤測(cè)量模塊，用于根據(jù)所述目標(biāo)跟蹤矢量及所述關(guān)系匹配或擬合的結(jié)果，跟蹤反射鏡位置，進(jìn)行位姿跟蹤測(cè)量。

39、可選地，所述裝置還包括：

40、適配器，所述適配器設(shè)置于被檢測(cè)對(duì)象的末端。

41、適配器的作用是作為中間組件將反射鏡穩(wěn)定安裝在被測(cè)對(duì)象上。

42、基于同一發(fā)明構(gòu)思，本申請(qǐng)還提供一種電子設(shè)備，所述電子設(shè)備包括：

43、存儲(chǔ)器，用于存儲(chǔ)計(jì)算機(jī)可執(zhí)行指令或者計(jì)算機(jī)程序；

44、處理器，用于執(zhí)行所述存儲(chǔ)器中存儲(chǔ)的計(jì)算機(jī)可執(zhí)行指令或者計(jì)算機(jī)程序時(shí)，實(shí)現(xiàn)上述的位姿跟蹤測(cè)量方法。

45、基于同一發(fā)明構(gòu)思，本申請(qǐng)還提供一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，存儲(chǔ)有計(jì)算機(jī)可執(zhí)行指令，所述計(jì)算機(jī)可執(zhí)行指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)上述的位姿跟蹤測(cè)量方法。

46、本發(fā)明公開了一種位姿跟蹤測(cè)量方法、裝置、設(shè)備及存儲(chǔ)介質(zhì)，所述方法通過采集點(diǎn)位構(gòu)建初始坐標(biāo)系；通過反射鏡設(shè)置跟蹤點(diǎn)位，根據(jù)反射鏡建立目標(biāo)坐標(biāo)系，根據(jù)所述目標(biāo)坐標(biāo)系旋轉(zhuǎn)前后的關(guān)系得到旋轉(zhuǎn)矩陣，實(shí)現(xiàn)了實(shí)時(shí)監(jiān)測(cè)被測(cè)對(duì)象的姿態(tài)變化，以及實(shí)時(shí)跟蹤被測(cè)對(duì)象目標(biāo)坐標(biāo)系，以反射鏡作為跟蹤對(duì)象能實(shí)現(xiàn)更高的測(cè)量精度；本發(fā)明的光學(xué)部件安裝自由度大，可代替現(xiàn)有的基于光學(xué)傳感器的位姿測(cè)量方案，解決了光學(xué)傳感器存在的位姿測(cè)量范圍受限問題；且通過坐標(biāo)系轉(zhuǎn)換映射法和向量積法對(duì)高精度點(diǎn)位數(shù)據(jù)進(jìn)行位姿矩陣數(shù)據(jù)求解，能很好地適應(yīng)被測(cè)對(duì)象間關(guān)聯(lián)關(guān)系構(gòu)建復(fù)雜的場(chǎng)景。

技術(shù)特征：

1.一種位姿跟蹤測(cè)量方法，其特征在于，包括以下步驟：

2.如權(quán)利要求1所述的位姿跟蹤測(cè)量方法，其特征在于，所述初始坐標(biāo)系至少包括初始工具坐標(biāo)系和初始偏移坐標(biāo)系中的一種；

3.如權(quán)利要求1所述的位姿跟蹤測(cè)量方法，其特征在于，所述反射鏡包括第一反射鏡、第二反射鏡和第三反射鏡；所述根據(jù)所述偏移矢量對(duì)所述初始坐標(biāo)系進(jìn)行平移，得到目標(biāo)坐標(biāo)系，包括步驟：

4.如權(quán)利要求3所述的位姿跟蹤測(cè)量方法，其特征在于，所述根據(jù)所述反射鏡的點(diǎn)位坐標(biāo)，計(jì)算空間平面的法向量，并將所述法向量轉(zhuǎn)化為單位向量，包括：

5.如權(quán)利要求4所述的位姿跟蹤測(cè)量方法，其特征在于，所述目標(biāo)坐標(biāo)系為o，旋轉(zhuǎn)后的坐標(biāo)系為a；所述根據(jù)目標(biāo)坐標(biāo)系旋轉(zhuǎn)前后的反射鏡坐標(biāo)系計(jì)算旋轉(zhuǎn)矩陣，根據(jù)所述旋轉(zhuǎn)矩陣和所述單位向量得到目標(biāo)跟蹤矢量，包括：

6.如權(quán)利要求2所述的位姿跟蹤測(cè)量方法，其特征在于，所述根據(jù)被測(cè)對(duì)象的測(cè)量需求，對(duì)所述目標(biāo)坐標(biāo)系進(jìn)行關(guān)系匹配或擬合，包括步驟：

7.一種位姿跟蹤測(cè)量裝置，其特征在于，包括：

8.如權(quán)利要求7所述的位姿跟蹤測(cè)量裝置，其特征在于，還包括：

9.一種電子設(shè)備，其特征在于，所述電子設(shè)備包括：

10.一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，存儲(chǔ)有計(jì)算機(jī)可執(zhí)行指令，其特征在于，所述計(jì)算機(jī)可執(zhí)行指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)權(quán)利要求1至6任一項(xiàng)所述的位姿跟蹤測(cè)量方法。

技術(shù)總結(jié)
本申請(qǐng)涉及位姿測(cè)量領(lǐng)域，尤其涉及一種位姿跟蹤測(cè)量方法、裝置、設(shè)備及存儲(chǔ)介質(zhì)。本申請(qǐng)?jiān)诒粶y(cè)對(duì)象上設(shè)置反射鏡作為跟蹤點(diǎn)，根據(jù)反射鏡建立目標(biāo)，并計(jì)算空間平面法向量的單位向量；根據(jù)所述目標(biāo)坐標(biāo)系旋轉(zhuǎn)前后的關(guān)系得到旋轉(zhuǎn)矩陣，根據(jù)旋轉(zhuǎn)矩陣和所述單位向量得到目標(biāo)跟蹤矢量；根據(jù)被測(cè)對(duì)象的測(cè)量需求，對(duì)所述目標(biāo)坐標(biāo)系進(jìn)行關(guān)系匹配或擬合；根據(jù)所述目標(biāo)跟蹤矢量及所述關(guān)系匹配或擬合的結(jié)果，跟蹤反射鏡位置，進(jìn)行位姿跟蹤測(cè)量，解決了現(xiàn)有技術(shù)位姿測(cè)量范圍受限、測(cè)量精度要求高、測(cè)量對(duì)象間關(guān)聯(lián)關(guān)系構(gòu)建復(fù)雜等實(shí)際測(cè)量問題。

技術(shù)研發(fā)人員：程志遠(yuǎn),李美琦,史東亮,歐雷,龍萍,龔偉,徐超,羅蕓卿,王曉娟,趙慧婷
受保護(hù)的技術(shù)使用者：成都飛機(jī)工業(yè)（集團(tuán)）有限責(zé)任公司
技術(shù)研發(fā)日：
技術(shù)公布日：2025/1/9