[發明專利]一種基于六軸陀螺儀的掃地機角度測算方法

有效

• 申請號: 202010350412.3
• 申請日: 2020-04-28
• 公開（公告）號: CN111504254B
• 公開（公告）日: 2021-04-20
• 發明（設計）人: 左海明
• 申請（專利權）人: 深圳市云鼠科技開發有限公司
• 主分類號: G01C1/00
• 分類號: G01C1/00;G01C21/16;A47L11/40
• 代理公司: 深圳正和天下專利代理事務所(普通合伙) 44581
• 代理人: 楊波

1.一種基于六軸陀螺儀的掃地機角度測算方法，其特征在于：包括以下步驟，

S1：掃地機開啟，慣導初始化，任務掛起；預設5MS的任務執行周期，并進行慣導模式判斷；所述慣導模式判斷包括正常模式、復位角度、慣導初始化以及以及IIC恢復；

S2：正常模式下的數據更新包括第一個5MS周期更新以及第二個5MS周期更新；所述第一個5MS周期更新具體包括陀螺儀數據更新以及邏輯計算、零偏更新以及姿態解算；所述第二個5MS周期更新具體包括加速度計數據更新；

S3：所述邏輯計算、零偏更新以及姿態解算具體分為開機初始零偏計算Z軸零偏作Rz、計算隨機零偏、六軸數據去零偏以及姿態解算；

S4：開機初始零偏計算Z軸零偏作Rz具體包括判斷機器是否震動過大、計算六軸零偏、記錄實時溫度以及計算數據方差即Z軸方差記作COV(0)；計算隨機零偏包括溫度檢測和判斷機器是否符合計算隨機零偏條件；姿態解算包括加速計、陀螺儀、PI互補濾波計算四元數以及四元數轉換為歐拉角；

S5：數據測算以及更新輸出完成。

2.如權利要求1所述的一種基于六軸陀螺儀的掃地機角度測算方法，其特征在于：所述步驟S4中溫度檢測過程，若溫度值大于設定閾值則申請停機計算零偏。

3.如權利要求1所述的一種基于六軸陀螺儀的掃地機角度測算方法，其特征在于：所述步驟S4中判斷機器是否符合計算隨機零偏條件包括計算約150組Z軸數據的平均值Rk方差COV(k)、數據離散程度判斷、溫度變化是否超過設定閾值以及零偏校準。

4.如權利要求3所述的一種基于六軸陀螺儀的掃地機角度測算方法，其特征在于：所述零偏校準具體通過卡爾曼濾波動態更新陀螺儀零偏方法，包括以下步驟，

B1：步驟S1：開機靜止4s，計算400組陀螺儀數據的平均值作為初始offset，記作E(0),此時該400組數據測量值的方差記作COV(0)，并記錄此時溫度；

B2：通過IMU六軸數據判斷是否超過閾值從而判斷機器是否為靜態；

B3：機器處于靜止狀態，計算此時陀螺儀數據的平均值，記作E(k),并計算其方差，記作COV(k),k表示第k次成功獲取靜態數據，若之后能繼續成功獲取靜態載體姿態數據，則k＝k+1,利用卡爾曼算法計算，得到新的陀螺儀offset,之后跳轉到所述步驟S2，形成循環；

B4：溫度超過設定設定閾值，則申請停機，重新采集數據，復位卡爾曼濾波的參數；初值賦值如下：其中P(0|0)＝COV(0)，X(0|0)＝E(0),Q(k)＝0.0001(Q(k)可根據具體情況設置，R(k)＝COV(k)，通過迭代計算，可以得到不同k時刻的補償值X(k|k)；X(k|k-1)＝X(k-1|k-1)其中k表示第k次測量滿足靜態條件的判定(此后稱之為k時刻)，此時X(k-1|k-1)為k-1時刻Rz的最優補償值，即Offset，X(k|k-1)為通過k-1時刻的Rz最優補償值得到的k時刻Rz的估計補償值；P(k|k-1)＝P(k-1|k-1)+Q(k)P(k-1|k-1)為k-1時刻的Rz的最優補償值的協方差；P(k|k-1)為k時刻的估計補償值的協方差，Q(k)為k時刻估計噪聲的方差；X(k|k)＝X(k|k-1)+Kg(k)(E(k)–X(k|k-1))；Kg(k)＝P(k|k-1)/(P(k|k-1)+R(k))；P(k|k)＝(1–Kg(k))P(k|k-1)；Offset＝X(k|k)；其中R(k)為k時刻測量噪聲的方差，Kg(k)為k時刻的卡爾曼增益，P(k|k)為k時刻最優補償值的協方差，X(k|k)表示對k時刻Rz的測量平均值E(k)與估計值X(k|k-1)通過卡爾曼增益值分配權重獲得的k時刻Rz的最優補償值，最后將X(k|k)賦值給Offset，完成Rz補償值的更新，此時的Offset為獲取的最終補償值。

5.如權利要求1所述的一種基于六軸陀螺儀的掃地機角度測算方法，其特征在于：所述步驟S1中的掃地機包括主機體以及設置于所述主機體內部的電源、主控制器、數據存儲器、用于進行信號采集的六軸陀螺儀；還包括與外部移動終端進行通訊連接的無線通訊傳輸單元、儲水水箱以及安設于主機體下部的邊刷組件以及左右驅動輪組件；在所述主機體內部分別設置有用于對左右驅動輪組件的運行進行控制、接收來自控制器的PWM控制信號的左右輪控制驅動電路單元、用于對左右驅動輪組件的運行速率進行實時檢測的左右輪速度檢測電路單元以及用于對邊刷組件的運行進行控制的邊刷驅動電路單元；該無線通訊傳輸單元、左右輪控制驅動電路單元、左右輪速度檢測電路單元、邊刷驅動電路單元、六軸陀螺儀與主控制器電性連接。