本發(fā)明涉及跟隨導(dǎo)航，具體涉及果蔬收運自動跟隨與自主導(dǎo)航系統(tǒng)。

背景技術(shù)：

1、隨著科技的進步和農(nóng)業(yè)現(xiàn)代化進程的加速，農(nóng)業(yè)生產(chǎn)方式正逐步向智能化、自動化轉(zhuǎn)型，傳統(tǒng)果蔬收運過程中，需要大量人力進行搬運和跟隨，不僅勞動強度大，而且效率低下，而隨著消費者對果蔬品質(zhì)要求的提高和農(nóng)業(yè)生產(chǎn)規(guī)模的擴大，對高效、智能的果蔬收運系統(tǒng)需求日益增加，通過實現(xiàn)果蔬收運跟隨車的自動跟隨和自主導(dǎo)航，能夠顯著提高農(nóng)業(yè)生產(chǎn)效率，降低人力成本，符合農(nóng)業(yè)現(xiàn)代化的發(fā)展趨勢。

2、在軌跡跟蹤和自動跟隨的過程中，由于感知視野不夠?qū)挄绊懙礁S車相對于引導(dǎo)車的航向偏轉(zhuǎn)角，容易出現(xiàn)跟隨車偏離預(yù)定方向并丟失目標(biāo)而難以復(fù)位的情況，因此，如何調(diào)整跟隨車與引導(dǎo)車的相對航向偏角，實現(xiàn)連續(xù)的偏轉(zhuǎn)角檢測，以提高系統(tǒng)的控制精度，是我們要解決的問題，為此，現(xiàn)提出果蔬收運自動跟隨與自主導(dǎo)航系統(tǒng)。

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

1、本發(fā)明目的在于提供果蔬收運自動跟隨與自主導(dǎo)航系統(tǒng)，以解決上述背景技術(shù)中提出的問題。

2、為解決上述技術(shù)問題，本發(fā)明所采用的技術(shù)方案是：

3、果蔬收運自動跟隨與自主導(dǎo)航系統(tǒng)，包括控制中心，所述控制中心通信連接有環(huán)境感知模塊、目標(biāo)識別定位模塊、相對航向偏角檢測模塊、偏角調(diào)整決策模塊、動力與轉(zhuǎn)向執(zhí)行模塊、自主導(dǎo)航規(guī)劃模塊以及人機交互模塊，其中，各模塊間電信號連接；

4、所述環(huán)境感知模塊，用于收集果蔬收運跟隨車周圍的環(huán)境信息，包含引導(dǎo)車位置、速度、方向、周圍障礙物以及道路邊界信息，提供實時、準確的感知數(shù)據(jù)，為后續(xù)的決策和控制提供基礎(chǔ)；

5、所述目標(biāo)識別定位模塊，基于環(huán)境感知模塊獲取的數(shù)據(jù)，識別并定位引導(dǎo)車在空間中的坐標(biāo)位置、姿態(tài)信息，區(qū)分引導(dǎo)車與其他干擾物體，持續(xù)鎖定引導(dǎo)車，確保跟隨車的跟隨對象明確無誤，保證跟隨車能始終清楚要跟隨的目標(biāo)所在，提高跟隨的準確性；

6、所述相對航向偏角檢測模塊，對比跟隨車和引導(dǎo)車的位置、姿態(tài)數(shù)據(jù)，分析跟隨車相對于引導(dǎo)車的航向偏角，實現(xiàn)連續(xù)的偏轉(zhuǎn)角檢測，為調(diào)整跟隨車的方向提供精確依據(jù)；

7、所述偏角調(diào)整決策模塊，基于相對航向偏角檢測模塊的分析結(jié)果，判斷當(dāng)前跟隨情況是否正常，并對出現(xiàn)的偏離趨勢進行預(yù)警，進而更新跟隨車調(diào)整方向的策略；

8、所述動力與轉(zhuǎn)向執(zhí)行模塊，根據(jù)偏角調(diào)整決策模塊的策略下發(fā)控制指令，并控制跟隨車的轉(zhuǎn)向、加速和制動動作，實現(xiàn)跟隨車的精確控制，提高系統(tǒng)的控制精度和響應(yīng)速度；

