[發明專利]三維姿態定位方法、裝置及電子設備

有效

• 申請號: 201710961392.1
• 申請日: 2017-10-17
• 公開（公告）號: CN107608541B
• 公開（公告）日: 2021-03-05
• 發明（設計）人: 任仲超;陸小松;張濤;蒲天發
• 申請（專利權）人: 寧波視睿迪光電有限公司
• 主分類號: G06F3/0346
• 分類號: G06F3/0346;G06T7/70
• 代理公司: 北京超凡志成知識產權代理事務所(普通合伙) 11371
• 代理人: 蘇勝

1.一種三維姿態定位方法，其特征在于，所述方法包括：

獲取內部設置有紅外二極管的飛鼠的傳感器測定的所述飛鼠在多個預設位置分別對應的多個第一位置數據，所述第一位置數據包括姿態角數據以及第一平面坐標；

根據設置有紅外濾波片的雙目CMOS攝像頭采集的所述飛鼠在所述多個預設位置的位置圖像獲得所述飛鼠在所述多個預設位置分別對應的多個第一空間坐標；

基于第一預設算法獲得多個所述第一平面坐標以及多個所述第一空間坐標對應的多個第一位置坐標；

基于所述多個第一位置坐標以及多個所述姿態角數據生成所述飛鼠在所述多個預設位置分別對應的第二位置數據；

其中，所述根據設置有紅外濾波片的雙目CMOS攝像頭采集的所述飛鼠在所述多個預設位置的位置圖像獲得所述飛鼠在所述多個預設位置分別對應的多個第一空間坐標，包括：

獲取所述雙目CMOS攝像頭的第一CMOS攝像頭以及第二CMOS攝像頭分別采集的所述飛鼠在一所述預設位置的第一圖像以及第二圖像；

獲取所述第一圖像對應的第一反向投影圖以及所述第二圖像對應的第二反向投影圖，所述第一反向投影圖以及所述第二反向投影圖中包括所述飛鼠對應的區域；

根據RANSAC匹配算法獲取所述第一反向投影圖以及所述第二反向投影圖中所述飛鼠對應的區域中的坐標點對應的第二變換矩陣以及滿足所述第二變換矩陣的第三坐標點；

基于所述第三坐標點、所述第二變換矩陣以及預設模型獲取所述飛鼠在所述預設位置的第二橫坐標、第二縱坐標以及第一豎坐標；

重復所述獲取所述雙目CMOS攝像頭的第一CMOS攝像頭以及第二CMOS攝像頭分別采集的所述飛鼠在一所述預設位置的第一圖像以及第二圖像的步驟，至所述基于所述第三坐標點、所述第二變換矩陣以及預設模型獲取所述飛鼠在所述預設位置的第二橫坐標、第二縱坐標以及第一豎坐標的步驟，獲得所述飛鼠在所述多個預設位置的第二橫坐標、第二縱坐標以及第一豎坐標構成的第一空間坐標。

2.根據權利要求1所述的方法，其特征在于，所述第一平面坐標包括第一橫坐標以及第一縱坐標，所述第一空間坐標包括第二橫坐標、第二縱坐標以及第一豎坐標，所述基于第一預設算法獲得多個所述第一平面坐標以及多個所述第一空間坐標對應的多個第一位置坐標，包括：

基于預設坐標校正算法獲得所述多個所述第一橫坐標以及多個所述第一縱坐標，與多個所述第二橫坐標以及多個所述第二縱坐標的校正函數；

基于所述校正函數對多個所述第一橫坐標、多個所述第一縱坐標、多個所述第二橫坐標以及多個所述第二縱坐標進行校正，獲得所述飛鼠在多個預設位置處的第三橫坐標以及第三縱坐標；

基于多個所述第三橫坐標、多個所述第三縱坐標以及多個所述第一豎坐標生成所述飛鼠在多個預設位置處的第一位置坐標。

3.根據權利要求2所述的方法，其特征在于，所述基于預設坐標校正算法獲得所述多個所述第一橫坐標以及多個所述第一縱坐標，與多個所述第二橫坐標以及多個所述第二縱坐標的校正函數，包括：

基于預設特征匹配算法獲得多個所述第一橫坐標以及多個所述第一縱坐標構成的第一集合映射到多個所述第二橫坐標以及多個所述第二縱坐標構成的第二集合的第一變換矩陣；

獲取所述第二集合經過所述第一變換矩陣映射的第三集合與所述第二集合的差值集合；

利用最小二乘法對所述差值集合的平方求偏導，獲得偏差最小的差值點對應的第一橫坐標與第一縱坐標構成的第一坐標點，以及第二橫坐標與第二縱坐標構成的第二坐標點；

基于所述第一坐標點以及所述第二坐標點生成第一擬合直線函數作為所述校正函數。