本發(fā)明公開一種基于最優(yōu)控制理論的無(wú)人農(nóng)機(jī)動(dòng)態(tài)避障和路徑跟蹤方法，屬于農(nóng)機(jī)智能控制領(lǐng)域。

背景技術(shù)：

1、在現(xiàn)代農(nóng)業(yè)生產(chǎn)領(lǐng)域，無(wú)人農(nóng)機(jī)正變得越來(lái)越重要，它們?cè)谔嵘鳂I(yè)效率、減少人力成本以及推動(dòng)精準(zhǔn)農(nóng)業(yè)的實(shí)施等方面起著關(guān)鍵作用。這些進(jìn)步不僅優(yōu)化了農(nóng)業(yè)生產(chǎn)流程，還有助于實(shí)現(xiàn)資源的更高效利用和作物管理的精細(xì)化。盡管無(wú)人農(nóng)機(jī)在農(nóng)業(yè)自動(dòng)化中展現(xiàn)出巨大潛力，但它們?cè)谖粗驈?fù)雜環(huán)境中的表現(xiàn)仍然受限于其感知和決策能力。為了實(shí)現(xiàn)從車庫(kù)到田間的自動(dòng)駕駛，然后進(jìn)行作業(yè)活動(dòng)，無(wú)人農(nóng)機(jī)必須能夠持續(xù)而準(zhǔn)確地獲取周圍環(huán)境的信息，并及時(shí)做出反應(yīng)以避免與靜止或移動(dòng)的障礙物發(fā)生碰撞。這要求車輛具備高度的環(huán)境感知能力和動(dòng)態(tài)決策能力。

2、liu提出了一種單級(jí)結(jié)構(gòu)的非線性模型預(yù)測(cè)控制(nlmpc)算法。該算法旨在優(yōu)化大型自動(dòng)駕駛汽車在非結(jié)構(gòu)化環(huán)境中導(dǎo)航的轉(zhuǎn)向角度，算法目標(biāo)是最小化到達(dá)目標(biāo)目的地所需的時(shí)間，同時(shí)繞過(guò)靜止的障礙物，但該實(shí)驗(yàn)環(huán)境為越野環(huán)境下，缺乏明確的車道。考慮到農(nóng)業(yè)作業(yè)的特殊性，無(wú)人農(nóng)機(jī)的行駛道路邊界往往由植被或農(nóng)田構(gòu)成，這道路環(huán)境增加了行駛的復(fù)雜性，要求車輛不僅要能夠應(yīng)對(duì)突發(fā)情況，避開突然出現(xiàn)的障礙物，如其他車輛、行人或動(dòng)物，還要遵守道路邊界的約束。

3、此外，農(nóng)業(yè)環(huán)境中道路的動(dòng)態(tài)變化也對(duì)無(wú)人農(nóng)機(jī)的避障能力提出了更高要求。季節(jié)性作物的生長(zhǎng)、田間作業(yè)的進(jìn)行以及天氣變化都可能對(duì)道路產(chǎn)生影響。因此，將動(dòng)態(tài)避障策略納入模型預(yù)測(cè)控制算法，對(duì)于確保無(wú)人農(nóng)機(jī)在現(xiàn)實(shí)農(nóng)業(yè)場(chǎng)景中安全、有效地運(yùn)行至關(guān)重要。

4、因此，研究的重點(diǎn)在于開發(fā)一種能夠適應(yīng)農(nóng)業(yè)環(huán)境動(dòng)態(tài)性的控制策略，使無(wú)人農(nóng)機(jī)能夠在保持高效作業(yè)的同時(shí)，確保行駛安全，提高無(wú)人農(nóng)機(jī)在實(shí)際應(yīng)用中的可靠性和適應(yīng)性，從而推動(dòng)現(xiàn)代農(nóng)業(yè)向更高水平的自動(dòng)化和智能化發(fā)展。這種綜合考慮靜態(tài)、動(dòng)態(tài)障礙物和道路邊界的控制策略，不僅能夠提升無(wú)人農(nóng)機(jī)的操作效率，還能夠增強(qiáng)其在復(fù)雜農(nóng)業(yè)環(huán)境中的魯棒性。

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

1、本發(fā)明為解決現(xiàn)有無(wú)人農(nóng)機(jī)在現(xiàn)實(shí)復(fù)雜的農(nóng)業(yè)場(chǎng)景下的動(dòng)態(tài)避障和路徑跟蹤問(wèn)題，從車輛建模、動(dòng)態(tài)避障方法、以及路徑跟蹤控制三個(gè)方面進(jìn)行無(wú)人農(nóng)機(jī)動(dòng)態(tài)避碰和路徑跟蹤控制研究，提出一種基于最優(yōu)控制理論的無(wú)人農(nóng)機(jī)動(dòng)態(tài)避障和路徑跟蹤方法。

