本發(fā)明涉及無人車編隊(duì)控制，特別涉及一種無人車編隊(duì)動(dòng)態(tài)控制方法及系統(tǒng)。

背景技術(shù)：

1、無人車在非結(jié)構(gòu)化、復(fù)雜地形中執(zhí)行協(xié)同任務(wù)的需求不斷增加。然而，當(dāng)多輛無人車同時(shí)自主執(zhí)行任務(wù)時(shí)，往往面臨多種挑戰(zhàn)，如相互避讓、優(yōu)先級(jí)不明確，導(dǎo)致行駛路線的混亂。采用編隊(duì)行駛不僅能夠有效提升任務(wù)效率和協(xié)同作業(yè)能力，還能提升多車在任務(wù)區(qū)域的快速抵達(dá)能力。然而，不編隊(duì)的多車行駛模式存在顯著問題，包括缺乏協(xié)作機(jī)制和路徑規(guī)劃效率低下等。

2、為了適應(yīng)無人車編隊(duì)行駛的需求，控制架構(gòu)發(fā)展出集中式、分布式和混合式三種主要結(jié)構(gòu)。集中式控制結(jié)構(gòu)由一個(gè)中央處理單元進(jìn)行全局控制，適用于需要全局規(guī)劃的場(chǎng)景，但在信息交互和環(huán)境適應(yīng)性方面存在瓶頸。分布式控制結(jié)構(gòu)則由各無人車自行決策，具備較好的環(huán)境適應(yīng)性，但在全局規(guī)劃上有所不足?；旌鲜娇刂平Y(jié)構(gòu)結(jié)合了集中式與分布式的優(yōu)勢(shì)，適合在復(fù)雜環(huán)境下的多車協(xié)同任務(wù)場(chǎng)景。

3、無人車編隊(duì)控制的實(shí)現(xiàn)主要依賴于編隊(duì)算法，包括虛擬結(jié)構(gòu)法、基于行為法和領(lǐng)航-跟隨法。盡管這些算法在一定程度上實(shí)現(xiàn)了多車協(xié)同，但仍存在模型局限性，如隊(duì)形保持能力不足、動(dòng)態(tài)環(huán)境適應(yīng)性欠佳。此外，無人車編隊(duì)在運(yùn)動(dòng)中面臨復(fù)雜的障礙物環(huán)境，因此編隊(duì)避障策略的設(shè)計(jì)至關(guān)重要。人工勢(shì)場(chǎng)法、旋轉(zhuǎn)勢(shì)場(chǎng)法和基于柵格的避障方法在傳統(tǒng)避障中應(yīng)用廣泛，但它們?cè)诰庩?duì)完整性保持和動(dòng)態(tài)適應(yīng)性方面存在不足。

4、現(xiàn)有的編隊(duì)控制算法難以同時(shí)兼顧隊(duì)形穩(wěn)定性與動(dòng)態(tài)適應(yīng)性，尤其是在狹窄地形下，單一的固定隊(duì)形往往無法滿足通行需求，降低了通行效率；傳統(tǒng)避障算法（如人工勢(shì)場(chǎng)法）在面對(duì)復(fù)雜障礙物分布時(shí)容易出現(xiàn)局部極小值問題，導(dǎo)致目標(biāo)不可達(dá)，此外，自主避障方法在編隊(duì)中使用較為頻繁，雖然保證了單車安全性，但易破壞編隊(duì)完整性，影響編隊(duì)效率；當(dāng)任務(wù)車輛需要脫離隊(duì)形執(zhí)行獨(dú)立任務(wù)時(shí)，現(xiàn)有方法較難實(shí)現(xiàn)編隊(duì)與獨(dú)立任務(wù)間的動(dòng)態(tài)切換，可能導(dǎo)致編隊(duì)混亂。

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

