[發明專利]一種基于多傳感器的定位設備及定位方法、系統和機械臂

有效

• 申請號: 201810433771.8
• 申請日: 2018-05-08
• 公開（公告）號: CN108592902B
• 公開（公告）日: 2020-07-31
• 發明（設計）人: 王喆垚;趙鶴
• 申請（專利權）人: 清華大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/20
• 代理公司: 北京潤澤恒知識產權代理有限公司 11319
• 代理人: 莎日娜

1.一種基于多傳感器的定位設備，應用于機械臂，其特征在于，所述定位設備包括多個傳感器和處理器，所述處理器分別與每個所述傳感器相連接，其中：

所述多個傳感器用于檢測所述機械臂的三軸加速度值、三軸角速度值、三軸磁場強度值和超聲數據，并輸出到所述處理器；

所述處理器用于根據所述三軸加速度值計算第一位移，根據所述三軸角速度值計算第一旋轉角度，根據所述三軸磁場強度值和所述三軸加速度值計算第二旋轉角度，根據所述超聲數據計算第二位移；

將所述第一位移、第一旋轉角度、第二位移和第二旋轉角度通過卡爾曼濾波算法進行融合計算，得到所述機械臂的位置數據和方向數據，具體包括：

將所述第一位移和第二位移通過卡爾曼濾波算法進行位置融合計算，得到所述機械臂的位置數據；

將所述第一旋轉角度和第二旋轉角度通過卡爾曼濾波算法進行方向融合計算，得到所述機械臂的方向數據；

所述位置數據和方向數據用于確定機械臂在執行任務時的任務軌跡，利用所述任務軌跡訓練機械臂的控制系統，所述控制系統用于對機械臂或更換后的機械臂進行控制。

2.如權利要求1所述的定位設備，其特征在于，所述多個傳感器包括三軸加速度傳感器、三軸陀螺儀、三軸地磁傳感器和單發三收超聲波傳感器，其中：

所述三軸加速度傳感器用于檢測所述三軸加速度值；

所述三軸陀螺儀用于檢測所述三軸角速度值；

所述三軸地磁傳感器用于檢測所述三軸磁場強度值；

所述單發三收超聲波傳感器用于檢測所述超聲數據。

3.如權利要求1所述的定位設備，其特征在于，所述處理器還用于將所述位置數據和所述方向數據輸出到上位計算機，以使所述上位計算機根據所述位置數據和所述方向數據精確定位所述機械臂的工作軌跡。

4.一種定位方法，應用于如權利要求1～3任一項所述的定位設備，其特征在于，所述定位方法包括步驟：

獲取多個傳感器檢測的三軸加速度值、三軸角速度值、三軸磁場強度值和超聲數據；

將所述三軸加速度值對時間做二重積分計算，得到第一位移；

將所述三軸角速度值對時間做積分計算，得到第一旋轉角度；

將所述三軸磁場強度值和所述三軸加速度值做坐標變換計算，得到第二旋轉角度；

根據所述超聲數據的數據變化進行計算，得到第二位移；

將所述第一位移、所述第一旋轉角度、所述第二位移和所述第二旋轉角度利用卡爾曼濾波算法進行融合計算，得到機械臂的位置數據和方向數據，具體包括：

將所述第一位移和第二位移通過卡爾曼濾波算法進行位置融合計算，得到所述機械臂的位置數據；

將所述第一旋轉角度和第二旋轉角度通過卡爾曼濾波算法進行方向融合計算，得到所述機械臂的方向數據；

所述位置數據和方向數據用于確定機械臂在執行任務時的任務軌跡，利用所述任務軌跡訓練機械臂的控制系統，所述控制系統用于對機械臂或更換后的機械臂進行控制。

5.如權利要求4所述的定位方法，其特征在于，所述將所述第一位移、所述第一旋轉角度、所述第二位移和所述第二旋轉角度利用卡爾曼濾波算法進行融合計算，包括：

利用所述第二位移對所述第一位移進行位置校正計算，得到所述位置數據；

利用所述第二旋轉角度對所述第一旋轉角度進行方向校正計算，得到所述方向數據。

6.如權利要求4所述的定位方法，其特征在于，所述卡爾曼濾波算法為擴展卡爾曼濾波算法或者無損卡爾曼濾波算法。