本發(fā)明涉及無(wú)人駕駛，具體為一種面向移動(dòng)載體安全性的模型預(yù)測(cè)控制方法及系統(tǒng)。

背景技術(shù)：

1、在航站樓、展覽館等大型室內(nèi)環(huán)境中，傳統(tǒng)運(yùn)輸車受制于工作人員的操作，運(yùn)行效率不高。隨著無(wú)人駕駛技術(shù)的發(fā)展，移動(dòng)載體可以有效減少人工成本，提升大型室內(nèi)環(huán)境的運(yùn)行效率。移動(dòng)載體利用差速驅(qū)動(dòng)底盤，具備快速機(jī)動(dòng)性和節(jié)能優(yōu)勢(shì)。然而，大型室內(nèi)環(huán)境的非結(jié)構(gòu)化空間和人群密集特點(diǎn)使得移動(dòng)載體的通行控制非常挑戰(zhàn)。實(shí)時(shí)通行控制是移動(dòng)載體安全運(yùn)送乘客的關(guān)鍵。

2、移動(dòng)載體的通行控制是指在大型室內(nèi)密集人群環(huán)境下，移動(dòng)載體能夠在密集人群中通行，安全避開(kāi)行人等障礙物。在航站樓、展覽館等動(dòng)態(tài)密集人群環(huán)境下，具有安全性、舒適、快捷等約束的通行控制是十分現(xiàn)實(shí)的需求。

3、模型預(yù)測(cè)控制(model?predictive?control,mpc)是一種基于動(dòng)態(tài)模型的優(yōu)化控制方法，它通過(guò)使用系統(tǒng)的數(shù)學(xué)模型來(lái)預(yù)測(cè)未來(lái)一段時(shí)間內(nèi)的系統(tǒng)行為，并在當(dāng)前時(shí)刻選擇最優(yōu)的控制動(dòng)作實(shí)現(xiàn)對(duì)系統(tǒng)的控制。近年來(lái)，模型預(yù)測(cè)控制由于其可以預(yù)測(cè)未來(lái)狀態(tài)、應(yīng)對(duì)動(dòng)態(tài)約束和處理不確定性的優(yōu)勢(shì)，在動(dòng)態(tài)環(huán)境中的規(guī)劃控制研究逐漸加強(qiáng)。使用模型預(yù)測(cè)控制局部規(guī)劃算法的實(shí)時(shí)性和多種約束限制設(shè)計(jì)通行控制方法是一種比較現(xiàn)實(shí)的選擇。

4、但是現(xiàn)有的模型預(yù)測(cè)控制方法技術(shù)實(shí)際使用時(shí)，難以適應(yīng)具有密集人群的大型室內(nèi)環(huán)境，并且未考慮移動(dòng)載體運(yùn)行存在安全性等多種約束問(wèn)題。

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

1、本發(fā)明的目的在于提供一種面向移動(dòng)載體安全性的模型預(yù)測(cè)控制方法及系統(tǒng)，以解決難以適應(yīng)大型室內(nèi)環(huán)境中密集人群的特點(diǎn)，并且未考慮移動(dòng)載體運(yùn)行存在安全性等多種約束問(wèn)題。

2、為實(shí)現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：一種面向移動(dòng)載體安全性的模型預(yù)測(cè)控制方法及系統(tǒng)，包括以下步驟：

3、s1：建立移動(dòng)載體運(yùn)動(dòng)學(xué)模型和安全約束下的距離函數(shù)模型；

4、s2：引入步驟s1的距離函數(shù)模型并設(shè)計(jì)控制障礙函數(shù)，將安全約束轉(zhuǎn)化為集合前向不變性以實(shí)現(xiàn)安全通行區(qū)域，用于針對(duì)障礙物體積不確定性問(wèn)題；

5、s3：依據(jù)步驟s2設(shè)計(jì)的控制障礙函數(shù)，設(shè)計(jì)目標(biāo)代價(jià)函數(shù)和系統(tǒng)約束并實(shí)時(shí)優(yōu)化求解，規(guī)劃移動(dòng)載體未來(lái)時(shí)刻的參考軌跡；