1、本發(fā)明的目的在于提供一種無人車編隊(duì)動(dòng)態(tài)控制方法及系統(tǒng)，用于解決上述至少一個(gè)技術(shù)問題，其能夠在復(fù)雜環(huán)境中提高無人車編隊(duì)的隊(duì)形生成和保持能力，優(yōu)化編隊(duì)避障策略，實(shí)現(xiàn)編隊(duì)與獨(dú)立任務(wù)的平衡。通過動(dòng)態(tài)感知環(huán)境信息，自動(dòng)確定最優(yōu)隊(duì)形，基于主車從車模型與誤差控制法實(shí)現(xiàn)高效編隊(duì)跟蹤，提供了任務(wù)車輛脫隊(duì)和編隊(duì)切換的動(dòng)態(tài)控制方案。

2、本發(fā)明的實(shí)施例是這樣實(shí)現(xiàn)的：

3、一種無人車編隊(duì)動(dòng)態(tài)控制方法，其包括：通過傳感器和通信設(shè)備收集當(dāng)前環(huán)境的感知信息。

4、根據(jù)收集到的所述感知信息，分析并確定當(dāng)前情況下效率最高的隊(duì)形。

5、建立主車從車編隊(duì)模型，使每輛無人車根據(jù)所述主車從車編隊(duì)模型的運(yùn)動(dòng)學(xué)方程進(jìn)行運(yùn)動(dòng)。

6、建立誤差動(dòng)態(tài)模型，設(shè)計(jì)誤差控制法，使得從車與主車之間的跟蹤誤差收斂到最小鄰域內(nèi)。

7、當(dāng)有任務(wù)車輛需駛離隊(duì)形時(shí)，所述任務(wù)車輛對(duì)其他車輛的軌跡進(jìn)行預(yù)測(cè)，并基于動(dòng)態(tài)預(yù)測(cè)避障控制方法進(jìn)行避障行駛，前往任務(wù)點(diǎn)位。

8、根據(jù)當(dāng)前的環(huán)境條件和任務(wù)要求，進(jìn)行隊(duì)形切換。

9、在本發(fā)明較佳的實(shí)施例中，上述無人車編隊(duì)動(dòng)態(tài)控制方法中，所述通過傳感器和通信設(shè)備收集當(dāng)前環(huán)境的感知信息包括：

10、每輛無人車配備若干傳感器，所述傳感器包括激光雷達(dá)、相機(jī)、慣性測(cè)量單元和gps中的至少一種，實(shí)時(shí)收集當(dāng)前環(huán)境的感知信息。

11、主車與從車之間通過無線通信共享所述感知信息。

12、使用傳感器數(shù)據(jù)融合算法對(duì)所述感知信息進(jìn)行整合。

13、其技術(shù)效果在于：通過多傳感器的組合使用，在復(fù)雜、動(dòng)態(tài)的環(huán)境中更精確地識(shí)別障礙物、道路特征和其他車輛的位置；主車與從車通過無線通信實(shí)時(shí)共享感知信息，確保整個(gè)編隊(duì)對(duì)當(dāng)前環(huán)境有一致的理解，每輛車不僅依賴自身傳感器，還可以利用其他車輛的感知信息，避免盲區(qū)和信息滯后，增強(qiáng)復(fù)雜環(huán)境下的動(dòng)態(tài)適應(yīng)性。

14、在本發(fā)明較佳的實(shí)施例中，上述無人車編隊(duì)動(dòng)態(tài)控制方法中，所述根據(jù)收集到的所述感知信息，分析并確定當(dāng)前情況下效率最高的隊(duì)形包括：

15、進(jìn)行環(huán)境約束分析，收集道路寬度、障礙物信息、地形變化和交通流量數(shù)據(jù)。

16、進(jìn)行任務(wù)要求分析，確定執(zhí)行的任務(wù)、任務(wù)的時(shí)間和位置要求、任務(wù)的區(qū)域大小和形態(tài)、任務(wù)的協(xié)作需求。

17、建立隊(duì)形效率評(píng)估模型，設(shè)定用于評(píng)估隊(duì)形效率的穩(wěn)定性指標(biāo)、避障能力指標(biāo)和行駛效率指標(biāo)，所述穩(wěn)定性指標(biāo)通過計(jì)算車輛之間的相對(duì)位置誤差和速度差建立，所述避障能力指標(biāo)通過計(jì)算車輛之間的相對(duì)距離和避障算法的反應(yīng)時(shí)間建立，所述行駛效率指標(biāo)通過計(jì)算完成任務(wù)的時(shí)間、隊(duì)形的路徑長(zhǎng)度和速度。

