[發明專利]一種6自由度空間位置和姿態的檢測系統和方法

在審

• 申請號: 201710611047.5
• 申請日: 2017-07-25
• 公開（公告）號: CN107270900A
• 公開（公告）日: 2017-10-20
• 發明（設計）人: 林焯華
• 申請（專利權）人: 廣州阿路比電子科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 北京品源專利代理有限公司11332
• 代理人: 孟金喆

1.一種6自由度空間位置和姿態的檢測系統，其特征在于，包括傳感器模塊、圖像采集模塊和處理器，所述傳感器模塊和所述圖像采集模塊均設于被測目標上；所述傳感器模塊和所述圖像采集模塊分別與所述處理器連接；其中：

所述傳感器模塊，用于測量與所述被測目標的位置和姿態相關的傳感數據；

所述圖像采集模塊，用于采集所述被測目標四周的圖像數據；

所述處理器，用于處理所述傳感數據和所述圖像數據獲得6自由度空間位置和姿態數據。

2.根據權利要求1所述的檢測系統，其特征在于，

所述傳感器模塊，包括加速度計、陀螺儀和磁力計；其中：所述加速度計用于測量所述被測目標的加速度，所述陀螺儀用于測量所述被測目標的角速度，所述磁力計用于測量所述被測目標四周的磁場的分布量；

所述圖像采集模塊，包括至少四個攝像頭。

3.根據權利要求2所述的檢測系統，其特征在于，

所述檢測系統的四個側面分別包括至少一個所述攝像頭。

4.根據權利要求2所述的檢測系統，其特征在于，

所述加速度計為三軸加速度計；

所述陀螺儀為三軸陀螺儀；

所述磁力計為三軸磁力計；

所述攝像頭為廣角攝像頭。

5.根據權利要求1-4任一項所述的檢測系統，其特征在于，還包括：

無線傳輸模塊，與所述處理器連接，用于輸出所述6自由度空間位置和姿態數據；和

電源管理模塊，與所述傳感器模塊、所述圖像采集模塊、所述處理器和所述無線傳輸模塊連接，用于對各部分進行電力供應和電力管理。

6.一種6自由度空間位置和姿態的檢測方法，其特征在于，

該方法由權利要求1-5任一項所述的6自由度空間位置和姿態的檢測系統的處理器來執行，包括：

接收原始數據，所述原始數據包括與被測目標的位置和姿態相關的傳感數據和圖像數據；

對所述原始數據進行數據標定、融合或相互補償處理獲得6自由度空間位置和姿態數據。

7.根據權利要求6所述的檢測方法，其特征在于，所述傳感數據包括加速度、角速度和磁場的分布量。

8.根據權利要求7所述的檢測方法，其特征在于，對所述原始數據進行數據標定、融合或相互補償處理，包括：

對每個所述攝像頭采集到的所述圖像數據進行標定及坐標系轉換；

對每個所述經標定及坐標系轉換后的圖像數據進行處理，得到與傳感器模塊的坐標系相對應的初始空間位置數據和姿態數據；

將多個所述初始空間位置數據和所述姿態數據進行融合，得到融合后的所述空間位置數據和所述姿態數據；

利用所述角速度計算所述被測目標的原始姿態數據，所述原始姿態數據包括俯仰角、偏航角和滾轉角；

利用所述加速度、所述磁場的分布量和所述融合后的姿態數據對所述原始姿態數據進行校正，得到所述3自由度姿態數據；

利用所述加速度、所述原始姿態數據和所述空間位置信息得到所述3自由度空間位置數據。

9.根據權利要求8所述的檢測方法，其特征在于，

在將多個所述初始空間位置數據和所述姿態數據進行融合前，還包括：為多個所述初始空間位置和所述姿態數據分別設定一個權值。

10.根據權利要求8所述的檢測方法，其特征在于，

利用所述加速度、所述磁場的分布量和所述融合后的姿態數據對所述原始姿態數據進行校正，得到所述3自由度姿態數據，包括：

利用所述加速度，對所述俯仰角和所述滾轉角進行補償校正，利用所述磁場的分布量和所述融合后的姿態數據對所述偏航角進行補償校正，得到校正后的所述3自由度姿態數據；

利用所述加速度、所述原始姿態數據和所述空間位置信息得到所述3自由度空間位置數據，包括：

利用所述加速度和所述原始姿態數據，計算線性加速度，并對所述線性加速度進行積分計算，得到第一速度值，對所述空間位置信息進行微分計算，得到第二速度值，利用所述第二速度值對所述第一速度值進行補償校正，并對補償校正后的速度值進行積分計算，得到所述3自由度空間位置數據。