所屬的技術(shù)人員能夠理解，本公開的各個方面可以實現(xiàn)為系統(tǒng)、方法或程序產(chǎn)品。因此，本公開的各個方面可以具體實現(xiàn)為以下形式，即：完全的硬件實施例、完全的軟件實施例(包括固件、微代碼等)，或硬件和軟件方面結(jié)合的實施例，這里可以統(tǒng)稱為“電路”、“模塊”或“系統(tǒng)”。下面參照圖10來描述根據(jù)本公開的這種實施例的電子設(shè)備1000。圖10所示的電子設(shè)備1000僅僅是一個示例，不應(yīng)對本公開實施例的功能和使用范圍帶來任何限制。如圖10所示，電子設(shè)備1000以通用計算設(shè)備的形式表現(xiàn)。電子設(shè)備1000的組件可以包括但不限于：上述至少一個處理單元1010、上述至少一個存儲單元1020、連接不同系統(tǒng)組件(包括存儲單元1020和處理單元1010)的總線1030、顯示單元1040。其中，存儲單元存儲有程序代碼，程序代碼可以被處理單元1010執(zhí)行，使得處理單元1010執(zhí)行本說明書上述“示例性方法”部分中描述的根據(jù)本公開各種示例性實施例的步驟；本示例性實施例中，處理單元1010可以為微處理器、微控制單元、單片機(jī)以及嵌入式處理器中的一種或多種。存儲單元1020可以包括易失性存儲單元形式的可讀介質(zhì)，例如隨機(jī)存取存儲單元1021(ram)和/或高速緩存存儲單元1022，還可以進(jìn)一步包括只讀存儲單元1023(rom)。存儲單元1020還可以包括具有一組(至少一個)程序模塊1024的程序/實用工具，這樣的程序模塊1024包括但不限于：操作系統(tǒng)、一個或者多個應(yīng)用程序、其它程序模塊以及程序數(shù)據(jù)，這些示例中的每一個或某種組合中可能包括網(wǎng)絡(luò)環(huán)境的實現(xiàn)?？偩€1030可以為表示幾類總線結(jié)構(gòu)中的一種或多種，包括存儲單元總線或者存儲單元控制器、外圍總線、圖形加速端口、處理單元或者使用多種總線結(jié)構(gòu)中的任意總線結(jié)構(gòu)的局域總線。電子設(shè)備1000也可以與一個或多個外部設(shè)備1070(例如鍵盤、指向設(shè)備、藍(lán)牙設(shè)備等)通信，還可與一個或者多個使得用戶能與該電子設(shè)備1000交互的設(shè)備通信，和/或與使得該電子設(shè)備1000能與一個或多個其它計算設(shè)備進(jìn)行通信的任何設(shè)備(例如路由器、調(diào)制解調(diào)器等等)通信。這種通信可以通過輸入/輸出(i/o)接口1050進(jìn)行。并且，電子設(shè)備1000還可以通過網(wǎng)絡(luò)適配器1060與一個或者多個網(wǎng)絡(luò)(例如局域網(wǎng)(lan)，廣域網(wǎng)(wan)和/或公共網(wǎng)絡(luò)，例如因特網(wǎng))通信。如圖所示，網(wǎng)絡(luò)適配器1060通過總線1030與電子設(shè)備1000的其他模塊通信。應(yīng)當(dāng)明白，盡管圖中未示出，可以結(jié)合電子設(shè)備1000使用其他硬件和/或軟件模塊，包括但不限于：微代碼、設(shè)備驅(qū)動器、冗余處理單元、外部磁盤驅(qū)動陣列、raid系統(tǒng)、磁帶驅(qū)動器以及數(shù)據(jù)備份存儲系統(tǒng)等。通過以上的實施例的描述，本領(lǐng)域的技術(shù)人員易于理解，這里描述的示例實施例可以通過軟件實現(xiàn)，也可以通過軟件結(jié)合必要的硬件的方式來實現(xiàn)。因此，根據(jù)本公開實施例的技術(shù)方案可以以軟件產(chǎn)品的形式體現(xiàn)出來，該軟件產(chǎn)品可以存儲在一個非易失性存儲介質(zhì)(可以是cd-rom，u盤，移動硬盤等)中或網(wǎng)絡(luò)上，包括若干指令以使得一臺計算設(shè)備(可以是個人計算機(jī)、服務(wù)器、終端裝置，或者網(wǎng)絡(luò)設(shè)備等)執(zhí)行根據(jù)本公開實施例的方法。在本公開的示例性實施例中，還提供了一種計算機(jī)可讀存儲介質(zhì)，其上存儲有能夠?qū)崿F(xiàn)本說明書上述方法的程序產(chǎn)品。在一些可能的實施例中，本公開的各個方面還可以實現(xiàn)為一種程序產(chǎn)品的形式，其包括程序代碼，當(dāng)程序產(chǎn)品在終端設(shè)備上運行時，程序代碼用于使終端設(shè)備執(zhí)行本說明書上述“示例性方法”部分中描述的根據(jù)本公開各種示例性實施例的步驟。參考圖11示，描述了根據(jù)本公開的實施例的用于實現(xiàn)上述慣性測量器件標(biāo)定方法的程序產(chǎn)品1100，其可以采用便攜式緊湊盤只讀存儲器(cd-rom)并包括程序代碼，并可以在終端設(shè)備，例如個人電腦上運行。然而，本公開的程序產(chǎn)品不限于此，在本文件中，可讀存儲介質(zhì)可以是任何包含或存儲程序的有形介質(zhì)，該程序可以被指令執(zhí)行系統(tǒng)、裝置或者器件使用或者與其結(jié)合使用。程序產(chǎn)品可以采用一個或多個可讀介質(zhì)的任意組合?？勺x介質(zhì)可以是可讀信號介質(zhì)或者可讀存儲介質(zhì)?？勺x存儲介質(zhì)例如可以為但不限于電、磁、光、電磁、紅外線、或半導(dǎo)體的系統(tǒng)、裝置或器件，或者任意以上的組合?？勺x存儲介質(zhì)的更具體的例子(非窮舉的列表)包括：具有一個或多個導(dǎo)線的電連接、便攜式盤、硬盤、隨機(jī)存取存儲器(ram)、只讀存儲器(rom)、可擦式可編程只讀存儲器(eprom或閃存)、光纖、便攜式緊湊盤只讀存儲器(cd-rom)、光存儲器件、磁存儲器件、或者上述的任意合適的組合。計算機(jī)可讀信號介質(zhì)可以包括在基帶中或者作為載波一部分傳播的數(shù)據(jù)信號，其中承載了可讀程序代碼。這種傳播的數(shù)據(jù)信號可以采用多種形式，包括但不限于電磁信號、光信號或上述的任意合適的組合?？勺x信號介質(zhì)還可以是可讀存儲介質(zhì)以外的任何可讀介質(zhì)，該可讀介質(zhì)可以發(fā)送、傳播或者傳輸用于由指令執(zhí)行系統(tǒng)、裝置或者器件使用或者與其結(jié)合使用的程序?？勺x介質(zhì)上包含的程序代碼可以用任何適當(dāng)?shù)慕橘|(zhì)傳輸，包括但不限于無線、有線、光纜、電磁波等等，或者上述的任意合適的組合?？梢砸砸环N或多種程序設(shè)計語言的任意組合來編寫用于執(zhí)行本公開操作的程序代碼，程序設(shè)計語言包括面向?qū)ο蟮某绦蛟O(shè)計語言—諸如java、c++等，還包括常規(guī)的過程式程序設(shè)計語言—諸如“c”語言或類似的程序設(shè)計語言。程序代碼可以完全地在用戶計算設(shè)備上執(zhí)行、部分地在用戶設(shè)備上執(zhí)行、作為一個獨立的軟件包執(zhí)行、部分在用戶計算設(shè)備上部分在遠(yuǎn)程計算設(shè)備上執(zhí)行、或者完全在遠(yuǎn)程計算設(shè)備或服務(wù)器上執(zhí)行。在涉及遠(yuǎn)程計算設(shè)備的情形中，遠(yuǎn)程計算設(shè)備可以通過任意種類的網(wǎng)絡(luò)，包括局域網(wǎng)(lan)或廣域網(wǎng)(wan)，連接到用戶計算設(shè)備，或者，可以連接到外部計算設(shè)備(例如利用因特網(wǎng)服務(wù)提供商來通過因特網(wǎng)連接)。此外，上述附圖僅是根據(jù)本公開示例性實施例的方法所包括的處理的示意性說明，而不是限制目的。易于理解，上述附圖所示的處理并不表明或限制這些處理的時間順序。另外，也易于理解，這些處理可以是例如在多個模塊中同步或異步執(zhí)行的。通過以上的實施例的描述，本領(lǐng)域的技術(shù)人員易于理解，這里描述的示例實施例可以通過軟件實現(xiàn)，也可以通過軟件結(jié)合必要的硬件的方式來實現(xiàn)。因此，根據(jù)本公開實施例的技術(shù)方案可以以軟件產(chǎn)品的形式體現(xiàn)出來，該軟件產(chǎn)品可以存儲在一個非易失性存儲介質(zhì)(可以是cd-rom，u盤，移動硬盤等)中或網(wǎng)絡(luò)上，包括若干指令以使得一臺計算設(shè)備(可以是個人計算機(jī)、服務(wù)器、觸控終端、或者網(wǎng)絡(luò)設(shè)備等)執(zhí)行根據(jù)本公開實施例的方法。最后，以上優(yōu)選實施例僅用來說明本技術(shù)的技術(shù)方案并非限制性的。盡管已對本技術(shù)進(jìn)行詳細(xì)的描述，但本領(lǐng)域技術(shù)人員應(yīng)理解，可以對其做出形式和細(xì)節(jié)上的變更，而不偏離本技術(shù)權(quán)利要求書所限定的范圍。附圖尺寸與具體實物無關(guān)，實物尺寸可任意變換。

