本發(fā)明涉及移動(dòng)機(jī)器人定位與建圖，更具體地，涉及一種基于足腿類機(jī)器人運(yùn)動(dòng)特征約束的定位建圖方法及系統(tǒng)。

背景技術(shù)：

1、定位建圖是移動(dòng)機(jī)器人運(yùn)動(dòng)控制的重要一環(huán)，是機(jī)器人實(shí)現(xiàn)自主路徑規(guī)劃與任務(wù)執(zhí)行的重要前提，現(xiàn)有的定位建圖僅依靠單一的觀測類傳感器進(jìn)行增量式定位估計(jì)，盡管這些方法能夠?qū)崿F(xiàn)實(shí)時(shí)狀態(tài)的復(fù)雜場景定位建圖，但是對于足腿類移動(dòng)機(jī)器人其特殊的腿式運(yùn)動(dòng)方式使得在運(yùn)動(dòng)過程中造成高頻抖動(dòng)特性，在面向復(fù)雜環(huán)境中，傳統(tǒng)的定位方法極易產(chǎn)生定位漂移累積造成退化現(xiàn)象，直接影響系統(tǒng)定位質(zhì)量與建圖效率。

2、中國專利公開號(hào)cn113390411a公開了一種基于變構(gòu)型感知裝置的足式機(jī)器人導(dǎo)航及定位方法，該方法增加相關(guān)機(jī)械結(jié)構(gòu)的旋轉(zhuǎn)自由度改變傳感器的視野范圍以增強(qiáng)足式機(jī)器人的定位能力，但其變構(gòu)機(jī)械的特殊性及尺寸大小不能夠適應(yīng)大多數(shù)的足腿類機(jī)器人；中國專利公開號(hào)cn106056643a公開了一種基于點(diǎn)云的室內(nèi)動(dòng)態(tài)場景slam方法及系統(tǒng)，該方法通過lm迭代優(yōu)化收斂得到最優(yōu)位姿估計(jì)，但其建圖為二維平面地圖，無法適用于足腿類機(jī)器人的三維狀態(tài)空間狀態(tài)估計(jì)；中國專利公開號(hào)cn114987642a公開了一種基于對傳感器融合的移動(dòng)機(jī)器人定位方法，該方法能夠融合多種傳感器數(shù)據(jù)得到高精度的實(shí)時(shí)定位，但是較為依賴機(jī)器人的重定位性能，無法保證足腿類機(jī)器人其在復(fù)雜環(huán)境中的實(shí)時(shí)魯棒性需求；中國專利公開號(hào)cn117451032a公開了一種低算力與松耦合的激光雷達(dá)和imu的slam方法和系統(tǒng)，其方法能夠適用于機(jī)器人實(shí)時(shí)定位估計(jì)，但其數(shù)據(jù)融合采用松耦合處理，無法適用于足腿類機(jī)器人復(fù)雜環(huán)境下高頻足式運(yùn)動(dòng)工況下的高精度定位需求。

3、綜上所述，當(dāng)前移動(dòng)機(jī)器人定位建圖雖然較為成熟，但是適配于足腿類移動(dòng)機(jī)器人的定位建圖方法較少，其足腿類移動(dòng)機(jī)器人的高頻腿式運(yùn)動(dòng)在復(fù)雜環(huán)境中存在局部定位魯棒性不足、定位建圖差異較大和定位失真退化嚴(yán)重等問題。因此，需要提供一種對上述現(xiàn)有技術(shù)不足的改進(jìn)技術(shù)方案，同時(shí)還要具備能在足腿類機(jī)器人的主機(jī)上實(shí)時(shí)運(yùn)行的能力。

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