18、將所述環(huán)境約束分析和所述任務(wù)要求分析的量輸入所述隊(duì)形效率評(píng)估模型中，對(duì)不同隊(duì)形進(jìn)行效率評(píng)估，得到最適合當(dāng)前環(huán)境和任務(wù)的隊(duì)形。

19、其技術(shù)效果在于：通過環(huán)境約束分析（包括道路寬度、障礙物、地形和交通流量）和任務(wù)要求分析（包括任務(wù)類型、時(shí)間、位置、區(qū)域大小及協(xié)作需求），無人車編隊(duì)能夠快速適應(yīng)不同的環(huán)境條件和任務(wù)場(chǎng)景；通過穩(wěn)定性指標(biāo)來考察編隊(duì)車輛之間的相對(duì)位置誤差和速度差，在不同環(huán)境和任務(wù)場(chǎng)景中保持隊(duì)形結(jié)構(gòu)，增強(qiáng)隊(duì)形的穩(wěn)定性和協(xié)同控制，提高編隊(duì)的避障能力和安全性。

20、在本發(fā)明較佳的實(shí)施例中，上述無人車編隊(duì)動(dòng)態(tài)控制方法中，所述建立主車從車編隊(duì)模型，使每輛無人車根據(jù)所述主車從車編隊(duì)模型的運(yùn)動(dòng)學(xué)方程進(jìn)行運(yùn)動(dòng)包括：

21、建立主車的運(yùn)動(dòng)學(xué)方程，

22、其中，為所述主車的位置，為所述主車在水平方向上的運(yùn)動(dòng)速度，為所述主車在垂直方向上的運(yùn)動(dòng)速度，為所述主車的航向角，為所述主車的線速度，為所述主車的角速度。

23、基于所述主車的位置，建立從車的運(yùn)動(dòng)學(xué)方程，

24、其中，為所述從車的期望位置，為所述從車與所述主車之間的期望相對(duì)距離，為所述從車相對(duì)于所述主車的期望相對(duì)角度，為所述從車的航向角。

25、其技術(shù)效果在于：設(shè)定主車的運(yùn)動(dòng)學(xué)方程作為參考，使從車跟隨主車，形成預(yù)定的隊(duì)形，確保了從車能夠?qū)崟r(shí)跟蹤主車的動(dòng)態(tài)位置和航向變化，實(shí)現(xiàn)精確的編隊(duì)控制和保持隊(duì)形結(jié)構(gòu)，從車的運(yùn)動(dòng)行為在空間上與主車保持協(xié)調(diào)性，有利于適應(yīng)動(dòng)態(tài)環(huán)境變化，增強(qiáng)隊(duì)形的安全性和避免碰撞的能力，便于實(shí)現(xiàn)隊(duì)形調(diào)整和靈活性。

26、在本發(fā)明較佳的實(shí)施例中，上述無人車編隊(duì)動(dòng)態(tài)控制方法中，所述建立誤差動(dòng)態(tài)模型，設(shè)計(jì)誤差控制法，使得從車與主車之間的跟蹤誤差收斂到最小鄰域內(nèi)包括：

27、計(jì)算所述從車的實(shí)際位置誤差，

28、其中，為所述從車的實(shí)際位置。

29、建立所述主車與所述從車之間的誤差動(dòng)態(tài)模型，

30、其中，為所述實(shí)際位置誤差的變化率。

31、設(shè)計(jì)滑模面和控制律，使所述實(shí)際位置誤差收斂到零，使所述從車跟隨所述主車。

32、所述滑模面的計(jì)算公式為?，其中，、、和為用于決定誤差收斂的速率的權(quán)重系數(shù)。

33、所述控制律的計(jì)算公式為，其中，為控制增益，，為符號(hào)函數(shù)。