背景技術(shù)：

1、慣性測量器件(inertial?measurement?unit,imu)廣泛應(yīng)用于機(jī)器人、無人機(jī)、移動設(shè)備及導(dǎo)航系統(tǒng)中。大多數(shù)慣性測量器件存在尺度因子和偏差等系統(tǒng)誤差，且這些誤差會隨著時間和使用環(huán)境的變化而漂移，影響測量精度。因此，需要對慣性測量器件標(biāo)定，以校正其尺度因子和偏差，提高精度。

2、常規(guī)的慣性測量器件標(biāo)定方法多采用基于梯度下降的非線性最小二乘優(yōu)化算法。但該方法需要對大量原始數(shù)據(jù)進(jìn)行緩存，并依賴多次迭代計算，導(dǎo)致計算復(fù)雜度較高，不適合資源受限的嵌入式系統(tǒng)。

3、部分相關(guān)技術(shù)中開始采用擴(kuò)展卡爾曼濾波(extended?kalman?filter,ekf)標(biāo)定方法，該方法能夠在無需緩存大量原始數(shù)據(jù)的情況下，直接利用實時觀測數(shù)據(jù)進(jìn)行參數(shù)更新，實時估計和修正傳感器的尺度因子和偏差。

4、在該方法中，需要計算卡爾曼增益k＝ppred·ht·(h·ppred·ht+r)-1；其中，ppred為上一時刻的誤差協(xié)方差預(yù)測值、ht為觀測矩陣的轉(zhuǎn)置矩陣、h為觀測矩陣、r為測量噪聲協(xié)方差。當(dāng)測量值為多維向量時，計算得到的觀測矩陣h則為多維矩陣，進(jìn)而(h·ppred·ht+r)為復(fù)雜的多維矩陣，(h·ppred·ht+r)-1為復(fù)雜多維矩陣的求逆運算。

