本發(fā)明涉及自動駕駛領(lǐng)域的橫縱向控制方法，具體涉及應(yīng)用于自主泊車和行車過程中的基于非線性模型預(yù)測控制(nonlinear?model?predictive?control，nmpc)的一種車輛縱橫向控制方法、系統(tǒng)、存儲介質(zhì)及車輛。

背景技術(shù)：

1、模型預(yù)測控制(model?predictive?control，mpc)是自動駕駛領(lǐng)域常用的控制算法之一，常用于泊車、行車等場景的車輛橫縱向控制。大多數(shù)mpc求解步驟是利用當(dāng)前車輛狀態(tài)與參考軌跡構(gòu)造誤差模型，然后對誤差模型進(jìn)行線性化、離散化后構(gòu)成二次型代價函數(shù)；在設(shè)置控制量約束后，求解代價函數(shù)獲得一系列控制增量，并將序列的第一個控制增量作用于控制對象。下一周期重復(fù)以上步驟，由此形成對車輛橫縱向的可持續(xù)控制。

2、由于在預(yù)測時域內(nèi)，mpc基于第一個誤差狀態(tài)對后續(xù)的狀態(tài)進(jìn)行線性預(yù)測，其對誤差模型進(jìn)行線性化處理會造成一定程度的精度損失；另外，在泊車等連續(xù)大曲率場景下，運動學(xué)模型在應(yīng)對不連續(xù)的大曲率變化時，會存在導(dǎo)致方向盤抖動和轉(zhuǎn)向不足等問題。

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

1、本發(fā)明所要解決的技術(shù)問題在于，提供一種車輛縱橫向控制方法、系統(tǒng)、存儲介質(zhì)及車輛，可以提高車輛縱橫向控制的平滑性和穩(wěn)定性，避免方向盤抖動和轉(zhuǎn)向不足等問題的出現(xiàn)。

2、為解決上述技術(shù)問題，作為本發(fā)明的一方面，提供一種車輛縱橫向控制方法，其至少包括如下步驟：

3、接收被控車輛的決策規(guī)劃軌跡，以及被控車輛的實際車輛狀態(tài)；

4、根據(jù)決策規(guī)劃軌跡、車輛狀態(tài)以及當(dāng)前車輛動態(tài)誤差，在軌跡擬合線上選取多個參考軌跡點，獲得本預(yù)測周期內(nèi)的實際參考軌跡，所述車輛動態(tài)誤差為車輛當(dāng)前時刻的實際參考軌跡與預(yù)測軌跡信息的偏差，其至少包括車輛橫向誤差；

5、根據(jù)所述實際參考軌跡、預(yù)設(shè)的代價函數(shù)對非線性車輛預(yù)測模型進(jìn)行滾動優(yōu)化，獲得預(yù)測時域內(nèi)最優(yōu)控制量序列；

6、根據(jù)所述最優(yōu)控制量序列控制所述被控車輛。

7、其中，所述預(yù)設(shè)的代價函數(shù)中至少包含有可調(diào)節(jié)權(quán)重的橫向偏移誤差、航向誤差；

8、所述方法進(jìn)一步包括：

9、獲得上一預(yù)測周期的動態(tài)誤差，當(dāng)動態(tài)誤差中車輛橫向誤差大于第三閾值時，在本預(yù)測周期內(nèi)增大所述代價函數(shù)中的橫向誤差的權(quán)重，并同時減少航向誤差的權(quán)重；

10、當(dāng)動態(tài)誤差中車輛橫向誤差小于第四閾值時，在本預(yù)測周期內(nèi)增加所述代價函數(shù)中的航向誤差的權(quán)重，并同時減少橫向誤差的權(quán)重；

11、其中，所述第四閾值小于所述第三閾值。

12、其中，所述根據(jù)決策規(guī)劃軌跡、車輛狀態(tài)以及當(dāng)前車輛動態(tài)誤差，在軌跡擬合線上選取多個參考軌跡點，獲得本預(yù)測周期內(nèi)的實際參考軌跡，進(jìn)一步包括：

13、對所述決策規(guī)劃軌跡進(jìn)行末端的延長處理；