6、s4：根據(jù)步驟s3中的參考軌跡，設(shè)計(jì)分層閉環(huán)控制算法實(shí)現(xiàn)實(shí)時(shí)跟蹤，以使在大型室內(nèi)密集人群環(huán)境下，移動(dòng)載體在密集人群中安全通行，到達(dá)預(yù)設(shè)定的目標(biāo)位姿，實(shí)現(xiàn)移動(dòng)載體通行控制。

7、優(yōu)選的，所述步驟s1建立移動(dòng)載體運(yùn)動(dòng)學(xué)模型和安全約束下的距離函數(shù)模型具體為：

8、s11：建立移動(dòng)載體的運(yùn)動(dòng)學(xué)模型：選取點(diǎn)p＝[x,y,θ]t作為移動(dòng)載體當(dāng)前位姿，x和y是位置坐標(biāo)，θ是朝向角，其移動(dòng)載體運(yùn)動(dòng)學(xué)模型為：

9、

10、其中表示的是移動(dòng)載體當(dāng)前位姿p＝[x,y,θ]t的時(shí)間導(dǎo)數(shù)，即移動(dòng)載體的速度向量；

11、是移動(dòng)載體在x方向上的速度分量，表示載體沿x軸方向的速度變化率；

12、是移動(dòng)載體在y方向上的速度分量，表示載體沿y軸方向的速度變化率；

13、是移動(dòng)載體的角速度分量，表示載體繞其參考點(diǎn)旋轉(zhuǎn)的角速度；

14、v和ω分別是移動(dòng)載體的線速度和角速度；

15、s12：建立安全約束下的距離函數(shù)模型：選取移動(dòng)載體主體部分的最小外接圓作為當(dāng)前時(shí)刻移動(dòng)載體的外輪廓空間，將障礙物表面最小外接圓作為障礙物最小安全膨脹范圍，將移動(dòng)載體的外輪廓空間看作一個(gè)質(zhì)心，由幾何關(guān)系可得：

16、

17、其中，dobs為移動(dòng)載體與障礙物的實(shí)際距離，其中x和y是點(diǎn)p(x,y)的位置坐標(biāo)值，且xobs和yobs是點(diǎn)pobs(xobs,yobs)的位置坐標(biāo)值，p(x,y)和pobs(xobs,yobs)分別是移動(dòng)載體外輪廓空間圓心和障礙物表面最小外接圓圓心的中心位置坐標(biāo)，r1,r2分別是移動(dòng)載體外輪廓空間和障礙物最小安全膨脹范圍的半徑；

18、為保證移動(dòng)載體符合安全性約束，dobs應(yīng)滿足下式：

19、dobs＞safe_dist

20、其中，safe_dist為最小安全距離；

21、定義距離函數(shù)b(p)如下式所示：

22、b(p)＝(x-xobs)2+(y-yobs)2-dsafe2

23、＝λ(dobs-safe_dist)

24、其中，dsafe＝r1+r2+safe_dist，其中dsafe為移動(dòng)載體與障礙物的安全距離；

25、當(dāng)b(p)＞0時(shí)，dobs＞safe_dist，滿足安全性約束條件。

26、優(yōu)選的，所述步驟s2中設(shè)計(jì)控制障礙函數(shù)具體為：

27、

28、其中，kcbf(p)為安全集合，表示控制障礙函數(shù)b(p)的時(shí)間導(dǎo)數(shù)，即關(guān)于位置p的變化率，b(p)為控制障礙函數(shù)，γ＞0，為控制障礙函數(shù)參數(shù)，對(duì)通行控制效果有重要影響，根據(jù)上式可得：

29、b(p)≥e-γp+c＞0

30、其中，c為常數(shù)，無(wú)論位置p如何變化，控制障礙函數(shù)b(p)＞0始終成立，滿足安全約束條件dobs＞safe_dist；

31、對(duì)控制障礙函數(shù)離散化，可得：

32、b(pk+1)-b(pk)+γb(pk)≥0

33、其中k代表離散時(shí)間步的索引；

34、安全約束的控制障礙函數(shù)b(p)＞0始終成立，控制障礙函數(shù)b(p)的安全集合kcbf(p)具有前向不變性，移動(dòng)載體處于安全通行區(qū)域。

