本發(fā)明涉及室內(nèi)定位，尤其涉及用于植物工廠的實(shí)時(shí)激光超聲慣導(dǎo)緊耦合高精度定位方法，用于農(nóng)業(yè)機(jī)器人在植物工廠內(nèi)實(shí)現(xiàn)10cm量級(jí)的全局定位。

背景技術(shù)：

1、隨著全球農(nóng)業(yè)生產(chǎn)的不斷發(fā)展，植物工廠在現(xiàn)代農(nóng)業(yè)中扮演著越來越重要的角色。植物工廠提供了一種受控的環(huán)境，可以優(yōu)化作物的生長(zhǎng)條件，提高產(chǎn)量和質(zhì)量，并減少對(duì)自然資源的依賴。目前，農(nóng)業(yè)機(jī)器人在農(nóng)業(yè)生產(chǎn)中得到了廣泛應(yīng)用，如種植、澆水、施肥和采摘等任務(wù)。為了實(shí)現(xiàn)自主化的農(nóng)業(yè)生產(chǎn)，農(nóng)業(yè)機(jī)器人需要準(zhǔn)確地感知和理解大棚內(nèi)的環(huán)境，并能夠在復(fù)雜的場(chǎng)景中進(jìn)行高精度的定位和導(dǎo)航。

2、在室內(nèi)植物工廠中，傳統(tǒng)的全球定位系統(tǒng)(gps)由于信號(hào)受屏蔽，無法提供足夠的定位精度，因此，精確的全局定位與就位對(duì)移動(dòng)機(jī)器人而言是一項(xiàng)挑戰(zhàn)，需要精準(zhǔn)的控制操作才能實(shí)現(xiàn)。而傳統(tǒng)的視覺導(dǎo)航方法受制于光照條件和環(huán)境特性，可能在遇到復(fù)雜的大棚結(jié)構(gòu)時(shí)定位效果不佳。

3、經(jīng)過對(duì)現(xiàn)有技術(shù)的檢索發(fā)現(xiàn)，中國專利文獻(xiàn)號(hào)cn113538410b，公開日2022.05.20，公開了一種基于3d激光雷達(dá)和uwb的室內(nèi)slam建圖與定位方法，為同樣領(lǐng)域內(nèi)實(shí)現(xiàn)移動(dòng)機(jī)器人全局定位的方法。該方法在室內(nèi)場(chǎng)景中部署uwb定位系統(tǒng)，并通過攜帶3d激光雷達(dá)傳感器的機(jī)器人探索室內(nèi)場(chǎng)景區(qū)域，利用融合激光雷達(dá)數(shù)據(jù)和uwb數(shù)據(jù)的slam算法生成所探索區(qū)域的地圖。但是，該技術(shù)不僅依賴uwb系統(tǒng)提供的初值信息，同時(shí)還未對(duì)uwb系統(tǒng)提供的位置信息在環(huán)境中的穩(wěn)定性進(jìn)行檢驗(yàn)，其定位精度相對(duì)受限，無法滿足農(nóng)業(yè)機(jī)器人在植物工廠中高精度定位作業(yè)的需求；

4、中國專利文獻(xiàn)號(hào)cn118298122a，公開日2024.07.05，公開了一種基于激光雷達(dá)-慣導(dǎo)的ndt-icp緊耦合的地圖構(gòu)建方法，包括對(duì)激光雷達(dá)采集的原始點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理；基于慣性測(cè)量單元采集的高頻信號(hào)，進(jìn)行imu預(yù)積分，得到激光雷達(dá)幀的相對(duì)位姿數(shù)據(jù)，進(jìn)而消除預(yù)處理后的點(diǎn)云數(shù)據(jù)的點(diǎn)云畸變，得到畸變校正后的點(diǎn)云數(shù)據(jù)；從畸變校正后的點(diǎn)云數(shù)據(jù)中提取邊緣特征與平面特征；基于ndt?icp配準(zhǔn)算法實(shí)現(xiàn)點(diǎn)云的精確配準(zhǔn)和位姿估計(jì)；由預(yù)處理后的點(diǎn)云數(shù)據(jù)確定閉環(huán)因子；構(gòu)建局部地圖，并將激光雷達(dá)位姿因子、地面約束因子以及閉環(huán)因子添加至因子圖中，更新局部地圖中所有關(guān)鍵幀的位姿，得到高精地圖；但該方法僅依賴激光雷達(dá)與慣性傳感器，缺少全局傳感器信息，在面對(duì)重復(fù)度較高的環(huán)境以及幾何特征缺失、遮擋的環(huán)境易產(chǎn)生滑移，導(dǎo)致地圖信息的明顯錯(cuò)誤

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

