本發(fā)明涉及鋼筋綁扎機(jī)器人的定位與導(dǎo)航，具體的為一種基于bim多傳感器融合的自適應(yīng)加權(quán)定位方法及電子設(shè)備。

背景技術(shù)：

1、在建筑行業(yè)中，準(zhǔn)確的定位對(duì)于鋼筋綁扎機(jī)器人執(zhí)行任務(wù)至關(guān)重要；傳統(tǒng)的定位方法通常依賴于單一的傳感器，例如慣性測(cè)量單元(imu)、激光雷達(dá)等，然而隨著科技飛速發(fā)展，使用單個(gè)傳感器時(shí)的定位精度會(huì)容易受到環(huán)境干擾或自身局限性影響，從而導(dǎo)致定位誤差累積、信號(hào)遮擋以及環(huán)境變化導(dǎo)致的可靠性問(wèn)題；例如，激光雷達(dá)在遇到玻璃或反射面時(shí)容易出現(xiàn)測(cè)量誤差，視覺(jué)傳感器在低光照或復(fù)雜視覺(jué)環(huán)境下易失效，而輪速里程計(jì)在輪子打滑時(shí)會(huì)導(dǎo)致位置估算失準(zhǔn)；為了獲得更加精確、更穩(wěn)定的定位系統(tǒng)，現(xiàn)有技術(shù)提供了一種多傳感器融合定位方法，融合imu、激光雷達(dá)等傳感器獲得的位姿信息來(lái)確定機(jī)器人所處位置。

2、現(xiàn)有的多傳感器融合定位方法通常依賴預(yù)先構(gòu)建的地圖進(jìn)行定位，但在建筑室內(nèi)環(huán)境中，地圖的精度和更新往往不足以應(yīng)對(duì)復(fù)雜的建筑物結(jié)構(gòu)；隨著建筑信息建模(bim)的廣泛應(yīng)用，bim模型作為一種數(shù)字化的建筑信息表達(dá)方式，可以提供精確的建筑物幾何和拓?fù)湫畔?，為室?nèi)定位提供了一種新的數(shù)據(jù)源；如果將bim與多傳感器融合定位方法結(jié)合起來(lái)，則可以彌補(bǔ)傳統(tǒng)的多傳感器融合定位方法在建筑室內(nèi)環(huán)境中的精度與更新不足問(wèn)題。

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

1、本發(fā)明的目的在于提供一種能夠有效提高鋼筋綁扎機(jī)器人在動(dòng)態(tài)施工環(huán)境中定位精度與穩(wěn)定性的基于bim多傳感器融合的自適應(yīng)加權(quán)定位方法及電子設(shè)備。

2、基于上述目的，本發(fā)明采用如下技術(shù)方案：

3、一種基于bim多傳感器融合的自適應(yīng)加權(quán)定位方法，包括以下步驟：

4、s1、獲取包含建筑框架結(jié)構(gòu)與鋼筋布置信息的二維柵格地圖；

5、s2、采集數(shù)據(jù)：采集慣性測(cè)量單元imu、激光雷達(dá)、視覺(jué)傳感器以及輪速里程計(jì)的實(shí)時(shí)數(shù)據(jù)，并同步對(duì)齊時(shí)間戳；

6、s3、激光數(shù)據(jù)處理：根據(jù)步驟s2中采集得到的數(shù)據(jù)信息，進(jìn)行坐標(biāo)變換；利用imu與輪速里程計(jì)計(jì)算初步位姿變化，并對(duì)激光雷達(dá)的點(diǎn)云數(shù)據(jù)進(jìn)行畸變校正與剔除噪聲；

7、s4、激光點(diǎn)云篩選與傳輸：判斷機(jī)器人當(dāng)前處于縱向還是橫向移動(dòng)模式，根據(jù)不同的運(yùn)動(dòng)類型選擇特定角度范圍內(nèi)的激光雷達(dá)點(diǎn)云數(shù)據(jù)，剔除無(wú)效或冗余點(diǎn)云信息，并將點(diǎn)云數(shù)據(jù)實(shí)時(shí)傳輸至系統(tǒng)，用于在cartographer算法中進(jìn)行環(huán)境匹配；

8、s5、生成誤差項(xiàng)：評(píng)估各傳感器實(shí)時(shí)數(shù)據(jù)的質(zhì)量，生成各自的誤差項(xiàng)；

9、s6、計(jì)算傳感器權(quán)重：對(duì)誤差項(xiàng)進(jìn)行正則化處理，計(jì)算各傳感器數(shù)據(jù)的初始權(quán)重；

10、s7、平滑權(quán)重變化：根據(jù)指數(shù)加權(quán)移動(dòng)平均方法平滑權(quán)重變化；