2、一種基于最優(yōu)控制理論的無(wú)人農(nóng)機(jī)動(dòng)態(tài)避障和路徑跟蹤方法，包括以下步驟：

3、s1：利用北斗rtk定位模塊、激光雷達(dá)和衛(wèi)星地圖獲取無(wú)人農(nóng)機(jī)狀態(tài)信息、障礙物信息以及道路信息；

4、s2：根據(jù)無(wú)人農(nóng)機(jī)和障礙物的當(dāng)前狀態(tài)信息，建立無(wú)人農(nóng)機(jī)和障礙物的運(yùn)動(dòng)學(xué)模型，并將兩者結(jié)合為一個(gè)非線性系統(tǒng)模型，用歐拉離散法對(duì)上述方程進(jìn)行積分，得到離散化的非線性系統(tǒng)模型，作為非線性模型預(yù)測(cè)控制算法的預(yù)測(cè)模型；

5、s3：從所需跟蹤路徑上選取無(wú)人農(nóng)機(jī)路徑跟蹤的參考點(diǎn)；

6、s4：根據(jù)獲取的障礙物信息和道路邊界信息，綜合考慮無(wú)人農(nóng)機(jī)和障礙物的狀態(tài)，設(shè)計(jì)避障約束條件；

7、s5：建立避障約束目標(biāo)函數(shù)；

8、s6：結(jié)合無(wú)人農(nóng)機(jī)動(dòng)力系統(tǒng)約束條件，將路徑跟蹤誤差、控制偏差以及避障約束目標(biāo)函數(shù)相結(jié)合設(shè)計(jì)總目標(biāo)函數(shù)，轉(zhuǎn)化為多約束最優(yōu)控制問(wèn)題并進(jìn)行求解。

9、2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述s2包括如下步驟：

10、s2.1.建立無(wú)人農(nóng)機(jī)的運(yùn)動(dòng)學(xué)模型，具體形式如下所示：

11、

12、式中：vx表示無(wú)人農(nóng)機(jī)的縱向速度，vy表示無(wú)人農(nóng)機(jī)的橫向速度；lf表示無(wú)人農(nóng)機(jī)前軸到重心的距離，l表示無(wú)人農(nóng)機(jī)的軸距，ψ表示無(wú)人農(nóng)機(jī)的航向角，δf表示無(wú)人農(nóng)機(jī)前輪轉(zhuǎn)向角；

13、s2.2.建立障礙物的運(yùn)動(dòng)學(xué)模型，具體形式如下：

14、

15、式中：(xm，ym)表示障礙物的位置，vm表示障礙物的速度，ψm表示障礙物的航向角；

16、s2.3.將無(wú)人農(nóng)機(jī)和障礙物的運(yùn)動(dòng)學(xué)模型結(jié)合，得到一個(gè)系統(tǒng)模型，具體形式如下所示：

17、

18、式中：z表示無(wú)人農(nóng)機(jī)和障礙物的狀態(tài)信息，u表示控制量，包括無(wú)人農(nóng)機(jī)轉(zhuǎn)向角δf和算法每次迭代的時(shí)間步長(zhǎng)δt：

19、s2.4.通過(guò)歐拉離散法對(duì)上述方程積分，得到離散化的非線性系統(tǒng)模型，具體形式如下：

20、z(k+1)＝z(k)+δtf(z，u)，k∈[1，n]

21、式中：δt表示算法每次迭代的時(shí)間步長(zhǎng)，n表示算法總預(yù)測(cè)步數(shù)。

22、3.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述s3包括以下步驟：

23、s3.1在非線性模型預(yù)測(cè)控制算法中，根據(jù)無(wú)人農(nóng)機(jī)當(dāng)前狀態(tài)預(yù)測(cè)其未來(lái)的n步狀態(tài)；根據(jù)獲取的無(wú)人農(nóng)機(jī)狀態(tài)信息和道路信息，在所需跟蹤路徑上確定n+1個(gè)點(diǎn)，這些點(diǎn)將作為無(wú)人農(nóng)機(jī)路徑跟蹤時(shí)的參考點(diǎn)；

24、s3.2當(dāng)無(wú)人農(nóng)機(jī)沒(méi)有偏離所需跟蹤路徑時(shí)，直接選取距離無(wú)人農(nóng)機(jī)當(dāng)前位置最近，即無(wú)人農(nóng)機(jī)正前方的參考點(diǎn)即可；