14、根據(jù)車輛動態(tài)誤差，確定參考軌跡點中相鄰軌跡點之間的距離；

15、根據(jù)車輛當(dāng)前速度和循環(huán)時間，分別計算預(yù)測時域內(nèi)每一時刻車輛行駛的距離，選取多個參考軌跡點，獲得本預(yù)測周期內(nèi)的實際參考軌跡。

16、其中，所述根據(jù)車輛動態(tài)誤差，確定參考軌跡點中相鄰軌跡點之間的距離，具體為：

17、獲得上一預(yù)測周期的動態(tài)誤差，當(dāng)動態(tài)誤差中的車輛橫向誤差大于第一閾值時，減少動態(tài)調(diào)整因子的值，以在本預(yù)測周期內(nèi)縮小所選取的參考點中相鄰參考點之間的距離；

18、當(dāng)動態(tài)誤差中的車輛橫向誤差小于第二閾值時，增大動態(tài)調(diào)整因子的值，以在本預(yù)測周期內(nèi)擴(kuò)大所選取的參考點中相鄰參考點之間的距離；

19、其中，所述第二閾值小于所述第一閾值。

20、其中，所述根據(jù)所述實際參考軌跡、預(yù)設(shè)的代價函數(shù)對非線性車輛預(yù)測模型進(jìn)行滾動優(yōu)化，獲得預(yù)測時域內(nèi)最優(yōu)控制量序列，進(jìn)一步包括：構(gòu)造基于非線性模型預(yù)測控制的車輛預(yù)測模型，并獲得被控車輛的預(yù)測軌跡；包括：

21、采用下述的公式，計算獲得預(yù)測周期內(nèi)的每一時刻的預(yù)測軌跡點信息；

22、

23、

24、

25、vk+1＝vk+ak*dt

26、

27、

28、其中，xk為本預(yù)測周期內(nèi)k時刻車輛橫坐標(biāo)值；yk為k時刻車輛縱坐標(biāo)值；為k時刻車輛航向角；vk為k時刻車輛速度；σk為k時刻車輛前輪角度；ak為k時刻車輛角速度；ref-xk為k時刻參考軌跡橫坐標(biāo)；ref-yk為k時刻車輛參考軌跡縱坐標(biāo)；為k時刻車輛參考航向；xk+1為k+1時刻車輛橫坐標(biāo)值；yk+1為k+1時刻車輛縱坐標(biāo)值；為k+1時刻車輛航向角；vk+1為k+1時刻車輛速度；ctek+1為k+1時刻車輛橫向誤差；epsik+1為k+1時刻車輛航向誤差。

29、其中，所述預(yù)設(shè)的代價函數(shù)為：

30、

31、其中，表示預(yù)測值與參考軌跡點上曲率值對應(yīng)的前輪角度的偏差；

32、表示引入上一時刻的控制量計算結(jié)果；

33、表示加入約束橫向誤差增量變化率的權(quán)重；

34、分別表示橫向偏移誤差、航向誤差和速度誤差，λ用于動態(tài)調(diào)節(jié)橫向誤差和航向誤差的權(quán)重；

35、表示前輪角度和加速度等控制量；

36、表示前輪轉(zhuǎn)角和加速度的增量；

37、其中，所述約束條件為：

38、σ∈[σmin，σmax]

39、a∈[amin，amax]。

40、其中，所述根據(jù)所述實際參考軌跡、預(yù)設(shè)的代價函數(shù)對非線性車輛預(yù)測模型進(jìn)行滾動優(yōu)化，獲得預(yù)測時域內(nèi)最優(yōu)控制量序列，進(jìn)一步包括：

41、在k時刻，將代價函數(shù)和約束條件代入非線性求解器中進(jìn)行求解計算，計算獲得預(yù)測時域內(nèi)最優(yōu)控制量序列，其至少包括控制時域下各個時刻的車輛前輪轉(zhuǎn)角角度；

42、將所述最優(yōu)控制量序列中第一個控制量作用于被控車輛；

43、在k+1時刻重復(fù)以上步驟，由此形成對車輛橫縱向的可持續(xù)控制。