11、s8、初步定位：根據(jù)b適應(yīng)權(quán)重將各傳感器的數(shù)據(jù)進(jìn)行加權(quán)融合，生成初步定位結(jié)果；

12、s9、優(yōu)化定位結(jié)果：利用擴(kuò)展卡爾曼濾波對(duì)初步定位結(jié)果進(jìn)行優(yōu)化，得到最終定位結(jié)果。

13、優(yōu)選地，步驟s3激光數(shù)據(jù)處理的具體過(guò)程包括：

14、通過(guò)坐標(biāo)變換將將點(diǎn)云數(shù)據(jù)從激光雷達(dá)坐標(biāo)系轉(zhuǎn)換到全局坐標(biāo)系，確保與二維柵格地圖坐標(biāo)一致；

15、通過(guò)體素網(wǎng)格濾波減少點(diǎn)云數(shù)據(jù)量，將點(diǎn)云數(shù)據(jù)劃分為固定大小的體素，每個(gè)體素用體素內(nèi)所有點(diǎn)的重心替代，從而減少點(diǎn)云數(shù)據(jù)量；

16、通過(guò)統(tǒng)計(jì)濾波去除離群點(diǎn)，計(jì)算每個(gè)點(diǎn)的鄰域點(diǎn)個(gè)數(shù)，移除鄰域點(diǎn)個(gè)數(shù)小于設(shè)定閾值的點(diǎn)；

17、通過(guò)半徑濾波去除噪聲點(diǎn)，基于半徑范圍內(nèi)的鄰域點(diǎn)數(shù)量去除噪聲點(diǎn)；

18、進(jìn)行高度過(guò)濾，過(guò)濾掉高度不在合理范圍內(nèi)的點(diǎn)，如地面點(diǎn)或高度異常的點(diǎn)；

19、進(jìn)行條件濾波去除不符合條件的點(diǎn)。

20、imu提供角速度和加速度信息，通過(guò)積分得到姿態(tài)變化：

21、

22、其中，θ(t)為時(shí)間t的姿態(tài)，ω(t)為角速度；

23、輪速里程計(jì)提供機(jī)器人在平面上的位置變化，利用位姿變化信息，計(jì)算機(jī)器人相對(duì)于初始位置的位姿(平移和旋轉(zhuǎn))變化：

24、δx＝υx·δt，δy＝υy·δt

25、其中，δx和δy分別是x和y方向上的位移，υx和υy是機(jī)器人的速度分量；

26、將估計(jì)得到的初始位姿信息應(yīng)用于激光點(diǎn)云畸變矯正：

27、pcorr＝r(θ)·praw+t

28、其中，pcorr為校正后的點(diǎn)云，praw為原始點(diǎn)云，r(θ)為旋轉(zhuǎn)矩陣，t為位姿平移量。

29、優(yōu)選地，步驟s4激光點(diǎn)云篩選與傳輸?shù)木唧w過(guò)程包括：

30、根據(jù)機(jī)器人的當(dāng)前運(yùn)動(dòng)類型動(dòng)態(tài)調(diào)整激光雷達(dá)的掃描角度，確保采集到的點(diǎn)云數(shù)據(jù)覆蓋機(jī)器人前進(jìn)路徑的關(guān)鍵區(qū)域；

31、訂閱激光雷達(dá)數(shù)據(jù)并將其轉(zhuǎn)換為笛卡爾坐標(biāo)系數(shù)據(jù)；

32、訂閱里程計(jì)數(shù)據(jù)，根據(jù)一段時(shí)間內(nèi)的位置變化計(jì)算運(yùn)動(dòng)方向。如果機(jī)器人主要沿著x軸移動(dòng)，則認(rèn)為其縱向移動(dòng)；如果主要沿著y軸移動(dòng)，則認(rèn)為其橫向移動(dòng)；

33、縱向移動(dòng)時(shí)，重點(diǎn)選擇激光雷達(dá)前后90度范圍內(nèi)(45度到135度和225度到315度)的點(diǎn)云數(shù)據(jù)點(diǎn)。橫向移動(dòng)時(shí)，重點(diǎn)選擇激光雷達(dá)左右90度范圍內(nèi)(315度到45度和135度到225度)的點(diǎn)云數(shù)據(jù)點(diǎn)。經(jīng)預(yù)處理和校正后的點(diǎn)云數(shù)據(jù)輸入至cartographer算法，與靜態(tài)二維柵格地圖進(jìn)行匹配，計(jì)算機(jī)器人在全局坐標(biāo)系中的位姿；

34、優(yōu)選地，步驟s5生成誤差項(xiàng)的具體過(guò)程包括：