34、其技術(shù)效果在于：通過建立誤差動(dòng)態(tài)模型，實(shí)時(shí)計(jì)算從車的實(shí)際位置誤差，并在控制律的作用下不斷調(diào)整從車的速度和角速度，使得誤差收斂至最小鄰域內(nèi)，確保從車穩(wěn)定跟隨主車，精確保持期望隊(duì)形；通過滑?？刂坡傻囊龑?dǎo)，從車的運(yùn)動(dòng)調(diào)整不再依賴于簡(jiǎn)單的比例控制，而是依據(jù)誤差動(dòng)態(tài)模型和滑模面的設(shè)計(jì)，使得控制調(diào)整更加平滑、穩(wěn)定，防止了從車運(yùn)動(dòng)過程中出現(xiàn)大幅波動(dòng)和劇烈變化，提高了編隊(duì)控制的平穩(wěn)性和精確性，減少了對(duì)主車的不必要追隨干擾。

35、在本發(fā)明較佳的實(shí)施例中，上述無人車編隊(duì)動(dòng)態(tài)控制方法中，所述當(dāng)有任務(wù)車輛需駛離隊(duì)形時(shí)，所述任務(wù)車輛對(duì)其他車輛的軌跡進(jìn)行預(yù)測(cè)，并基于動(dòng)態(tài)預(yù)測(cè)避障控制方法進(jìn)行避障行駛，前往任務(wù)點(diǎn)位包括：

36、當(dāng)有任務(wù)車輛需駛離隊(duì)形時(shí)，建立其他車輛的軌跡預(yù)測(cè)模型，其中，為所述其他車輛在二維空間中的位置，為所述其他車輛的速度在x和y方向的分量，為所述其他車輛的航向角。

37、基于時(shí)間步長(zhǎng)對(duì)所述軌跡預(yù)測(cè)模型進(jìn)行狀態(tài)更新，為描述車輛動(dòng)力學(xué)的狀態(tài)轉(zhuǎn)移矩陣，為描述控制輸入對(duì)狀態(tài)影響的輸入矩陣，為控制輸入，，為所述其他車輛在x和y方向的加速度，為所述其他車輛的轉(zhuǎn)向角速度。

38、計(jì)算得到在未來時(shí)間的軌跡預(yù)測(cè)，為所述其他車輛在當(dāng)前時(shí)刻t的位置，，是所述其他車輛的速度，。

39、對(duì)于所述任務(wù)車輛的當(dāng)前位置、所述任務(wù)車輛的目標(biāo)位置和所述其他車輛的預(yù)測(cè)軌跡，設(shè)置所述任務(wù)車輛的軌跡與所述目標(biāo)位置的距離最小化避障策略，建立所述任務(wù)車輛的軌跡避免與障礙物碰撞的約束條件，其中，為安全距離。

40、通過模型預(yù)測(cè)控制法，求解最優(yōu)的控制輸入，得到所述任務(wù)車輛的加速度和轉(zhuǎn)向角速度，其中，為權(quán)重系數(shù)。

41、根據(jù)得到的所述任務(wù)車輛的加速度和轉(zhuǎn)向角速度，執(zhí)行路徑規(guī)劃算法，生成所述任務(wù)車輛的行駛軌跡，使所述任務(wù)車輛前往任務(wù)點(diǎn)位。

42、其技術(shù)效果在于：基于時(shí)間步長(zhǎng)的狀態(tài)更新和模型預(yù)測(cè)控制（mpc）方法，任務(wù)車輛快速獲取未來的軌跡，預(yù)測(cè)并動(dòng)態(tài)調(diào)整控制策略，任務(wù)車輛能夠通過軌跡預(yù)測(cè)、避障規(guī)劃和路徑優(yōu)化，靈活、安全地駛離隊(duì)形并前往任務(wù)點(diǎn)位，提升了實(shí)時(shí)性和避障決策的靈活性，增強(qiáng)編隊(duì)系統(tǒng)的協(xié)作性，避免對(duì)其他車輛的干擾，實(shí)現(xiàn)了路徑規(guī)劃與避障策略的有機(jī)結(jié)合。

43、在本發(fā)明較佳的實(shí)施例中，上述無人車編隊(duì)動(dòng)態(tài)控制方法中，所述任務(wù)車輛需駛離隊(duì)形，前往目標(biāo)位置時(shí)，基于所述感知信息，實(shí)時(shí)更新障礙物位置信息，修正所述動(dòng)態(tài)預(yù)測(cè)避障控制方法。