44、本發(fā)明的另一方面，還提供一種車輛縱橫向控制系統(tǒng)，其至少包括：

45、實時狀態(tài)獲取單元，用于接收被控車輛的決策規(guī)劃軌跡，以及被控車輛的實際車輛狀態(tài)；

46、參考軌跡獲取單元，用于根據(jù)決策規(guī)劃軌跡、車輛狀態(tài)以及當(dāng)前車輛動態(tài)誤差，在軌跡擬合線上選取多個參考軌跡點，獲得本預(yù)測周期內(nèi)的實際參考軌跡，所述車輛動態(tài)誤差為車輛當(dāng)前時刻的實際參考軌跡與預(yù)測軌跡信息的偏差，其至少包括車輛橫向誤差；

47、滾動優(yōu)化求解單元，用于根據(jù)所述實際參考軌跡、預(yù)設(shè)的代價函數(shù)對非線性車輛預(yù)測模型進(jìn)行滾動優(yōu)化，獲得預(yù)測時域內(nèi)最優(yōu)控制量序列；

48、控制處理單元，用于根據(jù)所述最優(yōu)控制量序列控制所述被控車輛。

49、其中，進(jìn)一步包括：

50、相鄰距離系數(shù)調(diào)節(jié)單元，用于根據(jù)車輛動態(tài)誤差，確定參考軌跡點中相鄰軌跡點之間的距離；具體地，當(dāng)上一預(yù)測周期的動態(tài)誤差中的車輛橫向誤差大于第一閾值時，在本預(yù)測周期內(nèi)縮小所選取的參考點中相鄰參考點之間的距離；當(dāng)所述動態(tài)誤差中的車輛橫向誤差小于第二閾值時，在本預(yù)測周期內(nèi)擴(kuò)大所選取的參考點中相鄰參考點之間的距離；

51、權(quán)重系數(shù)調(diào)節(jié)單元，用于獲得上一預(yù)測周期的動態(tài)誤差，當(dāng)動態(tài)誤差中車輛橫向誤差大于第三閾值時，在本預(yù)測周期內(nèi)增加代價函數(shù)中的橫向誤差的權(quán)重，并同時減少航向誤差的權(quán)重；當(dāng)動態(tài)誤差中車輛橫向誤差小于第四閾值時，在本預(yù)測周期內(nèi)增加代價函數(shù)中航向誤差的權(quán)重，并同時減少橫向誤差的權(quán)重；

52、其中，所述第二閾值小于所述第一閾值；所述第四閾值小于所述第三閾值。

53、其中，進(jìn)一步包括：

54、預(yù)測軌跡獲取單元，預(yù)測軌跡獲取單元，用于構(gòu)造基于非線性模型預(yù)測控制的車輛預(yù)測模型，并獲得被控車輛的預(yù)測軌跡；

55、具體地，所述預(yù)測軌跡獲取單元用于采用下述的公式，計算獲得預(yù)測周期內(nèi)的每一時刻的預(yù)測軌跡點信息：

56、

57、

58、

59、vk+1＝vk+ak*dt

60、

61、

62、其中，xk為本預(yù)測周期內(nèi)k時刻車輛橫坐標(biāo)值；yk為k時刻車輛縱坐標(biāo)值；為k時刻車輛航向角；vk為k時刻車輛速度；σk為k時刻車輛前輪角度；ak為k時刻車輛角速度；ref-xk為k時刻參考軌跡橫坐標(biāo)；ref-yk為k時刻車輛參考軌跡縱坐標(biāo)；為k時刻車輛參考航向；xk+1為k+1時刻車輛橫坐標(biāo)值；yk+1為k+1時刻車輛縱坐標(biāo)值；為k+1時刻車輛航向角；vk+1為k+1時刻車輛速度；ctek+1為k+1時刻車輛橫向誤差；epsik+1為k+1時刻車輛航向誤差。

63、其中，進(jìn)一步包括代價函數(shù)及約束設(shè)置單元，用于設(shè)置代價函數(shù)及約束條件，所述代價函數(shù)中至少包含有可調(diào)節(jié)權(quán)重的橫向偏移誤差、航向誤差；

