本發(fā)明涉及農(nóng)業(yè)設(shè)備，具體涉及一種多傳感器融合的溫室移動(dòng)底盤(pán)導(dǎo)航控制系統(tǒng)。

背景技術(shù)：

1、室溫移動(dòng)底盤(pán)導(dǎo)航控制系統(tǒng)是指在常溫環(huán)境下，應(yīng)用于機(jī)器人或其他移動(dòng)平臺(tái)的導(dǎo)航控制系統(tǒng)。該系統(tǒng)通過(guò)集成多種傳感器和先進(jìn)的算法，確保移動(dòng)平臺(tái)能夠在室內(nèi)或類(lèi)似環(huán)境中準(zhǔn)確定位、自主導(dǎo)航并避免障礙物。

2、多傳感器融合是指將來(lái)自多個(gè)不同類(lèi)型傳感器的數(shù)據(jù)進(jìn)行綜合處理，以獲得更準(zhǔn)確、可靠和全面的信息。這種方法利用各傳感器的優(yōu)勢(shì)，彌補(bǔ)單一傳感器的不足，從而提高系統(tǒng)的整體性能和魯棒性。

3、現(xiàn)有技術(shù)存在以下不足：現(xiàn)有的導(dǎo)航控制系統(tǒng)中數(shù)據(jù)采集是通過(guò)攝像頭采集周?chē)鷪D片數(shù)據(jù)，并圖片數(shù)據(jù)進(jìn)行特征提取，使用同步定位與地圖構(gòu)建(slam)算法，實(shí)時(shí)構(gòu)建環(huán)境地圖并定位移動(dòng)底盤(pán)，并基于構(gòu)建的環(huán)境地圖和當(dāng)前位置，使用路徑規(guī)劃算法生成從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑，但是這種數(shù)據(jù)采集方法回導(dǎo)致當(dāng)相機(jī)的深度數(shù)據(jù)不穩(wěn)定時(shí)造成的數(shù)據(jù)采集不準(zhǔn)確的問(wèn)題。

4、在所述背景技術(shù)部分公開(kāi)的上述信息僅用于加強(qiáng)對(duì)本公開(kāi)的背景的理解，因此它可以包括不構(gòu)成對(duì)本領(lǐng)域普通技術(shù)人員已知的現(xiàn)有技術(shù)的信息。

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

1、本發(fā)明的目的是提供一種多傳感器融合的溫室移動(dòng)底盤(pán)導(dǎo)航控制系統(tǒng)，以解決上述背景技術(shù)中的問(wèn)題。

2、為了實(shí)現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：

3、一種多傳感器融合的溫室移動(dòng)底盤(pán)導(dǎo)航控制系統(tǒng)，包括數(shù)據(jù)融合模塊、環(huán)境地圖構(gòu)建模塊、路徑規(guī)劃模塊以及導(dǎo)航與避障模塊，模塊之間通過(guò)信號(hào)連接，各模塊之間數(shù)據(jù)化處理步驟如下：

4、數(shù)據(jù)融合模塊，用于通過(guò)數(shù)據(jù)轉(zhuǎn)換將偽激光數(shù)據(jù)與激光雷達(dá)數(shù)據(jù)融合，并將融合后的數(shù)據(jù)傳輸至環(huán)境地圖構(gòu)建模塊；

5、環(huán)境地圖構(gòu)建模塊，用于接收數(shù)據(jù)融合模塊傳輸?shù)娜诤虾蟮臄?shù)據(jù)，并根據(jù)融合后的數(shù)據(jù)構(gòu)建環(huán)境地圖，并將環(huán)境地圖傳輸至路徑規(guī)劃模塊；

6、路徑規(guī)劃模塊，用于接收環(huán)境地圖構(gòu)建模塊傳輸?shù)沫h(huán)境地圖，并根據(jù)目的地進(jìn)行路徑規(guī)劃，并將路徑規(guī)劃信息傳輸至導(dǎo)航與避障模塊；

7、導(dǎo)航與避障模塊，用于接收路徑規(guī)劃模塊傳輸?shù)穆窂揭?guī)劃信息，并進(jìn)行導(dǎo)航與避障。