25、s3.3當(dāng)無(wú)人農(nóng)機(jī)因避障而短暫偏離所需跟蹤路徑時(shí)，由無(wú)人農(nóng)機(jī)重心，沿著全局坐標(biāo)系x、y軸投影到參考路徑上，兩個(gè)投影點(diǎn)中，靠近目標(biāo)的投影點(diǎn)將被選為參考點(diǎn)。

26、4.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述s4包括以下步驟：

27、s4.1.對(duì)于靜止障礙物，此前避障約束大多采用標(biāo)準(zhǔn)圓或標(biāo)準(zhǔn)橢圓，其避障效果會(huì)由于障礙物形狀的不同而出現(xiàn)較大差異，因此提出超橢圓避障約束，可以更精確地描繪障礙物的輪廓，獲得更準(zhǔn)確的安全區(qū)域，由此建立避障約束函數(shù)，具體形式如下：

28、

29、式中：(x(t)，y(t))表示預(yù)測(cè)步數(shù)為t時(shí)無(wú)人農(nóng)機(jī)的位置，表示預(yù)測(cè)步數(shù)為t時(shí)第j個(gè)靜止障礙物的位置；n1用于確定超橢圓的具體形狀；l表示無(wú)人農(nóng)機(jī)軸距，w表示無(wú)人農(nóng)機(jī)軌寬；r1和r2根據(jù)靜止障礙物幾何形狀取值，以獲取更準(zhǔn)確的避障區(qū)域；n表示總預(yù)測(cè)步數(shù)，s表示總靜止障礙物數(shù)量；

30、s4.2.對(duì)于移動(dòng)障礙物，采用和靜止障礙物相類似的方法，考慮到移動(dòng)障礙物的速度，采取相對(duì)保守的約束以保證絕對(duì)安全，具體形式如下：

31、

32、式中：(x(t)，y(t))表示預(yù)測(cè)步數(shù)為t時(shí)無(wú)人農(nóng)機(jī)的位置，表示預(yù)測(cè)步數(shù)為t時(shí)第k個(gè)移動(dòng)障礙物的位置；n2用于確定超橢圓的具體形狀；l表示無(wú)人農(nóng)機(jī)軸距，w表示無(wú)人農(nóng)機(jī)軌寬；r3和r4根據(jù)移動(dòng)障礙物幾何形狀取值，以獲取更準(zhǔn)確的避障區(qū)域；n表示總預(yù)測(cè)步數(shù)，m表示總移動(dòng)障礙物數(shù)量；

33、s4.3.對(duì)于道路邊界，將其離散化為邊界點(diǎn)，視為靜止障礙物并采用相類似的方法，具體形式如下：

34、

35、式中：(x(t)，y(t))表示預(yù)測(cè)步數(shù)為t時(shí)無(wú)人農(nóng)機(jī)的位置，表示預(yù)測(cè)步數(shù)為t時(shí)第i個(gè)邊界點(diǎn)的位置；n3用于確定超橢圓的具體形狀；l表示無(wú)人農(nóng)機(jī)軸距，w表示無(wú)人農(nóng)機(jī)軌寬；n表示總預(yù)測(cè)步數(shù)，r表示總邊界點(diǎn)。

36、5.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述s5中，結(jié)合無(wú)人農(nóng)機(jī)避障約束，綜合考慮障礙物速度和避碰權(quán)重，設(shè)置無(wú)人農(nóng)機(jī)避障的目標(biāo)函數(shù)jobs具體形式如下：

37、

38、式中：第一個(gè)和式表示道路邊界避碰的目標(biāo)函數(shù)，i表示第i個(gè)道路邊界點(diǎn)，r表示總的道路邊界點(diǎn)數(shù)量，ωor表示道路邊界避碰約束的權(quán)重；x和y表示無(wú)人農(nóng)機(jī)的位置，和表示第i個(gè)道路邊界點(diǎn)的位置；n表示算法總預(yù)測(cè)步數(shù)，ξ表示一個(gè)小的正常數(shù)以避免分母出現(xiàn)0；第二個(gè)和式表示靜止障礙物避碰的目標(biāo)函數(shù)，j表示第j個(gè)靜止障礙物，s表示總的靜止障礙物數(shù)量，ωos表示靜止障礙物避碰約束的權(quán)重；x和y表示無(wú)人農(nóng)機(jī)的位置，和表示第j個(gè)靜止障礙物的位置；n表示算法總預(yù)測(cè)步數(shù)，ξ表示一個(gè)小的正常數(shù)以避免分母出現(xiàn)0；第三個(gè)和式表示移動(dòng)障礙物避碰的目標(biāo)函數(shù)，k表示第k個(gè)移動(dòng)障礙物，m表示總的移動(dòng)障礙物數(shù)量，ωom表示移動(dòng)障礙物避碰約束的權(quán)重；x和y表示無(wú)人農(nóng)機(jī)的位置，和表示第k個(gè)移動(dòng)障礙物的位置；n表示算法總預(yù)測(cè)步數(shù)，ξ表示一個(gè)小的正常數(shù)以避免分母出現(xiàn)0。