1、發(fā)明目的：本技術(shù)的目的在于提供一種基于足腿類機(jī)器人運(yùn)動(dòng)特征約束的定位建圖方法及系統(tǒng)，該方法能夠?qū)崿F(xiàn)足式機(jī)器人在復(fù)雜環(huán)境下高精度實(shí)時(shí)定位建圖，解決足式機(jī)器人在復(fù)雜環(huán)境下局部定位退化失真，定位魯棒性不足和足腿運(yùn)動(dòng)帶來的定位漂移偏差累積等問題，并對足式機(jī)器人在長時(shí)間、長距離下的精確定位和精確地圖構(gòu)建的技術(shù)問題提供一種有效的解決方案。

2、技術(shù)方案：

3、本技術(shù)第一方面提供一種基于足腿類機(jī)器人運(yùn)動(dòng)特征約束的定位建圖方法及系統(tǒng)，包括以下步驟：

4、s1：獲取足式機(jī)器人的各項(xiàng)傳感器數(shù)據(jù)，所述傳感器數(shù)據(jù)包括足式機(jī)器人的各個(gè)關(guān)節(jié)電機(jī)編碼器數(shù)據(jù)、激光雷達(dá)原始點(diǎn)云數(shù)據(jù)、imu慣性單元數(shù)據(jù)，對各項(xiàng)傳感數(shù)據(jù)進(jìn)行時(shí)間戳對齊與數(shù)據(jù)預(yù)處理；

5、s2：根據(jù)imu慣性單元數(shù)據(jù)進(jìn)行時(shí)間積分，推算得到機(jī)器人的慣性先驗(yàn)位姿估計(jì)；

6、s3：根據(jù)機(jī)器人關(guān)節(jié)編碼器數(shù)據(jù)、慣性先驗(yàn)位姿狀態(tài)及機(jī)器人足式運(yùn)動(dòng)學(xué)，得到機(jī)器人的運(yùn)動(dòng)學(xué)狀態(tài)估計(jì)；

7、s4：根據(jù)慣性先驗(yàn)位姿及所述的激光雷達(dá)原始數(shù)據(jù)，將激光雷達(dá)采集點(diǎn)的運(yùn)動(dòng)畸變進(jìn)行逐點(diǎn)補(bǔ)償，并構(gòu)建激光特征點(diǎn)的殘差模型；

8、s5：根據(jù)機(jī)器人的運(yùn)動(dòng)學(xué)狀態(tài)和特征點(diǎn)殘差模型，構(gòu)建運(yùn)動(dòng)約束模型及迭代優(yōu)化模型，并基于收斂閾值或迭代次數(shù)進(jìn)行狀態(tài)優(yōu)化更新，輸出機(jī)器人實(shí)時(shí)最優(yōu)位姿狀態(tài)更新；

9、s6：根據(jù)機(jī)器人的實(shí)時(shí)最優(yōu)位姿狀態(tài)估計(jì)，篩選關(guān)鍵幀并構(gòu)建位姿圖優(yōu)化模型，對傳感器各項(xiàng)數(shù)據(jù)積分累計(jì)的狀態(tài)誤差進(jìn)行后端優(yōu)化，將優(yōu)化后的狀態(tài)數(shù)據(jù)在s5中進(jìn)行修正更新；將關(guān)鍵幀地圖轉(zhuǎn)換到初始坐標(biāo)系下，進(jìn)行地圖的累積與發(fā)布；循環(huán)執(zhí)行s1到s6，進(jìn)行機(jī)器人位置與姿態(tài)的定位更新，直至定位任務(wù)結(jié)束。

10、步驟s1中，所述的時(shí)間戳對齊，包括：以激光雷達(dá)傳感數(shù)據(jù)時(shí)間戳為基準(zhǔn)，將慣性單元imu、關(guān)節(jié)電機(jī)編碼器與激光雷達(dá)的時(shí)間戳對齊，完成時(shí)間同步，同步的數(shù)據(jù)計(jì)算方式：

11、