1、本發(fā)明主要在于提供一種用于植物工廠的實(shí)時(shí)激光超聲慣導(dǎo)緊耦合高精度定位方法，其能夠按照自主建立的評(píng)價(jià)標(biāo)準(zhǔn)賦以相應(yīng)權(quán)重并進(jìn)行綜合，最終完成建圖功能，并實(shí)現(xiàn)高精度的定位。

2、本發(fā)明提供一種用于植物工廠的實(shí)時(shí)激光超聲慣導(dǎo)緊耦合高精度定位方法，用于農(nóng)業(yè)機(jī)器人在單一層植物工廠內(nèi)實(shí)現(xiàn)10cm量級(jí)的全局定位，包括以下步驟：

3、調(diào)用imu里程計(jì)模塊、激光里程計(jì)模塊、回環(huán)檢測(cè)模塊和聲學(xué)信標(biāo)定位模塊，在slam過程中實(shí)時(shí)完成建圖與定位，同時(shí)使用因子圖融合以上各個(gè)模塊的信息，獲取得到優(yōu)化后的高精度定位；

4、其中所述imu里程計(jì)模塊用于獲取在任意關(guān)鍵幀時(shí)agv小車通過imu測(cè)定的運(yùn)動(dòng)狀態(tài)，其中所述激光里程計(jì)模塊用于實(shí)時(shí)獲取植物工廠內(nèi)環(huán)境的激光雷達(dá)掃描點(diǎn)云，其中所述回環(huán)檢測(cè)模塊用于消除回環(huán)過程中的累計(jì)漂移誤差，其中所述聲學(xué)信標(biāo)定位模塊用于通過部署在植物工廠中各個(gè)位置的信標(biāo)實(shí)時(shí)測(cè)定agv小車到各信標(biāo)的距離，并據(jù)此計(jì)算出agv小車在植物工廠中的位置坐標(biāo)，其中所述位置坐標(biāo)屬聲學(xué)信標(biāo)坐標(biāo)系下。

5、根據(jù)本發(fā)明一實(shí)施例，所述植物工廠的地面為水平面，所述植物工廠的每個(gè)角落均部署有聲學(xué)信標(biāo)定位基站，或在所述植物工廠的外邊沿均勻部署有多個(gè)聲學(xué)信標(biāo)定位基站。

6、根據(jù)本發(fā)明一實(shí)施例，所述imu里程計(jì)模塊的物理結(jié)構(gòu)為imu傳感器，imu傳感器被綁定在所述agv小車上，以獲取所述agv小車的三維速度、角速度和加速度數(shù)據(jù)，所述imu里程計(jì)模塊對(duì)傳感器的運(yùn)動(dòng)數(shù)據(jù)依次進(jìn)行如下處理步驟：

7、s11，獲取測(cè)量的角速度與加速度數(shù)據(jù)，并消除誤差組分：

8、ωmt＝ωt+bωt+nωt

9、amt＝rbw(at-g)+bat+nat