39、6.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述s6包括如下步驟：

40、s6.1.考慮到無(wú)人農(nóng)機(jī)機(jī)械連桿機(jī)構(gòu)對(duì)轉(zhuǎn)向系統(tǒng)的約束，將轉(zhuǎn)向角度限制在恒定范圍內(nèi)，具體表示為：

41、

42、式中：表示最小的前輪轉(zhuǎn)向角，表示最大的前輪轉(zhuǎn)向角；

43、s6.2.考慮到無(wú)人農(nóng)機(jī)動(dòng)力系統(tǒng)的特點(diǎn)和工作環(huán)境中障礙物的密度，對(duì)無(wú)人農(nóng)機(jī)的縱向速度、偏航角設(shè)定了具體的上下限，具體表示為：

44、

45、ψmin≤ψ(t)≤ψmax

46、式中：表示無(wú)人農(nóng)機(jī)縱向速度的最小值，表示無(wú)人農(nóng)機(jī)縱向速度的最大值；ψmin表示無(wú)人農(nóng)機(jī)航向角的最小值，ψmax表示無(wú)人農(nóng)機(jī)航向角的最大值；

47、s6.3.將跟蹤誤差、控制偏差以及避障約束目標(biāo)函數(shù)相結(jié)合，設(shè)計(jì)模型預(yù)測(cè)控制算法的目標(biāo)函數(shù)形式如下：

48、

49、式中：t表示第t次預(yù)測(cè)步數(shù)，x(t)表示無(wú)人農(nóng)機(jī)當(dāng)前位置，xref為無(wú)人農(nóng)機(jī)參考位置；u(t)表示無(wú)人農(nóng)機(jī)當(dāng)前控制量，uref表示無(wú)人農(nóng)機(jī)參考控制量；代表無(wú)人農(nóng)機(jī)位置x和y這兩個(gè)變量，表示無(wú)人農(nóng)機(jī)前輪轉(zhuǎn)角δf和算法每次迭代的時(shí)間步長(zhǎng)δt這兩個(gè)變量；jobs表示避障約束目標(biāo)函數(shù)；

50、s6.4.以總目標(biāo)函數(shù)，最小為目標(biāo)，同時(shí)滿足上述所提到的所有約束條件，利用ipopt求解器進(jìn)行數(shù)值求解。

51、通過(guò)以上技術(shù)方案，與現(xiàn)有技術(shù)相比，本發(fā)明的具有以下有益效果：

52、為了解決無(wú)人農(nóng)機(jī)在現(xiàn)實(shí)復(fù)雜的農(nóng)業(yè)場(chǎng)景下的動(dòng)態(tài)避障和路徑跟蹤問(wèn)題，本發(fā)明公開了一種基于最優(yōu)控制理論的無(wú)人農(nóng)機(jī)動(dòng)態(tài)避障和路徑跟蹤方法；首先，根據(jù)無(wú)人農(nóng)機(jī)和障礙物的狀態(tài)信息，構(gòu)建非線性系統(tǒng)模型，將其歐拉離散化后作為模型預(yù)測(cè)控制算法的預(yù)測(cè)模型；其次，建立避障約束，不僅考慮了移動(dòng)障礙物和靜止障礙物的避碰，而且考慮了道路邊界的限制，使其適用于更廣泛和更復(fù)雜的農(nóng)業(yè)場(chǎng)景。此外，為了使無(wú)人農(nóng)機(jī)能夠獲得更精確的安全區(qū)域和避障路徑，提出一種超橢圓避障約束，可以更好地描繪障礙物的輪廓，使無(wú)人農(nóng)機(jī)能夠安全地通過(guò)障礙物密集或不規(guī)則分布的區(qū)域。最后，將避障約束和無(wú)人農(nóng)機(jī)控制轉(zhuǎn)化為多約束的最優(yōu)控制問(wèn)題，實(shí)現(xiàn)無(wú)人農(nóng)機(jī)的動(dòng)態(tài)避障和路徑跟蹤，魯棒性高，實(shí)時(shí)性好，適用于復(fù)雜的農(nóng)業(yè)場(chǎng)景中。