9、所述自主導(dǎo)航規(guī)劃模塊，在無引導(dǎo)車或引導(dǎo)車信號丟失時，依靠自主導(dǎo)航技術(shù)，綜合障礙物因素為跟隨車規(guī)劃從當(dāng)前位置到目標(biāo)位置的最優(yōu)路徑，引導(dǎo)跟隨車前進，使跟隨車能夠安全、高效地到達目的地，確保在復(fù)雜環(huán)境中也能保持連續(xù)運行；

10、所述人機交互模塊，為操作人員提供可視化的操作界面，展示系統(tǒng)的運行狀態(tài)，包括跟隨車和引導(dǎo)車的位置、相對航向偏角、是否存在異常的信息，允許操作人員進行必要的參數(shù)設(shè)置、手動干預(yù)控制操作，方便在特殊情況下對系統(tǒng)進行靈活調(diào)整。

11、本發(fā)明技術(shù)方案的進一步改進在于：所述環(huán)境感知模塊中，環(huán)境信息的收集過程為：

12、在果蔬收運跟隨車上部署攝像頭、激光雷達的各類型傳感器，以捕捉周圍的環(huán)境信息，包括引導(dǎo)車的相對位置、速度、方向信息，周圍障礙物的距離、形狀、大小信息以及道路邊界的輪廓和位置信息；

13、通過攝像頭進行圖像采集和處理，識別車道、前方障礙物，通過激光雷達發(fā)送光束，同步接收反射回的光，利用時間差測量與障礙物的距離，并繪制點云圖，對障礙物的形狀、大小、距離進行確定；

14、將收集的周圍環(huán)境信息的相關(guān)數(shù)據(jù)傳輸至車輛的電子控制單元中進行預(yù)處理，預(yù)處理包括去噪、濾波、數(shù)據(jù)格式轉(zhuǎn)換，以提高數(shù)據(jù)的準確性和可用性，并對不同傳感器采集的數(shù)據(jù)進行校準與同步，使數(shù)據(jù)在時間和空間上保持一致；

15、將來自不同傳感器的數(shù)據(jù)進行融合，獲取全面、準確的環(huán)境信息，并從融合后的數(shù)據(jù)中提取障礙物的位置、速度、加速度以及道路邊界的幾何形狀特征，得到特征數(shù)據(jù)集。

16、本發(fā)明技術(shù)方案的進一步改進在于：所述目標(biāo)識別定位模塊中，區(qū)分引導(dǎo)車與其他干擾物體的過程為：

17、從環(huán)境感知模塊獲取的特征數(shù)據(jù)集中，逐一遍歷每個特征數(shù)據(jù)，使用圖像處理技術(shù)提取引導(dǎo)車尺寸、形狀、顏色的特征數(shù)據(jù)，以便于識別和定位；

18、通過機器學(xué)習(xí)算法，將提取的引導(dǎo)車特征數(shù)據(jù)與預(yù)先訓(xùn)練的引導(dǎo)車特征模型進行匹配，從匹配結(jié)果中篩選出符合引導(dǎo)車特征的候選目標(biāo)，對比候選目標(biāo)的特征與環(huán)境感知模塊提供的障礙物特征，剔除與引導(dǎo)車特征不符的干擾物體；

19、基于篩選出的引導(dǎo)車特征，利用圖像配準的圖像處理技術(shù)計算引導(dǎo)車在空間中的坐標(biāo)位置，并結(jié)合引導(dǎo)車的形狀和大小信息，估計引導(dǎo)車朝向、傾斜角度的姿態(tài)信息；

20、將引導(dǎo)車的坐標(biāo)位置、朝向、傾斜角度信息與從傳感器獲得的點云數(shù)據(jù)相關(guān)聯(lián)，以提高定位的準確性，并利用跟蹤算法（卡爾曼濾波）對引導(dǎo)車進行持續(xù)跟蹤和鎖定，確保跟隨車能夠始終準確跟隨引導(dǎo)車，根據(jù)環(huán)境感知模塊提供的實時數(shù)據(jù)，動態(tài)更新引導(dǎo)車的位置和姿態(tài)信息，以適應(yīng)環(huán)境變化。