10、其中，賦予下標(biāo)t的變量為t時(shí)刻的相關(guān)參數(shù)，賦予下標(biāo)m的變量為傳感器數(shù)據(jù)，參數(shù)b為傳感器測(cè)量偏差，參數(shù)n為環(huán)境白噪音帶來的測(cè)量誤差，g為環(huán)境中的重力常數(shù)，rbw是從imu坐標(biāo)系(即agv小車坐標(biāo)系)到世界坐標(biāo)系下的旋轉(zhuǎn)矩陣；消除相應(yīng)誤差組分后，得到可使用的imu角速度與加速度數(shù)據(jù)；

11、s12，積分，獲取下一時(shí)刻的小車運(yùn)動(dòng)參數(shù)：

12、vt+δt＝vt+gδt+rt(amt-bat-nat)δt

13、

14、

15、經(jīng)過以上計(jì)算獲取任意時(shí)刻agv小車的位置、速度與旋轉(zhuǎn)矩陣，并獲得從ti到tj時(shí)刻，agv小車的速度、位置、旋轉(zhuǎn)矩陣的變化量：

16、

17、根據(jù)本發(fā)明一實(shí)施例，所述激光里程計(jì)模塊的物理結(jié)構(gòu)為激光雷達(dá)，用于采集植物工廠內(nèi)環(huán)境的激光點(diǎn)云，其對(duì)傳感器的點(diǎn)云數(shù)據(jù)依次進(jìn)行以下處理步驟：

18、s21，按照預(yù)定規(guī)律將采樣數(shù)據(jù)標(biāo)記為關(guān)鍵幀；

19、s22，對(duì)傳感器獲取的點(diǎn)云數(shù)據(jù)進(jìn)行降采樣處理；

20、s23，提取直線與平面特征，在給定的歐幾里得空間中，將滿足要求的點(diǎn)云分組為若干集合；

21、s24，在相鄰關(guān)鍵幀之間，匹配對(duì)應(yīng)的直線與平面特征，其具體實(shí)現(xiàn)方法如下：

22、定義處于激光里程計(jì)的坐標(biāo)系下并已完成匹配的最新關(guān)鍵幀為tk，定義處于激光雷達(dá)的坐標(biāo)系下并未完成匹配的關(guān)鍵幀為tk+1，求取兩關(guān)鍵幀之間的位姿變換，優(yōu)化函數(shù)為：

23、

24、獲取tk+1時(shí)刻由imu測(cè)得的激光雷達(dá)到激光里程計(jì)的坐標(biāo)變換矩陣rt+1與位置pt+1，并對(duì)tk+1時(shí)刻的雷達(dá)點(diǎn)云進(jìn)行上述變換，使其從激光雷達(dá)坐標(biāo)系變換至里程計(jì)坐標(biāo)系下；

25、對(duì)完成變換的tk+1中的點(diǎn)云，記為{pi}，對(duì)任意的pi，在完成變換的tk中的點(diǎn)云所處的歐氏空間中尋找到與pi相鄰近的點(diǎn)pj，pl，pm，則：

26、

27、調(diào)整rt+1與pk+1，使取得極小值，迭代計(jì)算的結(jié)果即為經(jīng)過激光點(diǎn)云特征匹配后的tk+1時(shí)刻的激光點(diǎn)云數(shù)據(jù)到激光里程計(jì)的坐標(biāo)變換矩陣r′t+1與位置p′t+1；

28、將完成變換與匹配的關(guān)鍵幀點(diǎn)云信息進(jìn)行融合，即可獲得實(shí)時(shí)更新的點(diǎn)云地圖與從初始位姿開始的激光里程計(jì)。

29、根據(jù)本發(fā)明一實(shí)施例，步驟s21具體為按照預(yù)定的時(shí)間間隔將當(dāng)前幀的點(diǎn)云數(shù)據(jù)標(biāo)記為關(guān)鍵幀。

30、根據(jù)本發(fā)明一實(shí)施例，步驟s23具體為：將預(yù)定范圍內(nèi)的鄰近點(diǎn)中滿足同一棱線特征和滿足同一平面特征的點(diǎn)集標(biāo)記分組，劃分為若干個(gè)集合。