35、優(yōu)選的，所述步驟s3中設(shè)計(jì)目標(biāo)代價(jià)函數(shù)和系統(tǒng)約束并實(shí)時(shí)優(yōu)化求解的方法為：

36、s31：為了進(jìn)行模型預(yù)測(cè)控制器的設(shè)計(jì)，對(duì)運(yùn)動(dòng)學(xué)模型離散化，得：

37、

38、為方便書寫，將上式簡(jiǎn)化如下：

39、x(k+1)＝ax(k)+bu(k)

40、其中，a和b是系統(tǒng)矩陣，它們分別代表了系統(tǒng)狀態(tài)在沒(méi)有控制輸入時(shí)的動(dòng)態(tài)變化以及控制輸入對(duì)系統(tǒng)狀態(tài)的影響，t是采樣時(shí)間，x(k)＝[xk,yk,θk]t和x(k+1)＝[xk+1,yk+1,θk+1]t分別是k時(shí)刻及下一時(shí)刻的系統(tǒng)狀態(tài)量，u(k)＝[vk,ωk]t為控制輸入；

41、對(duì)于上述離散模型式，定義k時(shí)刻的系統(tǒng)狀態(tài)預(yù)測(cè)向量與控制輸入預(yù)測(cè)向量分別為：

42、

43、其中預(yù)測(cè)區(qū)間為np，控制區(qū)間為nc，且滿足nc≤np，取nc＝np＝n，設(shè)定到達(dá)期望目標(biāo)點(diǎn)位置的系統(tǒng)狀態(tài)為pn(xn,yn,θn)，則期望值即為最后時(shí)刻的系統(tǒng)狀態(tài)量，即x(n)＝pn(xn,yn,θn)；

44、s32：設(shè)計(jì)帶有安全約束的通行控制算法，將步驟s31中離散模型式調(diào)節(jié)到一個(gè)目標(biāo)最優(yōu)狀態(tài)，即在給定的預(yù)測(cè)區(qū)間n內(nèi)，在安全性的約束下，通過(guò)優(yōu)化控制輸入，使系統(tǒng)的性能指標(biāo)達(dá)到最佳狀態(tài)，規(guī)劃移動(dòng)載體未來(lái)時(shí)刻的參考軌跡，完成移動(dòng)載體在通行途中規(guī)避障礙物的任務(wù)，考慮動(dòng)力學(xué)軟約束，設(shè)計(jì)目標(biāo)代價(jià)函數(shù)，并引入安全約束的控制障礙函數(shù)作為硬約束條件，其中目標(biāo)代價(jià)函數(shù)形式如下：

45、

46、其中，q、r、f分別是狀態(tài)誤差權(quán)重矩陣、控制輸入權(quán)重矩陣和終端誤差權(quán)重矩陣，對(duì)j(x,u)進(jìn)行簡(jiǎn)化，可整理成如下二次規(guī)劃的一般形式：

47、j(uk)＝xktgxk+2xktduk+ukthuk

48、其中，xk是k時(shí)刻初始系統(tǒng)狀態(tài)量，g、d、h分別是各部分權(quán)重矩陣；尋找使性能指標(biāo)j(uk)最小的uk值，在保證移動(dòng)載體安全運(yùn)行的前提下，設(shè)計(jì)的帶有控制障礙函數(shù)的模型預(yù)測(cè)控制器如下所示：

49、

50、s.t.umin≤uk≤umax

51、xmin≤xk≤xmax

52、ymin≤yk≤ymax

53、θmin≤θk≤θmax

54、b(pk+1)-b(pk)+γb(pk)≥0

55、其中，umax和umin為控制量約束的上下界，xmax,ymax,θmax和xmin,ymin,θmin為狀態(tài)量約束的上下界，b(pk+1)-b(pk)+γb(pk)≥0為安全硬約束；

56、s33：只選取控制輸入預(yù)測(cè)向量的第一個(gè)優(yōu)化解作為移動(dòng)載體下一時(shí)刻的參考軌跡，即u(k+1)＝u(k|k)，在下一時(shí)刻k+1時(shí)刻下，重復(fù)k時(shí)刻的操作，根據(jù)新得到的系統(tǒng)狀態(tài)量為初始條件，重新預(yù)測(cè)系統(tǒng)的控制序列并求解優(yōu)化解，然后繼續(xù)將k+1時(shí)刻的第一個(gè)優(yōu)化解的作用于系統(tǒng)，重復(fù)上述過(guò)程；

