本技術(shù)涉及里程計設(shè)計，尤其是涉及一種基于輪速計和imu融合的里程計設(shè)計方法、系統(tǒng)及設(shè)備。

背景技術(shù)：

1、里程計是一種利用從移動傳感器獲得的數(shù)據(jù)來估計物體位置隨時間的變化而改變的方法。常用的里程計定位方法有輪式里程計、視覺里程計以及視覺慣性里程計。里程計除了用于車輛定位與建圖外，還可用于其他需要里程計的場景。

2、里程計設(shè)計的精確度越高，車輛航位推算的準確度就越高，航位推算是一個很常見的定位方法。然而由于現(xiàn)有的里程計系統(tǒng)包含有不能觀測的方向，這將導(dǎo)致航跡推算的精度與穩(wěn)定性都會比較差。

技術(shù)實現(xiàn)思路

1、本技術(shù)旨在提出一種基于輪速計和imu融合的里程計設(shè)計方法、系統(tǒng)及設(shè)備，能夠提高航跡推算的精度與穩(wěn)定性。

2、第一方面，本技術(shù)實施例提供了一種基于輪速計和imu融合的里程計設(shè)計方法，所述方法包括：

3、獲取慣性測量單元測量的多個數(shù)據(jù)，以及獲取所述慣性測量單元中將坐標系b系按照z軸、x軸和y軸的方向依次旋轉(zhuǎn)得到的姿態(tài)旋轉(zhuǎn)矩陣；

4、基于所述姿態(tài)旋轉(zhuǎn)矩陣，確定里程計的狀態(tài)空間模型；

5、基于所述狀態(tài)空間模型，構(gòu)建輪速計的速度觀測模型；

6、計算所述速度觀測模型的第一誤差狀態(tài)，并根據(jù)所述第一誤差狀態(tài)，確定所述輪速計的觀測矩陣；

7、將所述慣性測量單元測量的多個數(shù)據(jù)輸入至所述狀態(tài)空間模型，得到所述狀態(tài)空間模型輸出的預(yù)測值；

8、計算所述狀態(tài)空間模型的第二誤差狀態(tài)，并基于所述狀態(tài)空間模型、所述第二誤差狀態(tài)、所述預(yù)測值和所述輪速計的觀測矩陣，估計所述里程計的狀態(tài)。

9、與現(xiàn)有技術(shù)相比，本技術(shù)第一方面具有以下有益效果：

10、本方法通過獲取慣性測量單元測量的多個數(shù)據(jù)，以及獲取慣性測量單元中將坐標系b系按照z軸、x軸和y軸的方向依次旋轉(zhuǎn)得到的姿態(tài)旋轉(zhuǎn)矩陣；基于姿態(tài)旋轉(zhuǎn)矩陣，確定里程計的狀態(tài)空間模型；基于狀態(tài)空間模型，構(gòu)建輪速計的速度觀測模型；計算速度觀測模型的第一誤差狀態(tài)，并根據(jù)第一誤差狀態(tài)，確定輪速計的觀測矩陣；將慣性測量單元測量的多個數(shù)據(jù)輸入至狀態(tài)空間模型，得到狀態(tài)空間模型輸出的預(yù)測值；計算狀態(tài)空間模型的第二誤差狀態(tài)，并基于狀態(tài)空間模型、第二誤差狀態(tài)、預(yù)測值和輪速計的觀測矩陣，估計里程計的狀態(tài)。如此，通過將坐標系b系按照z軸、x軸和y軸的方向依次旋轉(zhuǎn)得到的姿態(tài)旋轉(zhuǎn)矩陣，然后根據(jù)姿態(tài)旋轉(zhuǎn)矩陣構(gòu)建狀態(tài)空間模型，再基于狀態(tài)空間模型，估計里程計系統(tǒng)的狀態(tài)，這個狀態(tài)空間模型中沒有包含不能觀測的方向，從而能夠提高航跡推算的精度與穩(wěn)定性。

11、在一些實施方式中，通過如下方式基于所述姿態(tài)旋轉(zhuǎn)矩陣，確定里程計的狀態(tài)空間模型：

12、

13、其中，表示慣性測量單元坐標系系在導(dǎo)航坐標系系下的速度的導(dǎo)數(shù)，表示坐標系系在導(dǎo)航坐標系系下的加速度，表示坐標系系到導(dǎo)航坐標系系的姿態(tài)旋轉(zhuǎn)矩陣，，表示坐標系系繞z軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示坐標系系繞x軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示坐標系系繞y軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示加速度計的測量值，表示所述加速度計的測量零偏，表示系下的重力加速度，表示系下的重力加速度，表示坐標系系到導(dǎo)航坐標系系的姿態(tài)旋轉(zhuǎn)矩陣的導(dǎo)數(shù)，表示坐標系系在導(dǎo)航坐標系系下的的角速度，表示陀螺儀的測量值，表示所述陀螺儀的測量零偏，表示所述加速度計的測量零偏的導(dǎo)數(shù)，表示所述陀螺儀的測量零偏的導(dǎo)數(shù)，表示轉(zhuǎn)置，表示叉乘的矩陣。