12、式中，data1和data2是對齊對象在時(shí)間線上的前后數(shù)據(jù)，datanew是與激光雷達(dá)數(shù)據(jù)幀采集時(shí)刻的同步數(shù)據(jù)，t1和t2是對齊對象在時(shí)間線上的時(shí)間戳，tnew是對齊后的時(shí)間戳；標(biāo)定imu內(nèi)部參數(shù)與imu與激光雷達(dá)的外部參數(shù)，外部參數(shù)包括旋轉(zhuǎn)參數(shù)和位移參數(shù)；所述imu內(nèi)部參數(shù)包括：imu數(shù)據(jù)中的陀螺儀數(shù)據(jù)、加速度數(shù)據(jù)均值及零偏。

13、所述機(jī)器人的慣性先驗(yàn)位姿估計(jì)由imu慣性單元數(shù)據(jù)在所述同步后的數(shù)據(jù)時(shí)間段內(nèi)通過數(shù)據(jù)時(shí)間積分推算得到；所述imu慣性單元數(shù)據(jù)由s1步驟進(jìn)行零漂修正得到。

14、步驟s3中，機(jī)器人運(yùn)動(dòng)學(xué)狀態(tài)估計(jì)，包括：

15、基于足式機(jī)器人的機(jī)械結(jié)構(gòu)與關(guān)節(jié)電機(jī)編碼器角度信息，得到機(jī)器人足端位置運(yùn)動(dòng)學(xué)方程jk(θ)，其中θ為所述關(guān)節(jié)電機(jī)編碼器的角度；基于機(jī)器人運(yùn)動(dòng)學(xué)雅可比矩陣j，得到機(jī)器人的足端速度方程其中為所述關(guān)節(jié)電機(jī)編碼器的角度對時(shí)間微分，即關(guān)節(jié)電機(jī)角速度；

16、根據(jù)步驟s2中所述的機(jī)器人慣性先驗(yàn)位姿及所述足端位置方程和足端速度方程，得到機(jī)器人空間位姿上的約束項(xiàng)及更新，約束項(xiàng)和更新項(xiàng)僅在足端處于與地面接觸時(shí)進(jìn)行；

17、空間約束方程如下：

18、

19、其中，δx為步驟s2中所述的機(jī)器人慣性先驗(yàn)位姿采樣計(jì)算結(jié)果，δt為采樣時(shí)間；

20、所述更新采用最小二乘法進(jìn)行更新，約束方程為上述空間約束方程，得到機(jī)器人基于運(yùn)動(dòng)學(xué)狀態(tài)估計(jì)x。

21、所述運(yùn)動(dòng)方程為足腿結(jié)構(gòu)的足端點(diǎn)正運(yùn)動(dòng)學(xué)方程，方程包含所述足腿類機(jī)器人n條腿部支撐點(diǎn)在其單個(gè)支撐作用周期內(nèi)相對于機(jī)身質(zhì)心的向量結(jié)果，運(yùn)動(dòng)學(xué)方程jk(θ)如下：

22、

23、其中，jk(θn)表示為該足腿類機(jī)器人第n條腿在其單個(gè)支撐作用周期內(nèi)相對于機(jī)身質(zhì)心的向量結(jié)果，θn表示為其第n條腿關(guān)節(jié)自由度的角度向量，該方程計(jì)算頻率與輪腿機(jī)器人的腿部運(yùn)動(dòng)周期一致；

24、其中單個(gè)支撐相根據(jù)關(guān)節(jié)對足腿所做的功率進(jìn)行線性擬合，計(jì)算和濾波得到其足端對地面施加的作用力，計(jì)算方式如下：

25、

26、其中，f為足端對地面施加的作用力，τi為機(jī)器人所計(jì)算的單條腿部第i個(gè)關(guān)節(jié)電機(jī)的力矩；根據(jù)足端對地面施加的作用力進(jìn)行閾值濾波，超過閾值設(shè)定區(qū)間判定為當(dāng)前腿處于支撐相狀態(tài)，閾值取值范圍為m為機(jī)器人機(jī)身質(zhì)量，k為機(jī)器人足腿數(shù)量，g為重力加速度。