8、優(yōu)選的，所述通過(guò)數(shù)據(jù)轉(zhuǎn)換將偽激光數(shù)據(jù)與激光雷達(dá)數(shù)據(jù)融合步驟為：

9、使用rgb-d攝像頭獲取深度圖像，將深度圖像轉(zhuǎn)換為偽激光數(shù)據(jù)，通過(guò)投影將深度值轉(zhuǎn)換為與激光雷達(dá)相似的二維點(diǎn)云，并直接從激光雷達(dá)獲取二維點(diǎn)云數(shù)據(jù)；

10、使用時(shí)間戳同步算法，確保偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)在同一時(shí)間戳上對(duì)齊，對(duì)偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)進(jìn)行濾波，去除噪聲以及異常點(diǎn)處理；

11、對(duì)偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)進(jìn)行傳感器間的外部標(biāo)定，獲取坐標(biāo)變換矩陣，確保偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)在同一坐標(biāo)系下；

12、使用擴(kuò)展卡爾曼濾波器融合偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)，建立狀態(tài)空間模型，利用偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)更新?tīng)顟B(tài)估計(jì)。

13、優(yōu)選的，所述將深度圖像轉(zhuǎn)換為偽激光數(shù)據(jù)步驟為：

14、從rgb-d攝像頭獲取深度圖像；

15、獲取相機(jī)內(nèi)參，包括焦距fx和fy以及主點(diǎn)坐標(biāo)cx和cy，這些參數(shù)由相機(jī)標(biāo)定獲得；

16、將深度圖像中的每個(gè)像素坐標(biāo)(u,v)及其深度值d(u,v)轉(zhuǎn)換為三維點(diǎn)(x,y,z)，其計(jì)算公式為z＝d(u,v)；

17、將三維點(diǎn)云(x,y,z)轉(zhuǎn)換為二維偽激光數(shù)據(jù)，忽略y軸，僅保留x和y坐標(biāo)，得到偽激光數(shù)據(jù)(x,y)；

18、對(duì)生成的偽激光數(shù)據(jù)進(jìn)行過(guò)濾，去除超出最大距離的點(diǎn)；

19、將點(diǎn)云數(shù)據(jù)按體素大小進(jìn)行分割，在每個(gè)體素中保留一個(gè)代表點(diǎn)。

20、優(yōu)選的，所述對(duì)偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)進(jìn)行傳感器間的外部標(biāo)定，獲取坐標(biāo)變換矩陣步驟為：

21、在標(biāo)定過(guò)程中使用黑白相間的棋盤(pán)格標(biāo)定板；

22、放置標(biāo)定板并同時(shí)采集偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)，并記錄下每個(gè)傳感器采集到的標(biāo)定板圖像及其對(duì)應(yīng)的時(shí)間戳；

23、對(duì)采集到的標(biāo)定板圖像進(jìn)行特征點(diǎn)提取，所述特征點(diǎn)包括角點(diǎn)和棋盤(pán)格內(nèi)部的交點(diǎn)；

24、將偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)的特征點(diǎn)進(jìn)行匹配，建立特征點(diǎn)之間的對(duì)應(yīng)關(guān)系；

25、使用標(biāo)定板的已知尺寸和特征點(diǎn)的對(duì)應(yīng)關(guān)系，利用相機(jī)標(biāo)定算法計(jì)算偽激光和激光雷達(dá)之間的外部標(biāo)定參數(shù)，所述參數(shù)包括旋轉(zhuǎn)矩陣r和平移向量t；

26、將旋轉(zhuǎn)矩陣r和平移向量t合并為一個(gè)坐標(biāo)變換矩陣，所述變換矩陣公式為

27、優(yōu)選的，所述使用擴(kuò)展卡爾曼濾波器融合偽激光數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)，建立狀態(tài)空間模型步驟為：

28、初始化系統(tǒng)狀態(tài)向量x和協(xié)方差矩陣p，設(shè)定過(guò)程噪聲協(xié)方差矩陣q和測(cè)量噪聲協(xié)方差矩陣r；

29、使用系統(tǒng)動(dòng)力學(xué)模型進(jìn)行狀態(tài)預(yù)測(cè)，計(jì)算狀態(tài)轉(zhuǎn)移矩陣f和控制輸入矩陣b，預(yù)測(cè)狀態(tài)向量和協(xié)方差矩陣，其計(jì)算公式為p-＝fpft+q；