21、本發(fā)明技術(shù)方案的進一步改進在于：所述引導(dǎo)車在空間中坐標(biāo)位置的計算公式為：

22、；

23、其中，p為引導(dǎo)車在空間中坐標(biāo)位置，x和y分別表示引導(dǎo)車在水平面上的坐標(biāo)位置，d為引導(dǎo)車與跟隨車在水平方向上的距離，為引導(dǎo)車與跟隨車在垂直方向上的距離，為引導(dǎo)車相對于水平面的傾斜角度，為垂直方向上的最大值，用于限制y的范圍；

24、所述引導(dǎo)車朝向的計算公式為：

25、；

26、其中，hd為引導(dǎo)車朝向，為考慮了象限的反正切函數(shù)，用于計算引導(dǎo)車的朝向角度，為引導(dǎo)車與跟隨車在垂直方向上的距離，d為引導(dǎo)車與跟隨車在水平方向上的距離；

27、所述引導(dǎo)車傾斜角度的計算公式為：

28、；

29、其中，為引導(dǎo)車傾斜角度，為引導(dǎo)車的高度變化，d為引導(dǎo)車與跟隨車在水平方向上的距離，為反正切函數(shù)，用于計算引導(dǎo)車的傾斜角度。

30、本發(fā)明技術(shù)方案的進一步改進在于：所述相對航向偏角檢測模塊中，連續(xù)偏轉(zhuǎn)角檢測的實現(xiàn)過程為：

31、通過環(huán)境感知模塊獲取跟隨車和引導(dǎo)車的位置、姿態(tài)數(shù)據(jù)，對獲取到的位置、姿態(tài)數(shù)據(jù)進行預(yù)處理，包括去噪、濾波、數(shù)據(jù)對齊，以提高數(shù)據(jù)的準確性和可靠性，并從預(yù)處理后的數(shù)據(jù)中提取用于計算相對航向偏角的特征信息，其中，位置數(shù)據(jù)為gps坐標(biāo)，姿態(tài)數(shù)據(jù)包括速度矢量（速度大小和方向）、方向角度（車輛朝向）；

32、將跟隨車的航向角（基于方向角度數(shù)據(jù)）與引導(dǎo)車的航向角進行對比，計算出兩者之間的差值，即為相對航向偏角，其中，所述相對航向偏角的計算公式為：；為相對航向偏角，為引導(dǎo)車的航向角，為跟隨車的航向角；

33、設(shè)置定時器，每隔500毫秒觸發(fā)一次數(shù)據(jù)更新和計算過程，定期更新跟隨車和引導(dǎo)車的位置和姿態(tài)數(shù)據(jù)，重復(fù)航向角對比的過程，進行連續(xù)的偏轉(zhuǎn)角檢測。

34、本發(fā)明技術(shù)方案的進一步改進在于：所述偏角調(diào)整決策模塊中，偏離趨勢預(yù)警的過程為：

35、設(shè)置相對航向偏角的正常范圍和預(yù)警閾值，其中，正常范圍為±5°，預(yù)警閾值為±10°，并接收來自相對航向偏角檢測模塊的實時分析結(jié)果，包括相對航向偏角的當(dāng)前值和歷史數(shù)據(jù)；

36、基于接收到的相對航向偏角數(shù)據(jù)，判斷當(dāng)前跟隨車相對于引導(dǎo)車的跟隨情況是否處于正常范圍內(nèi)，若在正常范圍內(nèi)，則當(dāng)前跟隨情況正常，繼續(xù)監(jiān)控，若超出正常范圍，則進入偏離預(yù)警和處理流程；

37、根據(jù)相對航向偏角的絕對值確定偏離程度，并結(jié)合連續(xù)多個時間點的相對航向偏角數(shù)據(jù)，計算偏離預(yù)警系數(shù)，分析相對航向偏角的偏離趨勢；