27、步驟s4中，激光特征點(diǎn)殘差模型，包括：

28、所述特征點(diǎn)采用運(yùn)動(dòng)畸變補(bǔ)償數(shù)據(jù)經(jīng)曲率計(jì)算得到；

29、所述運(yùn)動(dòng)畸變補(bǔ)償數(shù)據(jù)經(jīng)慣性先驗(yàn)位姿狀態(tài)估計(jì)對激光雷達(dá)原始數(shù)據(jù)進(jìn)行逐點(diǎn)運(yùn)動(dòng)反向傳播得到；

30、所述激光殘差模型為特征點(diǎn)到特征面的距離殘差值；

31、所述距離殘差值公式如下：

32、

33、其中，a，b，c，d為特征面的平面方程參數(shù)，d為計(jì)算的距離殘差值；

34、所述特征面為全局地圖中，激光雷達(dá)數(shù)據(jù)點(diǎn)所在的平面。

35、步驟s5中，迭代優(yōu)化模型，包括：

36、所述迭代優(yōu)化模型的迭代優(yōu)化算法為eskf濾波優(yōu)化算法；

37、所述迭代優(yōu)化的觀測模型方程為：

38、h＝ut((r-p+t)-q)；

39、其中，ut為特征面的法向量，即權(quán)利要求5中所述a?b?c，p為所述的激光雷達(dá)檢測的數(shù)據(jù)特征點(diǎn)，q為所述全局地圖中特征面中距離p最近的特征面點(diǎn)，r，t為所迭代計(jì)算的定位狀態(tài)量；

40、所述迭代優(yōu)化模型的觀測更新量為權(quán)利要求5中所述的距離殘差值d；

41、步驟s5中，運(yùn)動(dòng)約束模型，包括：

42、所述運(yùn)動(dòng)約束模型方程為：

43、

44、tz＝λ1h+λ2l；

45、其中，δhl和δhleg為所述的迭代優(yōu)化模型和機(jī)器人運(yùn)動(dòng)學(xué)狀態(tài)估計(jì)計(jì)算得到的位移狀態(tài)中的高度增量；σ為權(quán)重增益，取值在0.8-1.2之間；λ1，λ2為約束權(quán)重比，取值在0.4-0.7之間；h，l分別為高度方向的狀態(tài)量和收益狀態(tài)增量；tz為約束后的高度狀態(tài)量。

46、步驟s6中，關(guān)鍵幀位姿圖優(yōu)化模型，包括：

47、根據(jù)所述迭代優(yōu)化模型和運(yùn)動(dòng)約束模型得到的狀態(tài)量進(jìn)行關(guān)鍵幀篩選，所述關(guān)鍵幀篩選以時(shí)間和空間進(jìn)行篩選，相鄰關(guān)鍵幀時(shí)間δt取值在3-10s之間，相鄰關(guān)鍵幀的歐式距離||δx||取值在5-10m之間；

48、根據(jù)關(guān)鍵幀的位置編碼，構(gòu)建基于位姿的因子模型，將關(guān)鍵幀的位姿信息加入全局因子圖優(yōu)化中作為定位約束；

49、根據(jù)當(dāng)前關(guān)鍵幀位姿與優(yōu)化后的關(guān)鍵幀位姿進(jìn)行實(shí)時(shí)回環(huán)約束，所述回環(huán)約束包括回環(huán)約束關(guān)鍵幀檢測和回環(huán)約束配準(zhǔn)；

50、所述回環(huán)約束關(guān)鍵幀檢測以當(dāng)前關(guān)鍵幀對所有歷史關(guān)鍵幀的空間歐式距離和時(shí)間進(jìn)行約束，對約束的空間歐式距離和時(shí)間滿足條件的視為當(dāng)前幀的回環(huán)約束關(guān)鍵幀，回環(huán)約束的關(guān)鍵幀時(shí)間δt取值大于30s，回環(huán)約束的關(guān)鍵幀的歐式距離||δx||取值小于2m；