5、上述矩陣求逆運算計算復(fù)雜度高且需要大量的浮點運算，因此計算開銷很大。一方面，提升了對處理器的性能要求，導(dǎo)致設(shè)備成本難以下降；另一方面，導(dǎo)致算法難以運行在算力不充裕的場景，限制了應(yīng)用范圍。

6、為解決上述問題，公開號為cn110307842a的中國專利申請中，利用加速度計和地磁傳感器的輸出信息構(gòu)建兩個正交矢量，隨后利用這兩個正交矢量對觀測矩陣進(jìn)行簡化。簡化后的觀測矩陣如下：

7、

8、在該專利中，進(jìn)一步利用簡化后的觀測矩陣fk+1(qk+1/k)，將卡爾曼增益中的矩陣運算簡化為無需矩陣求逆的標(biāo)量運算。但這種簡化依賴于特定的初始姿態(tài)誤差的協(xié)方差矩陣，只有在將初始姿態(tài)誤差的協(xié)方差矩陣設(shè)定為pk/k＝a2i4×4才能規(guī)避矩陣求逆，存在較大的使用場景限制。

9、公開號為us20190266499a1的美國專利申請中，利用稀疏計算技術(shù)，通過構(gòu)建新的、尺寸更小的相關(guān)矩陣來減少計算量。對于上述卡爾曼濾波增益計算中的協(xié)方差矩陣求逆部分，構(gòu)建新的觀察矩陣hh＝[h1，0...；0，h2，0...，0，hn](n限于相關(guān)狀態(tài))，同時相應(yīng)地減少p矩陣為pp。通過這種方式，將復(fù)雜的矩陣求逆計算轉(zhuǎn)化為對較小矩陣的操作，從而降低計算復(fù)雜度；但該方法仍無法規(guī)避矩陣求逆運算。