30、使用測(cè)量模型進(jìn)行狀態(tài)更新，計(jì)算觀測(cè)矩陣h和測(cè)量殘差y，計(jì)算卡爾曼增益，其計(jì)算公式為k＝p-ht(hp-ht+r)-1，更新?tīng)顟B(tài)向量和協(xié)方差矩陣，其計(jì)算公式為p＝(i-kh)p-，返回更新后的狀態(tài)向量作為系統(tǒng)狀態(tài)的估計(jì)值。

31、優(yōu)選的，所述根據(jù)目的地進(jìn)行路徑規(guī)劃步驟為：

32、接收環(huán)境地圖，將環(huán)境劃分為一系列離散的單元格，每個(gè)單元格標(biāo)記為可通行或不可通行；

33、確定起點(diǎn)和終點(diǎn)在地圖上的位置，初始化開(kāi)放列表和關(guān)閉列表；

34、使用a*算法在圖中找到從起點(diǎn)到終點(diǎn)的最短路徑。

35、優(yōu)選的，所述使用a*算法在圖中找到從起點(diǎn)到終點(diǎn)的最短路徑步驟為：

36、步驟1：定義啟發(fā)函數(shù)h(x)和代價(jià)函數(shù)g(x)，使用曼哈頓距離，其計(jì)算公式為其中xgoal表示為目標(biāo)節(jié)點(diǎn)的位置，xcurrent表示當(dāng)前正在處理的節(jié)點(diǎn)的位置；

37、步驟2：計(jì)算節(jié)點(diǎn)x的總估計(jì)代價(jià)，其計(jì)算公式為f(x)＝g(x)+h(x)，將起點(diǎn)節(jié)點(diǎn)加入開(kāi)放列表，并設(shè)定g(start)＝0和f(start)＝h(start)；

38、步驟3：從開(kāi)放列表中選擇f值最小的節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)current，如果當(dāng)前節(jié)點(diǎn)current是終點(diǎn)，則結(jié)束搜索并構(gòu)建路徑；

39、步驟4：對(duì)于當(dāng)前節(jié)點(diǎn)current的每個(gè)鄰居節(jié)點(diǎn)neighbor，計(jì)算鄰居節(jié)點(diǎn)g值，其計(jì)算公式為g(neighbor)＝g(current)+當(dāng)前節(jié)點(diǎn)到鄰居節(jié)點(diǎn)的代價(jià)，計(jì)算鄰居節(jié)點(diǎn)f值，其計(jì)算公式為f(neighbor)＝g(neighbor)+h(neighbor)；

40、步驟5：若鄰居節(jié)點(diǎn)在關(guān)閉列表中且新的g值更大，則跳過(guò)該鄰居節(jié)點(diǎn)，若鄰居節(jié)點(diǎn)不在開(kāi)放列表中，或新的g值更小則將鄰居節(jié)點(diǎn)加入開(kāi)放列表，更新鄰居節(jié)點(diǎn)的父節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)current，更新鄰居節(jié)點(diǎn)g值和f值；

41、步驟6：將當(dāng)前節(jié)點(diǎn)current從開(kāi)放列表移至關(guān)閉列表，重復(fù)步驟2-5，直到找到終點(diǎn)或開(kāi)放列表為空。

42、在上述技術(shù)方案中，本發(fā)明提供的技術(shù)效果和優(yōu)點(diǎn)：

43、通過(guò)數(shù)據(jù)轉(zhuǎn)換將偽激光數(shù)據(jù)與激光雷達(dá)數(shù)據(jù)融合，接收數(shù)據(jù)融合模塊傳輸?shù)娜诤虾蟮臄?shù)據(jù)，并根據(jù)融合后的數(shù)據(jù)構(gòu)建環(huán)境地圖，接收環(huán)境地圖構(gòu)建模塊傳輸?shù)沫h(huán)境地圖，并根據(jù)目的地進(jìn)行路徑規(guī)劃，接收路徑規(guī)劃模塊傳輸?shù)穆窂揭?guī)劃信息，并進(jìn)行導(dǎo)航與避障，有效提高了采集數(shù)據(jù)的準(zhǔn)確性與導(dǎo)航系統(tǒng)的穩(wěn)定性。