38、結(jié)合偏離趨勢的分析結(jié)果和計算的偏離預(yù)警系數(shù)，若偏離預(yù)警系數(shù)接近或超過預(yù)警閾值，且偏離趨勢持續(xù)或惡化，則觸發(fā)預(yù)警機制，通過聲音、燈光方式通知駕駛員；

39、根據(jù)偏離預(yù)警系數(shù)和偏離趨勢的分析結(jié)果，更新跟隨車調(diào)整方向的策略，以糾正偏離并保持跟隨車在正確的航向上，其中，根據(jù)相對航向偏角和偏離趨勢，確定跟隨車需調(diào)整的方向（左、右或保持），結(jié)合偏離預(yù)警系數(shù)和預(yù)設(shè)的調(diào)整策略，計算需調(diào)整的角度或速度變化量，將調(diào)整指令發(fā)送給跟隨車的動力與轉(zhuǎn)向執(zhí)行模塊，以執(zhí)行方向調(diào)整操作。

40、本發(fā)明技術(shù)方案的進一步改進在于：所述偏離預(yù)警系數(shù)的計算公式為：

41、；

42、其中，d為偏離預(yù)警系數(shù)，為相對航向偏角的當(dāng)前值，為預(yù)設(shè)的偏離預(yù)警閾值，為標(biāo)準差，用于調(diào)整偏離預(yù)警系數(shù)的敏感度，為第i個連續(xù)時間點的相對航向偏角數(shù)據(jù)，n為連續(xù)時間點的數(shù)量，為連續(xù)時間點內(nèi)相對航向偏角的最大值，為連續(xù)時間點內(nèi)相對航向偏角的最小值，d的取值范圍限定在0至1之間，其中0表示沒有偏離，1表示嚴重偏離。

43、本發(fā)明技術(shù)方案的進一步改進在于：所述動力與轉(zhuǎn)向執(zhí)行模塊中，跟隨車控制的實現(xiàn)過程為：

44、動力與轉(zhuǎn)向執(zhí)行模塊通過通信接口接收偏角調(diào)整決策模塊更新的調(diào)整策略，提取調(diào)整策略中的控制指令，并進行指令解析；

45、基于控制指令的解析結(jié)果，提取出轉(zhuǎn)向角度、加速度或制動力的具體控制參數(shù)，并根據(jù)解析結(jié)果，確定需執(zhí)行的動作類型，分別為轉(zhuǎn)向、加速或制動；

46、對于轉(zhuǎn)向動作的執(zhí)行，若控制指令為轉(zhuǎn)向動作，則根據(jù)轉(zhuǎn)向角度參數(shù)，控制轉(zhuǎn)向機構(gòu)進行轉(zhuǎn)向，轉(zhuǎn)向過程中，通過傳感器實時監(jiān)測轉(zhuǎn)向角度和轉(zhuǎn)向速度，確保轉(zhuǎn)向動作的準確性和穩(wěn)定性；對于加速動作的執(zhí)行，若控制指令為加速動作，則根據(jù)加速度參數(shù)，控制發(fā)動機的輸出功率，實現(xiàn)加速，加速過程中，通過傳感器實時監(jiān)測車速和加速度，確保加速動作的平穩(wěn)性和安全性；對于制動動作的執(zhí)行，若控制指令為制動動作，則根據(jù)制動力參數(shù)，控制制動系統(tǒng)產(chǎn)生相應(yīng)的制動力，實現(xiàn)減速或停車，制動過程中，通過傳感器實時監(jiān)測車速和減速度，確保制動動作的及時性和可靠性，同時或順序執(zhí)行轉(zhuǎn)向、加速和制動動作，以實現(xiàn)對跟隨車精確控制；

47、在執(zhí)行轉(zhuǎn)向、加速或制動動作的過程中，通過傳感器實時監(jiān)測車輛的實際狀態(tài)，將實際狀態(tài)與預(yù)期目標(biāo)進行對比，評估執(zhí)行效果，將執(zhí)行結(jié)果（實際轉(zhuǎn)向角度、車速、加速度）通過通信接口反饋至偏角調(diào)整決策模塊，偏角調(diào)整決策模塊根據(jù)反饋結(jié)果，進一步調(diào)整控制策略，提高系統(tǒng)的控制精度和響應(yīng)速度。