31、根據(jù)本發(fā)明一實(shí)施例，所述回環(huán)檢測(cè)模塊基于歐幾里得直線距離實(shí)現(xiàn)回環(huán)檢測(cè)，具體為：

32、s31，將每個(gè)新標(biāo)記的關(guān)鍵幀經(jīng)過計(jì)算imu與激光雷達(dá)獲得的位置pi與歷史關(guān)鍵點(diǎn)逐個(gè)比較，在歷史關(guān)鍵幀中獲取到與其最為接近的關(guān)鍵幀kj；

33、s32，提取關(guān)鍵幀kj，以及在構(gòu)建順序上以kj為中心的共2m+1個(gè)關(guān)鍵幀{kj-m，...，kj，...，kj+m}，其中m為人為確定的參數(shù)；

34、s33，將新標(biāo)記的關(guān)鍵幀依次與以上一系列關(guān)鍵幀進(jìn)行特征匹配，獲取關(guān)鍵幀ki從激光雷達(dá)坐標(biāo)系到激光里程計(jì)坐標(biāo)系(地圖坐標(biāo)系)的變換矩陣。

35、根據(jù)本發(fā)明一實(shí)施例，所述聲學(xué)信標(biāo)定位模塊的物理結(jié)構(gòu)為固定在agv小車上的超聲波收發(fā)裝置以及固定在植物工廠中的聲學(xué)信標(biāo)定位基站，具體方法包括：

36、s41，控制所述超聲波收發(fā)裝置以預(yù)定頻率向周圍發(fā)射超聲波，同時(shí)所述植物工廠內(nèi)的聲學(xué)信標(biāo)定位基站在接收到超聲波后在同一時(shí)刻產(chǎn)生特定頻率的超聲波，供所述超聲波收發(fā)裝置接收；

37、agv小車與聲學(xué)信標(biāo)定位基站之間的距離通過以下公式計(jì)算：

38、

39、其中，t為超聲波收發(fā)裝置從發(fā)射超聲波到接收到超聲波之間的時(shí)間間隔，v為空氣聲速，約為340m/s；

40、獲取agv小車任意時(shí)刻到每個(gè)聲學(xué)信標(biāo)定位基站的距離，通過幾何計(jì)算求出agv小車在環(huán)境中的位置；

41、s42，針對(duì)聲學(xué)定位結(jié)果建立相關(guān)可靠性判據(jù)：

42、

43、其中，fu為超聲波的聲音頻率，為t時(shí)刻agv小車的位置，vmax為人為約定的agv小車最大車速，β為人為約定的中間評(píng)價(jià)指標(biāo)，為agv小車靜止在不同位置時(shí)測(cè)得的聲學(xué)信標(biāo)信號(hào)質(zhì)量函數(shù)，是信號(hào)噪聲隨位置分布的分布函數(shù)的函數(shù)：

44、

45、技術(shù)效果：與現(xiàn)有技術(shù)相比，本發(fā)明提供的用于植物工廠的實(shí)時(shí)激光超聲慣導(dǎo)緊耦合高精度定位方法通過綜合imu里程計(jì)模塊、激光里程計(jì)模塊、回環(huán)檢測(cè)模塊和聲學(xué)信標(biāo)定位模塊的處理后信息，利用因子圖結(jié)構(gòu)賦以相應(yīng)權(quán)重，可以獲取到精確的全局位置姿態(tài)，在給定的具有一定重復(fù)度的室內(nèi)實(shí)驗(yàn)環(huán)境中可以達(dá)到5.4cm的極佳的定位精度，能夠充分滿足高精度實(shí)時(shí)全局定位的要求，具有優(yōu)越的魯棒性與實(shí)時(shí)性能，且有望能夠進(jìn)一步提升大面積的植物工廠內(nèi)移動(dòng)機(jī)器人的定位與導(dǎo)航能力。

46、本發(fā)明的這些和其它目的、特點(diǎn)和優(yōu)勢(shì)，通過下述的詳細(xì)說明，得以充分體現(xiàn)。