64、具體地，在所述代價函數(shù)及約束設(shè)置單元，其所設(shè)置的橫向控制過程中代價函數(shù)如下：

65、

66、其中，表示預(yù)測值與參考軌跡點上曲率值對應(yīng)的前輪角度的偏差；

67、表示引入上一時刻的控制量計算結(jié)果；

68、表示加入約束橫向誤差增量變化率的權(quán)重；

69、分別表示橫向偏移誤差、航向誤差和速度誤差，λ用于動態(tài)調(diào)節(jié)橫向誤差和航向誤差的權(quán)重；

70、表示前輪角度和加速度等控制量；

71、表示前輪轉(zhuǎn)角和加速度的增量；

72、其中，所述約束條件為：

73、σ∈[σminσmax]

74、a∈[amin，amax]。

75、本發(fā)明的再一方面，還提供一種計算機(jī)可讀存儲介質(zhì)，其上存儲有計算機(jī)程序，所述計算機(jī)程序被處理器執(zhí)行時實現(xiàn)如前述的方法的步驟。

76、本發(fā)明的又一方面，還提供一種計算裝置，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機(jī)程序，所述處理器執(zhí)行所述計算機(jī)程序時實現(xiàn)如前述方法的步驟。

77、本發(fā)明的另一方面，還提供一種車輛，其上至少設(shè)置有變道決策裝置，在所述變道決策裝置中部署有如前述的系統(tǒng)。

78、實施本發(fā)明實施例，具有如下的有益效果：

79、在本發(fā)明提供的方法中，被控車輛在接收到規(guī)劃軌跡后，對軌跡進(jìn)行末端軌跡延長，以及根據(jù)車輛誤差在軌跡擬合線上選取合適的參考軌跡點，作為實際參考軌跡用于構(gòu)造基于非線性模型預(yù)測控制(npmc)的車輛預(yù)測模型；同時在設(shè)置控制量約束條件后，通過滾動優(yōu)化求解代價函數(shù)獲得一系列控制量，將第一個控制量作用于被控車輛；下一周期重復(fù)以上步驟，從而形成對車輛橫縱向的可持續(xù)控制。

80、在本發(fā)明的實施例中，使用了非線性模型進(jìn)行模型預(yù)測控制(npmc)，可以避免對模型線性化會產(chǎn)生精度損失；同時，在代價函數(shù)中引入?yún)⒖架壽E點曲率及其動態(tài)轉(zhuǎn)向不足修正因子、上一時刻控制量與當(dāng)前預(yù)測量偏差并設(shè)置合理的權(quán)重，解決連續(xù)大曲率轉(zhuǎn)彎場景下方向盤抖動和轉(zhuǎn)向不足等問題。

81、更具體地，在本發(fā)明中，通過設(shè)置一個用于轉(zhuǎn)向不足時的動態(tài)調(diào)整因子，調(diào)整參考點之間的間距，可以根據(jù)車輛狀態(tài)和誤差動態(tài)選取參考軌跡點，使選擇的參考軌跡點更符合nmpc模型預(yù)測的狀態(tài)，從而優(yōu)化求解最合適的控制量去減小誤差。

82、通過設(shè)置一個動態(tài)誤差控制因子，可以動態(tài)調(diào)整不同誤差的權(quán)重，當(dāng)橫向誤差和航向誤差同時存在時，優(yōu)先考慮減小橫向誤差；而橫向誤差較小后，再考慮修正航向誤差，由此快速完成參考軌跡點跟隨控制。

83、同時，在代價函數(shù)增加了參考點上的曲率作為前饋加入，充分利用參考軌跡點的信息，提高控制器的響應(yīng)速度，使得連續(xù)大曲率轉(zhuǎn)彎場景下nmpc計算的控制量也能讓方向盤快速響應(yīng)，從而保持較小的車輛誤差。

84、而且，在代價函數(shù)中引入了上一時刻控制量與當(dāng)前預(yù)測值的偏差，使nmpc計算值保持連續(xù)性，避免現(xiàn)有技術(shù)中由于控制量前后時刻變化太大而導(dǎo)致方向盤抖動的情形發(fā)生。