44、在本發(fā)明較佳的實(shí)施例中，上述無人車編隊(duì)動(dòng)態(tài)控制方法中，所述根據(jù)當(dāng)前的環(huán)境條件和任務(wù)要求，進(jìn)行隊(duì)形切換包括：

45、設(shè)置環(huán)境條件矩陣，其中，表示環(huán)境中第k個(gè)障礙物的狀態(tài)。

46、設(shè)置任務(wù)要求矩陣，其中，表示任務(wù)的第l個(gè)目標(biāo)。

47、定義隊(duì)形切換優(yōu)先級(jí)函數(shù)，其中，為環(huán)境權(quán)重，為任務(wù)權(quán)重，為環(huán)境條件優(yōu)先級(jí)量化值，為任務(wù)條件優(yōu)先級(jí)量化值。

48、基于所述優(yōu)先級(jí)函數(shù)的值，確定當(dāng)前最優(yōu)隊(duì)形，其中，每種隊(duì)形通過相對(duì)于所述主車的位置的目標(biāo)偏移量，計(jì)算每輛從車的目標(biāo)位置。

49、計(jì)算所述從車的目標(biāo)位置誤差。

50、設(shè)計(jì)比例-微分控制法，使所述目標(biāo)位置誤差收斂至零，從車線速度，從車角速度，其中，為比例增益，為微分增益。

51、根據(jù)得到的所述從車線速度和所述從車角速度，執(zhí)行路徑規(guī)劃算法，生成所述從車的行駛軌跡，進(jìn)行隊(duì)形切換。

52、在本發(fā)明較佳的實(shí)施例中，上述無人車編隊(duì)動(dòng)態(tài)控制方法中，進(jìn)行隊(duì)形切換時(shí)，當(dāng)前的環(huán)境條件和任務(wù)要求發(fā)生變化時(shí)，重新計(jì)算所述優(yōu)先級(jí)函數(shù)，選擇新的最優(yōu)隊(duì)形。

53、每次進(jìn)行隊(duì)形切換時(shí)，更新所述從車的目標(biāo)位置和目標(biāo)位置誤差，并根據(jù)新的隊(duì)形控制所述從車運(yùn)動(dòng)。

54、其技術(shù)效果在于：通過設(shè)置環(huán)境條件矩陣并通過優(yōu)先級(jí)函數(shù)計(jì)算隊(duì)形切換優(yōu)先級(jí)，在復(fù)雜、不確定的環(huán)境下能夠自動(dòng)調(diào)整編隊(duì)形態(tài)，有效避開障礙物，提高編隊(duì)的環(huán)境適應(yīng)能力，優(yōu)先級(jí)函數(shù)結(jié)合環(huán)境權(quán)重和任務(wù)權(quán)重，自動(dòng)選擇當(dāng)前最優(yōu)隊(duì)形，實(shí)現(xiàn)編隊(duì)形態(tài)的智能化切換；通過比例-微分控制法（pd控制）使從車的目標(biāo)位置誤差收斂至零，使得從車能夠穩(wěn)定地跟隨主車進(jìn)行隊(duì)形切換，有效降低目標(biāo)位置誤差，并減少因編隊(duì)切換而產(chǎn)生的震蕩和不穩(wěn)定現(xiàn)象，優(yōu)化從車的運(yùn)動(dòng)控制，保證隊(duì)形穩(wěn)定性。

55、一種無人車編隊(duì)動(dòng)態(tài)控制系統(tǒng)，其包括：

56、感知信息采集模塊，用于通過傳感器和通信設(shè)備收集當(dāng)前環(huán)境的感知信息。

57、隊(duì)形分析模塊，用于根據(jù)收集到的所述感知信息，分析并確定當(dāng)前情況下效率最高的隊(duì)形。

58、編隊(duì)模型建立模塊，用于建立主車從車編隊(duì)模型，使每輛無人車根據(jù)所述主車從車編隊(duì)模型的運(yùn)動(dòng)學(xué)方程進(jìn)行運(yùn)動(dòng)。

59、誤差控制模塊，用于建立誤差動(dòng)態(tài)模型，設(shè)計(jì)誤差控制法，使得從車與主車之間的跟蹤誤差收斂到最小鄰域內(nèi)。