35、評(píng)估imu陀螺儀輸出的噪聲水平以及測(cè)量imu加速度計(jì)在不同運(yùn)動(dòng)狀態(tài)下的線性度、零偏和尺度因子誤差，生成誤差項(xiàng)；

36、通過(guò)分析激光雷達(dá)獲取的點(diǎn)云數(shù)據(jù)的密度和一致性來(lái)評(píng)估其定位精度，統(tǒng)計(jì)分析激光雷達(dá)點(diǎn)云中的噪聲水平估計(jì)其測(cè)量誤差，生成誤差項(xiàng)；

37、基于圖像質(zhì)量的多個(gè)指標(biāo)生成視覺(jué)傳感器誤差項(xiàng)；

38、通過(guò)比較預(yù)期移動(dòng)距離和實(shí)際移動(dòng)距離的差異來(lái)生成輪速里程計(jì)誤差項(xiàng)，若檢測(cè)到打滑，則誤差項(xiàng)取一個(gè)極大的值，比較輪速里程計(jì)計(jì)算的速度與imu提供的速度，如果差異超過(guò)預(yù)設(shè)閾值，則認(rèn)為發(fā)生打滑。

39、優(yōu)選地，步驟s6計(jì)算傳感器權(quán)重的具體過(guò)程包括：

40、對(duì)誤差項(xiàng)進(jìn)行正則化處理：為了使誤差項(xiàng)在同一個(gè)尺度上進(jìn)行比較，將各個(gè)誤差項(xiàng)進(jìn)行歸一化處理：

41、

42、其中，max(e)和min(e)分別為所有傳感器誤差項(xiàng)中的最小值和最大值，表示正則化后的誤差項(xiàng)；記正則化后的誤差項(xiàng)分別為

43、優(yōu)選地，根據(jù)正則化后的誤差項(xiàng)計(jì)算初始權(quán)重：

44、

45、其中，wi表示各誤差項(xiàng)的初始權(quán)重，表示各個(gè)正則化后的誤差項(xiàng)之和；各個(gè)誤差項(xiàng)的初始權(quán)重分別記為wimu、wlidar、wvision和wodometry。

46、優(yōu)選地，步驟s7平滑權(quán)重變化的具體過(guò)程包括：

47、使用指數(shù)加權(quán)移動(dòng)平均(ewma)方法平滑權(quán)重變化，以減少因傳感器誤差波動(dòng)導(dǎo)致的權(quán)重劇烈變化：

48、wi(t+1)＝αwi(t)+(1-α)wi(t-1)

49、其中，α為平滑系數(shù)，取值范圍為0-1。

50、優(yōu)選地，步驟s8初步定位的具體過(guò)程包括：

51、根據(jù)自適應(yīng)權(quán)重將各個(gè)傳感器的數(shù)據(jù)進(jìn)行加權(quán)融合，生成初步定位結(jié)果：

52、

53、其中，為多傳感器數(shù)據(jù)融合后的初始位姿估計(jì)，pi為第i個(gè)傳感器的數(shù)據(jù)。

54、優(yōu)選地，步驟s9優(yōu)化定位結(jié)果的具體過(guò)程包括：

55、定義柵格單元：?jiǎn)卧院撩准?jí)分辨率表示鋼筋間距和空間位置；

56、障礙物標(biāo)記：將柱、梁等視為障礙物；

57、鋼筋可通行區(qū)域：標(biāo)記綁扎作業(yè)路徑區(qū)域；

58、所述二維柵格地圖可表示為：

59、

60、s91、對(duì)狀態(tài)向量、協(xié)方差矩陣、噪聲協(xié)方差進(jìn)行初始化：

61、狀態(tài)向量：設(shè)定初始狀態(tài)向量x0，包含初步定位結(jié)果，如位置和速度等信息；

62、x0＝[x，y，θ，υ，ω]t

63、其中，x，y為機(jī)器人位置坐標(biāo)，θ為姿態(tài)角，υ為線速度，ω為角速度；初始化時(shí)取自初步定位結(jié)果；

64、協(xié)方差矩陣：設(shè)定初始協(xié)方差矩陣p0表示初始狀態(tài)的不確定性，各參數(shù)的方差可根據(jù)系統(tǒng)特性估計(jì)：

65、

66、根據(jù)噪聲類型分別設(shè)定過(guò)程噪聲協(xié)方差矩陣和測(cè)量噪聲協(xié)方差矩陣：

67、過(guò)程噪聲協(xié)方差矩陣q：考慮機(jī)器人運(yùn)動(dòng)中的噪聲特性，例如輪速里程計(jì)和imu的測(cè)量誤差；