57、s34：將最優(yōu)化問(wèn)題轉(zhuǎn)化為求解在每個(gè)預(yù)測(cè)周期n的可到達(dá)區(qū)域與可行區(qū)域之間是否有重合的問(wèn)題，使最優(yōu)化問(wèn)題一定可解，最終規(guī)劃出移動(dòng)載體未來(lái)時(shí)刻的參考軌跡。

58、優(yōu)選的，所述步驟s4中設(shè)計(jì)分層閉環(huán)控制算法實(shí)現(xiàn)實(shí)時(shí)跟蹤，具體步驟如下：

59、s41：根據(jù)移動(dòng)載體通行控制任務(wù)需求建立分層的控制系統(tǒng)，高層的模型預(yù)測(cè)控制器根據(jù)移動(dòng)載體當(dāng)前的位姿狀態(tài)實(shí)時(shí)規(guī)劃通行控制軌跡，為低層滑模控制器提供參考軌跡，參考軌跡包括速度，角速度信息；低層滑模控制器依據(jù)移動(dòng)載體當(dāng)前參考軌跡進(jìn)行高頻實(shí)時(shí)控制，可以抑制擾動(dòng)干擾，計(jì)算出期望軌跡，期望軌跡包括擾動(dòng)抑制后的速度、角速度信息；

60、s42：將經(jīng)過(guò)算法優(yōu)化后得到的期望軌跡輸入到移動(dòng)載體的運(yùn)動(dòng)控制組件。

61、本發(fā)明還提供一種面向移動(dòng)載體安全性的模型預(yù)測(cè)控制方法的系統(tǒng)，包括傳感器組件、處理器模塊和運(yùn)動(dòng)控制組件；

62、所述傳感器組件包括激光雷達(dá)和視覺(jué)系統(tǒng)，用于檢測(cè)移動(dòng)載體周圍環(huán)境障礙物位置和距離信息；

63、所述處理器模塊包括安全約束的模型預(yù)測(cè)控制器和滑模控制器，且處理器模塊與傳感器組件和運(yùn)動(dòng)控制組件進(jìn)行通信，能夠?qū)崟r(shí)處理傳感器組件傳遞的數(shù)據(jù)，更新局部地圖，并能實(shí)時(shí)執(zhí)行所述面向移動(dòng)載體安全性的模型預(yù)測(cè)控制方法程序，實(shí)現(xiàn)移動(dòng)載體通行控制；

64、所述運(yùn)動(dòng)控制組件包括驅(qū)動(dòng)移動(dòng)載體通行的電機(jī)和從動(dòng)輪，用于對(duì)所述移動(dòng)載體的運(yùn)動(dòng)進(jìn)行控制。

65、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：

66、1、本發(fā)明通過(guò)將控制障礙函數(shù)與模型預(yù)測(cè)控制相結(jié)合，設(shè)計(jì)目標(biāo)代價(jià)函數(shù)和系統(tǒng)約束并實(shí)時(shí)優(yōu)化求解，從而確保移動(dòng)載體在密集人群場(chǎng)景下安全實(shí)時(shí)通行控制，提高了移動(dòng)載體在復(fù)雜環(huán)境中的適應(yīng)能力，實(shí)現(xiàn)了移動(dòng)載體在大型密集人群環(huán)境中的實(shí)時(shí)通行控制，通過(guò)引入控制障礙函數(shù)設(shè)計(jì)通行安全區(qū)域，將安全約束條件轉(zhuǎn)化為集合前向不變性，將安全約束的控制障礙函數(shù)作為硬約束條件，同時(shí)考慮動(dòng)力學(xué)軟約束，建立目標(biāo)代價(jià)函數(shù)并進(jìn)行優(yōu)化求解，實(shí)現(xiàn)移動(dòng)載體平穩(wěn)、安全地通過(guò)障礙物，在人群密集的航站樓等大型室內(nèi)環(huán)境中也是如此，具有十分重要的研究前景。