48、本發(fā)明技術(shù)方案的進一步改進在于：所述自主導(dǎo)航規(guī)劃模塊中，自主導(dǎo)航規(guī)劃引導(dǎo)跟隨車前進的過程為：

49、基于跟隨車采集的周圍環(huán)境信息，處理后轉(zhuǎn)化為柵格地圖表示，利用柵格地圖直觀反映環(huán)境中的障礙物和可行區(qū)域；

50、采用視覺slam算法，根據(jù)環(huán)境信息和已知地圖，估計跟隨車的當(dāng)前位置，并通過慣性測量單元（imu）傳感器，實時獲取跟隨車的姿態(tài)信息；

51、基于傳感器數(shù)據(jù)和地圖信息，實時檢測環(huán)境中的障礙物，根據(jù)障礙物的位置、形狀和大小，制定避障策略，包括繞行、停車等待，并在路徑規(guī)劃中引入障礙物代價函數(shù)，評估路徑中障礙物的影響程度，以避開障礙物并優(yōu)化路徑；

52、根據(jù)目標(biāo)位置和當(dāng)前位置，利用搜索算法（a*算法），在地圖中搜索避開障礙物、遵循交通規(guī)則的最短路徑，并在全局路徑規(guī)劃的基礎(chǔ)上，根據(jù)跟隨車的實時狀態(tài)和周圍環(huán)境的變化，進行動態(tài)調(diào)整和優(yōu)化，確保跟隨車能夠平穩(wěn)地行駛；

53、根據(jù)路徑跟蹤結(jié)果和跟隨車的實時狀態(tài)，生成控制指令，控制跟隨車的轉(zhuǎn)向、加速和制動動作，實現(xiàn)精確控制。

54、本發(fā)明技術(shù)方案的進一步改進在于：所述障礙物代價函數(shù)的表達式為：

55、；

56、其中，為障礙物代價函數(shù)，用于評估路徑中障礙物的影響程度，為跟隨車到第j個障礙物的距離，為障礙物距離的標(biāo)準差，用于調(diào)整代價函數(shù)對距離的敏感度，m為路徑中障礙物的總數(shù)，k為常數(shù)，用于調(diào)整指數(shù)函數(shù)的曲率，影響障礙物距離對代價函數(shù)的影響程度，為路徑中允許的最大角度偏差，為跟隨車與第j個障礙物之間的角度偏差，的取值范圍限定在0至1之間，其中0表示路徑完全避開障礙物，1表示路徑直接通過障礙物。

57、由于采用了上述技術(shù)方案，本發(fā)明相對現(xiàn)有技術(shù)來說，取得的技術(shù)進步是：

58、本發(fā)明提供果蔬收運自動跟隨與自主導(dǎo)航系統(tǒng)，通過自主導(dǎo)航和避障功能，使跟隨車能夠自動、準確地跟隨引導(dǎo)車或自主規(guī)劃路徑，無需人工干預(yù)完成復(fù)雜的運輸任務(wù)，同時，系統(tǒng)能夠根據(jù)目標(biāo)位置和當(dāng)前位置，快速規(guī)劃出最優(yōu)路徑，進一步縮短了運輸時間，提高了運輸效率，還具備高精度的定位與姿態(tài)估計能力，能夠?qū)崟r獲取跟隨車的位置和姿態(tài)信息，確保其在復(fù)雜環(huán)境中保持穩(wěn)定行駛。

59、本發(fā)明提供果蔬收運自動跟隨與自主導(dǎo)航系統(tǒng)，通過精確的路徑規(guī)劃和控制指令生成，使跟隨車能夠平穩(wěn)地行駛在規(guī)劃好的路徑上，避免了因急轉(zhuǎn)彎、急加速等不當(dāng)操作而導(dǎo)致的危險情況，并且通過實時監(jiān)測和分析周圍環(huán)境信息，能夠及時發(fā)現(xiàn)并避開潛在的危險障礙物，從而避免碰撞事故的發(fā)生。