10、類似的，論文《on?computational?complexity?reduction?methods?for?kalmanfilter?extensions》(作者matti?raitoharju等，ieee?aerospace?and?electronicsystems?magazine，2015年8月)等相關(guān)技術(shù)中，也是將高維矩陣求逆轉(zhuǎn)換為更小維度矩陣的逆運算問題。

11、申請人擬提供一種和上述方法不同的新的慣性測量器件標(biāo)定方法，以降低擴(kuò)展卡爾曼濾波標(biāo)定方法的運算復(fù)雜度。

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

1、本公開提供了一種慣性測量器件標(biāo)定方法及裝置、電子設(shè)備、機(jī)器人、存儲介質(zhì)，旨在解決相關(guān)技術(shù)中擴(kuò)展卡爾曼濾波標(biāo)定方法對于處理器的性能要求，導(dǎo)致設(shè)備成本難以下降，以及算法難以運行在算力不充裕的場景，限制了應(yīng)用范圍的問題。

2、本公開的額外方面和優(yōu)點將部分地在下面的描述中闡述，并且部分地將從描述中變得顯然，或者可以通過本公開的實踐而習(xí)得。

3、根據(jù)本公開的第一方面，提供一種慣性測量器件標(biāo)定方法，包括：

4、基于慣性測量器件上一時刻的狀態(tài)量預(yù)測值、當(dāng)前時刻實際測量值以及非線性觀測方程計算殘差值；其中，所述實際測量值為多維向量；

5、對所述非線性觀測方程進(jìn)行線性化得到n行一列的近似觀測矩陣，其中，n等于所述狀態(tài)量的維度；

6、基于上一時刻的誤差協(xié)方差預(yù)測值、測量噪聲協(xié)方差以及所述近似觀測矩陣計算卡爾曼增益；

7、根據(jù)所述卡爾曼增益以及所述近似觀測矩陣，計算當(dāng)前時刻的后驗誤差協(xié)方差預(yù)測值；以及，根據(jù)所述卡爾曼增益以及所述殘差值計算修正項，并利用所述修正項計算所述慣性測量器件在當(dāng)前時刻的后驗狀態(tài)量預(yù)測值。

8、在本公開的一種示例性實施例中，所述對所述非線性觀測方程進(jìn)行線性化得到n行一列的近似觀測矩陣，包括：

9、對所述非線性觀測方程在各所述狀態(tài)量預(yù)測值處求偏導(dǎo)，并將得到的各偏導(dǎo)值作為所述近似觀測矩陣的元素。

10、在本公開的一種示例性實施例中，所述非線性觀測方程為橢球模型：

11、h(state)＝a·(xi-x0)2+b·(yi-y0)2+c·(zi-z0)2

12、其中，[a，b，c，x0，y0，z0]為上一時刻的狀態(tài)量預(yù)測值；a、b、c分別為x、y、z軸的尺度因子平方；x0、y0、z0分別為x、y、z軸偏差；(xi，yi，zi)實際測量值，h(state)為標(biāo)量。

13、在本公開的一種示例性實施例中，所述對所述非線性觀測方程進(jìn)行線性化得到n行一列的近似觀測矩陣，包括：

14、對所述非線性觀測方程在各所述狀態(tài)量預(yù)測值處求偏導(dǎo)：

15、

16、將得到的各偏導(dǎo)值作為所述近似觀測矩陣hi的元素：

17、

18、在本公開的一種示例性實施例中，所述近似觀測矩陣乘上一時刻的誤差協(xié)方差預(yù)測值乘所述近似觀測矩陣的轉(zhuǎn)置矩陣的結(jié)果為標(biāo)量。

19、在本公開的一種示例性實施例中，所述基于上一時刻的誤差協(xié)方差預(yù)測值、測量噪聲協(xié)方差以及所述近似觀測矩陣計算卡爾曼增益，包括：

20、k＝ppred·hit·(hi·ppred·hit+r)-1

21、其中，k為卡爾曼增益，ppred為上一時刻的誤差協(xié)方差預(yù)測值、hit為所述近似觀測矩陣的轉(zhuǎn)置矩陣、hi為所述近似觀測矩陣、r為測量噪聲協(xié)方差。

22、在本公開的一種示例性實施例中，慣性測量器件包括加速度計、陀螺儀以及磁力計中的一種或多種。

23、根據(jù)本公開的第二方面，提供一種慣性測量器件標(biāo)定裝置，包括：