60、任務(wù)執(zhí)行避障控制模塊，用于當(dāng)有任務(wù)車輛需駛離隊(duì)形時(shí)，所述任務(wù)車輛對(duì)其他車輛的軌跡進(jìn)行預(yù)測(cè)，并基于動(dòng)態(tài)預(yù)測(cè)避障控制方法進(jìn)行避障行駛，前往任務(wù)點(diǎn)位。

61、隊(duì)形切換模塊，用于根據(jù)當(dāng)前的環(huán)境條件和任務(wù)要求，進(jìn)行隊(duì)形切換。

62、本發(fā)明實(shí)施例的有益效果是：

63、本發(fā)明通過建立主車從車編隊(duì)模型，設(shè)計(jì)基于誤差動(dòng)態(tài)模型的控制方法，實(shí)現(xiàn)了從車與主車之間跟蹤誤差的收斂，從而確保編隊(duì)整體的穩(wěn)定性與協(xié)同性。在編隊(duì)運(yùn)動(dòng)過程中，誤差控制模塊能夠使從車準(zhǔn)確跟隨主車，并通過比例-微分控制法實(shí)現(xiàn)從車目標(biāo)位置誤差的快速收斂，避免了傳統(tǒng)方法中因跟蹤誤差較大而導(dǎo)致的編隊(duì)不穩(wěn)定問題。

64、本發(fā)明在任務(wù)車輛需要脫離編隊(duì)前往特定任務(wù)位置時(shí)，能夠?qū)崟r(shí)預(yù)測(cè)其他車輛的軌跡，并使用動(dòng)態(tài)預(yù)測(cè)避障控制方法進(jìn)行規(guī)劃，避免任務(wù)車輛在獨(dú)立行動(dòng)中與其他車輛發(fā)生碰撞。同時(shí)，任務(wù)執(zhí)行避障控制模塊在任務(wù)車輛動(dòng)態(tài)預(yù)測(cè)避障中實(shí)時(shí)更新障礙物位置信息，實(shí)現(xiàn)對(duì)復(fù)雜環(huán)境的高效應(yīng)對(duì)，大幅提升了任務(wù)車輛的自主性和安全性，尤其適用于動(dòng)態(tài)變化的場(chǎng)景。

65、本發(fā)明通過設(shè)立環(huán)境條件矩陣與任務(wù)要求矩陣，并定義隊(duì)形切換優(yōu)先級(jí)函數(shù)，結(jié)合當(dāng)前環(huán)境和任務(wù)需求動(dòng)態(tài)切換隊(duì)形，確保在不同任務(wù)和環(huán)境下選擇最優(yōu)編隊(duì)模式。這種靈活的編隊(duì)切換機(jī)制使無人車編隊(duì)能夠高效適應(yīng)環(huán)境變化并滿足任務(wù)需求，提高了編隊(duì)的整體效率和任務(wù)完成效果。

66、本發(fā)明提出的隊(duì)形分析模塊通過環(huán)境約束和任務(wù)要求分析建立隊(duì)形效率評(píng)估模型，將穩(wěn)定性、避障能力和行駛效率等多個(gè)關(guān)鍵指標(biāo)結(jié)合，自動(dòng)評(píng)估并確定最適合當(dāng)前環(huán)境的隊(duì)形。相比于傳統(tǒng)的固定隊(duì)形，本發(fā)明的自動(dòng)化評(píng)估與決策機(jī)制大大提升了編隊(duì)的運(yùn)行效率與環(huán)境適應(yīng)能力。

67、通過每輛無人車的多種傳感器（如激光雷達(dá)、相機(jī)、慣性測(cè)量單元、gps等）實(shí)時(shí)采集環(huán)境感知信息，并通過無線通信實(shí)現(xiàn)信息共享，結(jié)合傳感器數(shù)據(jù)融合算法進(jìn)行整合處理，保證了環(huán)境感知的準(zhǔn)確性與實(shí)時(shí)性。這種數(shù)據(jù)融合方法顯著降低了感知信息的誤差和時(shí)滯，提升了無人車在復(fù)雜環(huán)境下的導(dǎo)航與決策能力。