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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

22、；

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

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

25、；

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

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

28、；

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

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

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

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

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

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

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

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

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

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

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

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

41、；

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

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

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

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

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

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

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

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

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

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

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

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

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

55、；

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

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

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

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