24、殘差計算模塊，用于基于慣性測量器件上一時刻的狀態(tài)量預(yù)測值、當(dāng)前時刻實際測量值以及非線性觀測方程計算殘差值；其中，所述實際測量值為多維向量；

25、觀測矩陣計算模塊，用于對所述非線性觀測方程進(jìn)行線性化得到n行一列的近似觀測矩陣；

26、卡爾曼增益計算模塊，用于基于上一時刻的誤差協(xié)方差預(yù)測值、測量噪聲協(xié)方差以及所述近似觀測矩陣計算卡爾曼增益；

27、數(shù)據(jù)更新模塊，用于根據(jù)所述卡爾曼增益以及所述近似觀測矩陣，計算當(dāng)前時刻的后驗誤差協(xié)方差預(yù)測值；以及，根據(jù)所述卡爾曼增益以及所述殘差值計算修正項，并利用所述修正項計算所述慣性測量器件在當(dāng)前時刻的后驗狀態(tài)量預(yù)測值。

28、根據(jù)本公開的第三方面，提供一種電子設(shè)備，包括：

29、處理器；以及

30、存儲器，所述存儲器上存儲有計算機(jī)可讀指令，所述計算機(jī)可讀指令被所述處理器執(zhí)行時實現(xiàn)如上述任意一項所述的慣性測量器件標(biāo)定方法。

31、在本公開的一種示例性實施例中，所述處理器為微處理器、微控制單元、單片機(jī)以及嵌入式處理器中的一種或多種。

32、根據(jù)本公開的第四方面，提供一種機(jī)器人，包括：

33、處理器；以及

34、存儲器，所述存儲器上存儲有計算機(jī)可讀指令，所述計算機(jī)可讀指令被所述處理器執(zhí)行時實現(xiàn)如上述任意一項所述的慣性測量器件標(biāo)定方法。

35、在本公開的一種示例性實施例中，所述機(jī)器人包括足式機(jī)器人、四足機(jī)器人、雙足機(jī)器人、輪式機(jī)器人、輪足機(jī)器人、四輪足機(jī)器人、人形機(jī)器人、清潔機(jī)器人、運輸機(jī)器人、移動機(jī)器人和機(jī)械臂中的任一種。

36、在本公開的一種示例性實施例中，所述處理器為微處理器、微控制單元、單片機(jī)以及嵌入式處理器中的一種或多種。

37、根據(jù)本公開的第五方面，提供一種計算機(jī)可讀存儲介質(zhì)，所述計算機(jī)可讀存儲介質(zhì)上存儲有計算機(jī)程序代碼指令，當(dāng)所述計算機(jī)程序代碼指令被機(jī)器人的處理器所調(diào)用時，使得所述機(jī)器人執(zhí)行如上述任意一項所述的慣性測量器件標(biāo)定方法。

38、由上述技術(shù)方案可知，本公開具備以下優(yōu)點和積極效果中的至少之一：

39、一方面，相關(guān)技術(shù)中，當(dāng)測量值為多維向量時，計算卡爾曼增益的過程涉及復(fù)雜的多維矩陣逆運算；而本公開的慣性測量器件標(biāo)定方法中，通過對非線性觀測方程進(jìn)行線性化，得到n行一列的近似觀測矩陣，從而可以將復(fù)雜的多維矩陣求逆運算簡化為標(biāo)量運算。這種簡化能夠有效降低計算復(fù)雜度，減少浮點運算需求，降低運算開銷，提升運算速度，并減少對硬件設(shè)備性能的要求，從而降低設(shè)備成本；例如，能夠適配mcu(micro?controllerunit，微處理器)、單片機(jī)等，進(jìn)而大幅度降低硬件的尺寸、成本和電量損耗。另一方面，在低算力設(shè)備中，矩陣求逆運算容易引發(fā)數(shù)值精度損失，進(jìn)而導(dǎo)致誤差積累，影響標(biāo)定的精確性。本公開通過規(guī)避矩陣求逆運算，顯著減少了低算力環(huán)境中的誤差積累，從而保證標(biāo)定精度不會受到顯著影響。因此，基于本公開的慣性測量器件標(biāo)定方法，擴(kuò)展卡爾曼濾波方法能夠應(yīng)用于低算力設(shè)備，擴(kuò)大了算法的應(yīng)用范圍。此外，本公開的慣性測量器件標(biāo)定方法對初始參數(shù)的依賴性較低，即使在初始條件不理想的情況下，也能夠通過實時更新卡爾曼增益，持續(xù)優(yōu)化標(biāo)定精度。