14、在一些實施方式中，通過如下方式基于所述狀態(tài)空間模型，構(gòu)建輪速計的速度觀測模型：

15、

16、

17、其中，表示不考慮噪聲時的輪速計速度觀測模型的標稱狀態(tài)，表示慣性測量單元坐標系系在導(dǎo)航坐標系系下的速度，表示慣性測量單元坐標系系到導(dǎo)航坐標系系再到水平坐標系系下的速度，表示坐標系系繞z軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示坐標系系繞x軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示坐標系系繞y軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示轉(zhuǎn)置。

18、在一些實施方式中，通過如下方式計算所述速度觀測模型的第一誤差狀態(tài)：

19、

20、其中，表示所述速度觀測模型的第一誤差狀態(tài)，表示輪速計的速度觀測模型的真實值，表示不考慮噪聲時的輪速計速度觀測模型的標稱狀態(tài)，表示的誤差狀態(tài)，表示慣性測量單元坐標系系到導(dǎo)航坐標系系再到水平坐標系系下的速度，表示叉乘的矩陣，，表示坐標系系繞x軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示坐標系系繞y軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示轉(zhuǎn)置，表示姿態(tài)旋轉(zhuǎn)矢量的誤差狀態(tài)。

21、在一些實施方式中，通過如下方式計算所述狀態(tài)空間模型的第二誤差狀態(tài)：

22、

23、其中，表示的誤差狀態(tài)，表示慣性測量單元坐標系系在導(dǎo)航坐標系系下的速度在水平坐標系系下的投影，表示的真實值，表示叉乘的矩陣，，表示坐標系系繞x軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示坐標系系繞y軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示轉(zhuǎn)置，表示姿態(tài)旋轉(zhuǎn)矢量的誤差狀態(tài)，表示的真實值，表示三行三列的單位矩陣。

24、在一些實施方式中，通過如下方式確定所述輪速計的觀測矩陣：

25、

26、其中，表示所述輪速計的能觀性矩陣，，表示坐標系系繞x軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示坐標系系繞y軸方向旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，表示轉(zhuǎn)置，表示慣性測量單元坐標系系在導(dǎo)航坐標系系下的速度在水平坐標系系下的投影，表示叉乘的矩陣，表示全為零的矩陣。

27、在一些實施方式中，所述基于所述狀態(tài)空間模型、所述第二誤差狀態(tài)、所述預(yù)測值和所述輪速計的觀測矩陣，估計所述里程計的狀態(tài)，包括：

28、基于所述狀態(tài)空間模型、所述第二誤差狀態(tài)、所述預(yù)測值和所述輪速計的觀測矩陣，采用卡爾曼濾波估計所述里程計的狀態(tài)。

29、第二方面，本技術(shù)實施例還提供了一種基于輪速計和imu融合的里程計設(shè)計系統(tǒng)，所述系統(tǒng)包括：

30、數(shù)據(jù)獲取單元，用于獲取慣性測量單元測量的多個數(shù)據(jù)，以及獲取所述慣性測量單元中將坐標系b系按照z軸、x軸和y軸的方向依次旋轉(zhuǎn)得到的姿態(tài)旋轉(zhuǎn)矩陣；

31、模型確定單元，用于基于所述姿態(tài)旋轉(zhuǎn)矩陣，確定里程計的狀態(tài)空間模型；

32、模型構(gòu)建單元，用于基于所述狀態(tài)空間模型，構(gòu)建輪速計的速度觀測模型；

33、數(shù)據(jù)計算單元，用于計算所述速度觀測模型的第一誤差狀態(tài)，并根據(jù)所述第一誤差狀態(tài)，確定所述輪速計的觀測矩陣；

34、數(shù)據(jù)預(yù)測單元，用于將所述慣性測量單元測量的多個數(shù)據(jù)輸入至所述狀態(tài)空間模型，得到所述狀態(tài)空間模型輸出的預(yù)測值；

35、狀態(tài)估計單元，用于計算所述狀態(tài)空間模型的第二誤差狀態(tài)，并基于所述狀態(tài)空間模型、所述第二誤差狀態(tài)、所述預(yù)測值和所述輪速計的觀測矩陣，估計所述里程計的狀態(tài)。

36、第三方面，本技術(shù)實施例還提供了一種電子設(shè)備，包括至少一個控制處理器和用于與所述至少一個控制處理器通信連接的存儲器；所述存儲器存儲有可被所述至少一個控制處理器執(zhí)行的指令，所述指令被所述至少一個控制處理器執(zhí)行，以使所述至少一個控制處理器能夠執(zhí)行如上所述的一種基于輪速計和imu融合的里程計設(shè)計方法。

37、第四方面，本技術(shù)實施例還提供了一種計算機可讀存儲介質(zhì)，所述計算機可讀存儲介質(zhì)存儲有計算機可執(zhí)行指令，所述計算機可執(zhí)行指令用于使計算機執(zhí)行如上所述的一種基于輪速計和imu融合的里程計設(shè)計方法。

38、可以理解的是，上述第二方面至第四方面與相關(guān)技術(shù)相比存在的有益效果與上述第一方面與相關(guān)技術(shù)相比存在的有益效果相同，可以參見上述第一方面中的相關(guān)描述，在此不再贅述。