51、所述的回環(huán)約束配準(zhǔn)采用幀-局部關(guān)鍵幀地圖的icp配準(zhǔn)方式得到約束結(jié)果，所述的局部關(guān)鍵幀為當(dāng)前關(guān)鍵幀對應(yīng)的回環(huán)約束關(guān)鍵幀前后n個(gè)關(guān)鍵幀構(gòu)建的局部地圖，n取值為15-20，所述約束結(jié)果為icp配準(zhǔn)得到的回環(huán)約束位姿轉(zhuǎn)換關(guān)系；

52、所述的回環(huán)約束，包括將對應(yīng)約束的兩個(gè)關(guān)鍵幀和所述的回環(huán)位姿轉(zhuǎn)換關(guān)系加入到全局因子圖優(yōu)化中。

53、本技術(shù)第二個(gè)方面提供一種電子設(shè)備，包括：至少一個(gè)處理器和存儲(chǔ)器；

54、所述存儲(chǔ)器存儲(chǔ)計(jì)算機(jī)執(zhí)行指令；

55、所述至少一個(gè)處理器執(zhí)行所述存儲(chǔ)器儲(chǔ)的計(jì)算機(jī)執(zhí)行命令，使得所述至少一個(gè)處理器執(zhí)行如上第一方面以及第一方面各種可能得設(shè)計(jì)所述的步驟方法。

56、本技術(shù)第三個(gè)方面提供一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中存儲(chǔ)有計(jì)算機(jī)執(zhí)行指令，當(dāng)處理器執(zhí)行所述計(jì)算機(jī)執(zhí)行指令時(shí)，實(shí)現(xiàn)如上第一方面以及第一方面各種可能得設(shè)計(jì)所述的步驟方法。

57、本發(fā)明與現(xiàn)有技術(shù)相比，具有以下的顯著優(yōu)點(diǎn)：

58、(1)本發(fā)明申請?zhí)峁┮环N基于足腿類機(jī)器人運(yùn)動(dòng)特征約束的定位建圖方法，在定位建圖過程中根據(jù)足式機(jī)器人的運(yùn)動(dòng)特性，結(jié)合足腿在環(huán)境中的運(yùn)動(dòng)約束與環(huán)境觀測約束，解決足腿類機(jī)器人在復(fù)雜環(huán)境運(yùn)動(dòng)中，由于高頻的足式運(yùn)動(dòng)所造成的場景定位魯棒性差、退化失真的問題；

59、(2)本發(fā)明申請通過采用濾波先驗(yàn)估計(jì)與因子優(yōu)化后端策略，相較于其他傳統(tǒng)算法，不僅能夠減少計(jì)算資源負(fù)荷實(shí)現(xiàn)實(shí)時(shí)定位刷新效率，還能夠優(yōu)化全局定位位姿，減少大規(guī)模復(fù)雜環(huán)境下的長程里程計(jì)帶來的累積漂移誤差；

60、(3)本發(fā)明申請采用關(guān)鍵運(yùn)動(dòng)特征約束和關(guān)鍵幀后端優(yōu)化，提高機(jī)器人在復(fù)雜環(huán)境中自身運(yùn)動(dòng)與環(huán)境觀測的交互關(guān)聯(lián)性，避免因冗余的特征匹配優(yōu)化帶來的估計(jì)不連續(xù)性，從根本上保證了位姿估計(jì)系統(tǒng)的幾何統(tǒng)一性。

61、通過以下參考附圖對本發(fā)明的示例性實(shí)施例的詳細(xì)描述，本發(fā)明的其它特征及其優(yōu)點(diǎn)將會(huì)變得清楚。