[發(fā)明專利]一種移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)裝置及方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202110276939.0
• 申請(qǐng)日: 2021-03-15
• 公開(kāi)（公告）號(hào): CN113029000A
• 公開(kāi)（公告）日: 2021-06-25
• 發(fā)明（設(shè)計(jì)）人: 柯映林;李江雄;柯臻錚
• 申請(qǐng)（專利權(quán)）人: 浙江大學(xué)
• 主分類號(hào): G01B11/02
• 分類號(hào): G01B11/02;G01B11/00;G06T7/292;G06T7/73
• 代理公司: 杭州天勤知識(shí)產(chǎn)權(quán)代理有限公司 33224
• 代理人: 顏果

1.一種移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)裝置，其特征在于，包括布置在地面的標(biāo)定板和設(shè)置在整體平臺(tái)底部的：

激光測(cè)距位移傳感器，布置在整體平臺(tái)的四角處，用于獲取整體平臺(tái)的四角與水平標(biāo)定板之間的距離，以判斷整體平臺(tái)在空間上是否發(fā)生翻轉(zhuǎn)；

視覺(jué)相機(jī)，布置有兩個(gè)，位于整體平臺(tái)前后兩側(cè)成對(duì)角布置，通過(guò)拍攝標(biāo)定板上的標(biāo)記點(diǎn)，判斷整體平臺(tái)在水平方向上是否發(fā)生移動(dòng)和旋轉(zhuǎn)。

2.根據(jù)權(quán)利要求1所述的移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)裝置，其特征在于，所述的標(biāo)定板上設(shè)有與每個(gè)視覺(jué)相機(jī)對(duì)應(yīng)的五孔陣列，包括四個(gè)均勻布置的小孔和設(shè)置在四個(gè)小孔中心位置的大孔。

3.根據(jù)權(quán)利要求1所述的移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)裝置，其特征在于，所述的標(biāo)定板包括安裝在站位底座的同一參考面內(nèi)的四個(gè)。

4.根據(jù)權(quán)利要求1所述的移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)裝置，其特征在于，所述的激光測(cè)距位移傳感器和所述的視覺(jué)相機(jī)安裝在同一水平面上。

5.根據(jù)權(quán)利要求1所述的移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)裝置，其特征在于，所述的視覺(jué)相機(jī)配有環(huán)形光源。

6.一種移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)方法，基于權(quán)利要求1～5任一權(quán)利要求所述的移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)裝置實(shí)現(xiàn)，其特征在于，包括以下步驟：

1)通過(guò)激光測(cè)距位移傳感器測(cè)量整體平臺(tái)四角至水平地面的距離，獲得四個(gè)當(dāng)前距離值，與期望姿態(tài)時(shí)的四個(gè)期望距離值相比，得到四個(gè)距離偏差值；

2)視覺(jué)相機(jī)的焦平面被配置在標(biāo)定板表面，獲取標(biāo)記點(diǎn)圖像，通過(guò)數(shù)字圖像處理算法提取標(biāo)記點(diǎn)坐標(biāo)值，得到標(biāo)記點(diǎn)的位置偏差；

3)結(jié)合距離偏差和位置偏差，通過(guò)匹配當(dāng)前特征點(diǎn)和期望特征點(diǎn)的坐標(biāo)值計(jì)算當(dāng)前位姿偏差，獲取整體平臺(tái)的當(dāng)前位姿。

7.根據(jù)權(quán)利要求1所述的移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)方法，其特征在于，步驟1)中，設(shè)置與所述視覺(jué)相機(jī)對(duì)應(yīng)的五孔陣列，五孔陣列包括四個(gè)均勻布置的小孔和設(shè)置在四個(gè)小孔中心位置的大孔，大孔的圓心作為所述的標(biāo)記點(diǎn)。

8.根據(jù)權(quán)利要求7所述的移動(dòng)平臺(tái)位姿狀態(tài)監(jiān)測(cè)方法，其特征在于，步驟1)中四個(gè)當(dāng)前距離值為(d₁，d₂，d₃，d₄)，四個(gè)距離偏差值為(Δd₁，Δd₂，Δd₃，Δd₄)，標(biāo)定板上兩個(gè)大孔的位置偏差為(Δx₁，Δy₁，Δx₂，Δy₂)，則六個(gè)監(jiān)測(cè)點(diǎn)的初始坐標(biāo)值分別為：

激光測(cè)距點(diǎn)：

大孔中心點(diǎn)：

X_Goal、Y_Goal、Z_Goal，分別表示監(jiān)測(cè)點(diǎn)在目標(biāo)坐標(biāo)系下的X、Y、Z三個(gè)方向上的理論值；

采用奇異值分解求變換矩陣，將目標(biāo)坐標(biāo)值變換至當(dāng)前坐標(biāo)系：

激光測(cè)距點(diǎn)：

大孔中心點(diǎn)：

X′_Cur、Y′_Cur、Z′_Cur，分別表示監(jiān)測(cè)點(diǎn)在當(dāng)前坐標(biāo)系下的X、Y、Z三個(gè)方向上的坐標(biāo)值；

迭代上述過(guò)程求解變換矩陣，直至匹配誤差收斂，獲得當(dāng)前的姿態(tài)偏差。