68、測(cè)量噪聲協(xié)方差矩陣r：反映激光雷達(dá)點(diǎn)云、視覺(jué)傳感器等的測(cè)量精度與噪聲水平；

69、s92、進(jìn)行狀態(tài)預(yù)測(cè)和協(xié)方差預(yù)測(cè)：

70、狀態(tài)預(yù)測(cè)：利用系統(tǒng)的狀態(tài)轉(zhuǎn)移模型f，預(yù)測(cè)下一時(shí)刻的狀態(tài)：

71、

72、其中，是時(shí)刻k的預(yù)測(cè)狀態(tài)，uk-1是時(shí)刻k-1控制輸入線速度，ωk-1是時(shí)刻k-1控制輸入角速度，θk-1是上一狀態(tài)的航向角(相對(duì)于x軸)，xk-1和yk-1為上一次狀態(tài)在平面中的位置，cos(θk-1)和sin(θk-1)分別是航向角的x方向分量和y方向分量；

73、協(xié)方差預(yù)測(cè)：利用雅可比矩陣f對(duì)協(xié)方差矩陣進(jìn)行更新：

74、

75、其中，是狀態(tài)轉(zhuǎn)移模型的雅可比矩陣，q為過(guò)程噪聲的協(xié)方差矩陣；

76、s93、更新階段：

77、計(jì)算卡爾曼增益：利用預(yù)測(cè)協(xié)方差矩陣和測(cè)量模型的雅可比矩陣h，計(jì)算卡爾曼增益kk：

78、kk＝pk|k-1htk(hkpk|k-1htk+r)-1

79、其中，是測(cè)量模型的雅可比矩陣，r為觀測(cè)噪聲的協(xié)方差矩陣；

80、狀態(tài)更新：利用卡爾曼增益和實(shí)際測(cè)量值z(mì)k更新?tīng)顟B(tài)估計(jì)：

81、

82、其中，h是測(cè)量模型函數(shù)，為狀態(tài)向量；

83、更新協(xié)方差矩陣：

84、pk＝(i-kkhk)pk|k-1

85、s94、迭代并輸出最終定位結(jié)果：重復(fù)步驟s92和步驟s93，隨著時(shí)間步長(zhǎng)k的增加，結(jié)合實(shí)時(shí)傳感器數(shù)據(jù)進(jìn)行定位更新，遞歸執(zhí)行預(yù)測(cè)和更新階段；經(jīng)過(guò)若干次迭代，最終的狀態(tài)向量和協(xié)方差矩陣pk就是擴(kuò)展卡爾曼濾波優(yōu)化后的定位結(jié)果。

86、一種電子設(shè)備，包括存儲(chǔ)器及處理器，存儲(chǔ)器上存儲(chǔ)有計(jì)算機(jī)程序，處理器執(zhí)行計(jì)算機(jī)程序時(shí)實(shí)現(xiàn)如上述任一的基于bim多傳感器融合的自適應(yīng)加權(quán)定位方法中的任一步驟。

87、本發(fā)明的有益效果包括：

88、本發(fā)明通過(guò)有效利用bim模型生成的二維柵格地圖，提供了高精度的地圖信息，減少了傳統(tǒng)地圖構(gòu)建與更新過(guò)程中的復(fù)雜性，從而簡(jiǎn)化了實(shí)施流程，有效提高了系統(tǒng)整體的易用性與實(shí)用性。

89、本發(fā)明將傳統(tǒng)的多種傳感器，包括激光雷達(dá)、imu、視覺(jué)傳感器和輪速里程計(jì)等的數(shù)據(jù)融合，顯著提高了定位精度；相較于傳統(tǒng)的單一傳感器定位方法，本發(fā)明將多鐘傳感器的數(shù)據(jù)融合之后，定位過(guò)程更加穩(wěn)定可靠，有效減少了定位誤差。

90、本發(fā)明在復(fù)雜的室內(nèi)環(huán)境中，尤其是在光照不足或視覺(jué)特征不明顯的情況下，依然能夠保持精準(zhǔn)定位；通過(guò)動(dòng)態(tài)調(diào)整各傳感器的數(shù)據(jù)權(quán)重，即使在激光雷達(dá)或視覺(jué)傳感器失效的黑暗環(huán)境中，依然可以通過(guò)imu和輪速里程計(jì)實(shí)現(xiàn)對(duì)鋼筋綁扎機(jī)器人的有效定位，從而增強(qiáng)了系統(tǒng)在動(dòng)態(tài)環(huán)境中的適應(yīng)性，增強(qiáng)了系統(tǒng)的魯棒性，極大拓展了本發(fā)明的使用范圍，能夠有效應(yīng)對(duì)環(huán)境變化帶來(lái)的挑戰(zhàn)。