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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

9、

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

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

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

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

14、v和ω分別是移動載體的線速度和角速度；

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

16、

17、其中，dobs為移動載體與障礙物的實(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)分別是移動載體外輪廓空間圓心和障礙物表面最小外接圓圓心的中心位置坐標(biāo)，r1,r2分別是移動載體外輪廓空間和障礙物最小安全膨脹范圍的半徑；

18、為保證移動載體符合安全性約束，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為移動載體與障礙物的安全距離；

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

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

27、

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

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

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

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

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

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

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

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

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

37、

38、為方便書寫，將上式簡化如下：

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

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

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

42、

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

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

45、

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

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

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

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ù)測向量的第一個優(yōu)化解作為移動載體下一時刻的參考軌跡，即u(k+1)＝u(k|k)，在下一時刻k+1時刻下，重復(fù)k時刻的操作，根據(jù)新得到的系統(tǒng)狀態(tài)量為初始條件，重新預(yù)測系統(tǒng)的控制序列并求解優(yōu)化解，然后繼續(xù)將k+1時刻的第一個優(yōu)化解的作用于系統(tǒng)，重復(fù)上述過程；

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

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

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

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

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

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

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

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

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

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