本技術(shù)涉及自動(dòng)駕駛，尤其涉及一種全向運(yùn)動(dòng)底盤(pán)裝置及其軌跡跟蹤控制方法。

背景技術(shù)：

1、隨著人工智能等技術(shù)的發(fā)展，自動(dòng)駕駛的相關(guān)研究也在不斷推進(jìn)。自動(dòng)駕駛技術(shù)作為未來(lái)社會(huì)發(fā)展中的一項(xiàng)重要技術(shù)，被廣泛應(yīng)用在運(yùn)輸物流、交通出行等方面。

2、近些年來(lái)汽車(chē)保有量的不斷增加，帶來(lái)了日益嚴(yán)重的交通堵塞和安全問(wèn)題，自動(dòng)駕駛技術(shù)正是這些問(wèn)題的一種有效解決方法。同時(shí)自動(dòng)駕駛產(chǎn)業(yè)也是當(dāng)前國(guó)家提升智能化發(fā)展水平的一個(gè)重要節(jié)點(diǎn)，自動(dòng)駕駛技術(shù)的研究推進(jìn)符合國(guó)家發(fā)展戰(zhàn)略的需要。但是近些年來(lái)自動(dòng)駕駛引起的交通事故嚴(yán)重影響了自動(dòng)駕駛的發(fā)展和落地速度，對(duì)自動(dòng)駕駛是否安全可靠的懷疑是自動(dòng)駕駛落地的主要阻力，而自動(dòng)駕駛測(cè)試正是證明自動(dòng)駕駛是否安全可靠的有效手段。

3、由于測(cè)試真實(shí)人車(chē)預(yù)碰撞和車(chē)車(chē)預(yù)碰撞等情況的危險(xiǎn)性和高成本，自動(dòng)駕駛測(cè)試引入了自動(dòng)駕駛底盤(pán)裝置，測(cè)試人員通過(guò)在底盤(pán)裝置上放置假人模型和車(chē)輛模型并通過(guò)控制方法控制底盤(pán)移動(dòng)可對(duì)現(xiàn)實(shí)中的各種人、車(chē)的移動(dòng)情況進(jìn)行模擬，在降低危險(xiǎn)性和成本的同時(shí)完成自動(dòng)駕駛測(cè)試的目的及要求。

4、相關(guān)自動(dòng)駕駛測(cè)試技術(shù)中，全向運(yùn)動(dòng)的自動(dòng)駕駛底盤(pán)裝置主要為四驅(qū)四轉(zhuǎn)、前驅(qū)兩轉(zhuǎn)和后驅(qū)兩轉(zhuǎn)類(lèi)型。其中，前驅(qū)兩轉(zhuǎn)為前面兩輪有電機(jī)驅(qū)動(dòng)且主動(dòng)轉(zhuǎn)向，后面兩輪為從動(dòng)輪；后驅(qū)兩轉(zhuǎn)為后面兩輪有電機(jī)驅(qū)動(dòng)且主動(dòng)轉(zhuǎn)向，前面兩輪為從動(dòng)輪。在這些底盤(pán)中，四驅(qū)四轉(zhuǎn)底盤(pán)控制精度較高但控制方法復(fù)雜；前驅(qū)兩轉(zhuǎn)底盤(pán)和后驅(qū)兩轉(zhuǎn)底盤(pán)控制方法較為簡(jiǎn)單但不利于平衡底盤(pán)的負(fù)荷和牽引力。

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

1、為了解決相關(guān)技術(shù)中四驅(qū)四轉(zhuǎn)底盤(pán)控制復(fù)雜和前驅(qū)兩轉(zhuǎn)、后驅(qū)兩轉(zhuǎn)底盤(pán)控制不利于平衡底盤(pán)的負(fù)荷和牽引力的技術(shù)問(wèn)題，本技術(shù)提供一種對(duì)角線(xiàn)兩主動(dòng)輪、兩從動(dòng)輪的運(yùn)動(dòng)方式的全向運(yùn)動(dòng)底盤(pán)及其軌跡跟蹤控制方法。

2、一方面，本技術(shù)提供一種全向運(yùn)動(dòng)底盤(pán)的軌跡跟蹤控制方法，所述控制方法包括：

3、基于底盤(pán)的對(duì)角線(xiàn)兩主動(dòng)輪、兩從動(dòng)輪的運(yùn)動(dòng)方式，構(gòu)建底盤(pán)的二自由度底盤(pán)動(dòng)力學(xué)模型；

4、根據(jù)底盤(pán)目標(biāo)路徑和跟蹤的實(shí)際路徑的幾何關(guān)系，對(duì)所述底盤(pán)動(dòng)力學(xué)模型進(jìn)行轉(zhuǎn)換，得到底盤(pán)軌跡跟蹤誤差模型，根據(jù)所述底盤(pán)軌跡跟蹤誤差模型建立離散的狀態(tài)方程和lqr的動(dòng)態(tài)代價(jià)函數(shù)，根據(jù)離散的狀態(tài)方程和lqr的動(dòng)態(tài)代價(jià)函數(shù)建立黎卡提方程；

5、獲取底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息和目標(biāo)路徑，根據(jù)所述底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息對(duì)所述黎卡提方程進(jìn)行求解，根據(jù)所述底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息和目標(biāo)路徑得到跟蹤誤差，根據(jù)黎卡提方程求解得到的系數(shù)和跟蹤誤差得到底盤(pán)的驅(qū)動(dòng)輪的初步轉(zhuǎn)角控制量；

6、獲取底盤(pán)當(dāng)前的航向誤差，采用所述底盤(pán)當(dāng)前的航向誤差對(duì)所述底盤(pán)的初步轉(zhuǎn)角控制量進(jìn)行pid調(diào)節(jié)，得到調(diào)節(jié)后的轉(zhuǎn)角控制量；

7、根據(jù)所述調(diào)節(jié)后的轉(zhuǎn)角控制量對(duì)底盤(pán)進(jìn)行軌跡跟蹤控制。

8、在一些可能的設(shè)計(jì)中，所述底盤(pán)的二自由度底盤(pán)動(dòng)力學(xué)模型為：

9、

10、其中，vx為底盤(pán)的運(yùn)行速度，為底盤(pán)的橫向位移加速度，為底盤(pán)的航向角加速度，m為底盤(pán)總質(zhì)量，im為底盤(pán)繞過(guò)質(zhì)心所在垂線(xiàn)的轉(zhuǎn)動(dòng)慣量，cf為底盤(pán)兩個(gè)前輪的側(cè)偏剛度之和；cr為底盤(pán)兩個(gè)后輪的側(cè)偏剛度之和，δf為底盤(pán)的前主動(dòng)輪轉(zhuǎn)角；δr為底盤(pán)的后主動(dòng)輪轉(zhuǎn)角，a為底盤(pán)的兩個(gè)前輪中心連接線(xiàn)的中點(diǎn)到質(zhì)心的距離，b為底盤(pán)的兩個(gè)后輪中心連接線(xiàn)的中點(diǎn)到質(zhì)心的距離，為底盤(pán)的橫向位移速度，為底盤(pán)的航向角速度。

11、在一些可能的設(shè)計(jì)中，所述底盤(pán)軌跡跟蹤誤差模型為：

12、

13、其中，ed為橫向誤差，為橫向誤差關(guān)于時(shí)間的一階導(dǎo)數(shù)；為橫向誤差關(guān)于時(shí)間的二階導(dǎo)數(shù)；為航向誤差，為航向誤差關(guān)于時(shí)間的一階導(dǎo)數(shù)；為航向誤差關(guān)于時(shí)間的二階導(dǎo)數(shù)；

14、將所述底盤(pán)軌跡跟蹤誤差模型轉(zhuǎn)換為：

15、其中，

16、在一些可能的設(shè)計(jì)中，全向運(yùn)動(dòng)底盤(pán)的離散化狀態(tài)方程為：xk+1＝adxk+bduk；

17、其中，bd＝bdt,i為單位矩陣，dt為小時(shí)間量，k為離散步長(zhǎng),xk+1為x在k+1時(shí)刻的值，xk為x在k時(shí)刻的值，uk為u在k時(shí)刻的值。

18、在一些可能的設(shè)計(jì)中，所述根據(jù)所述底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息對(duì)所述黎卡提方程進(jìn)行求解，根據(jù)所述底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息和目標(biāo)路徑得到跟蹤誤差，根據(jù)黎卡提方程求解得到的系數(shù)和跟蹤誤差得到底盤(pán)的驅(qū)動(dòng)輪的初步轉(zhuǎn)角控制量，包括：

19、設(shè)置lqr的動(dòng)態(tài)代價(jià)函數(shù)為：

20、

21、其中，q和r為lqr的狀態(tài)加權(quán)矩陣和動(dòng)態(tài)輸出加權(quán)矩陣,n為終點(diǎn)時(shí)刻；

22、

23、固定q矩陣的系數(shù)，對(duì)r矩陣進(jìn)行動(dòng)態(tài)調(diào)節(jié)，其中r1和r2可以表示為：

24、r1＝k1vx2+k2vx+k3

25、r2＝k4vx2+k5vx+k6

26、其中，q1為橫向誤差加權(quán)系數(shù)，q2為橫向誤差一階導(dǎo)數(shù)加權(quán)系數(shù)，q3為航向誤差加權(quán)系數(shù)，q4為航向誤差一階導(dǎo)數(shù)加權(quán)系數(shù)，r1為輸出前主動(dòng)輪轉(zhuǎn)角加權(quán)系數(shù)，r2為輸出后主動(dòng)輪轉(zhuǎn)角加權(quán)系數(shù)；k1,k2,k3,k4,k5,k6為調(diào)節(jié)系數(shù)；

27、建立黎卡提方程為：

28、求解跟蹤誤差:

29、ed＝ntderr

30、es＝τtderr

31、

32、其中，(x,y)為底盤(pán)當(dāng)前坐標(biāo)，θ為底盤(pán)當(dāng)前航向角，(xd,yd)為跟蹤目標(biāo)點(diǎn)坐標(biāo)，θd為跟蹤目標(biāo)點(diǎn)航向角，

33、為目標(biāo)點(diǎn)的切向量,為目標(biāo)點(diǎn)的法向量，k為目標(biāo)點(diǎn)曲率，為底盤(pán)航向角；

34、所述全向運(yùn)動(dòng)底盤(pán)裝置的驅(qū)動(dòng)輪的初步轉(zhuǎn)角控制量為：u＝-kx；

35、上式中，k＝(r+btpb)-1bpa；

36、上式中，p＝q+atp(i+br-1btp)-1a。

37、在一些可能的設(shè)計(jì)中，所述采用所述底盤(pán)當(dāng)前的航向誤差對(duì)所述底盤(pán)的初步轉(zhuǎn)角控制量進(jìn)行pid調(diào)節(jié)，得到調(diào)節(jié)后的轉(zhuǎn)角控制量，包括：

38、獲取全向運(yùn)動(dòng)底盤(pán)當(dāng)前的航向誤差

39、通過(guò)以下公式計(jì)算出全向運(yùn)動(dòng)底盤(pán)裝置的轉(zhuǎn)角調(diào)節(jié)量δδ：

40、

41、根據(jù)轉(zhuǎn)角調(diào)節(jié)量δδ得到兩個(gè)主動(dòng)輪的調(diào)節(jié)后的轉(zhuǎn)角控制量分別為：

42、δ1＝δf-δδ，δ2＝δr+δδ；

43、其中，kp為比例系數(shù)，ki為積分系數(shù)，kd為微分系數(shù)，δδ為調(diào)節(jié)量，δ1為前主動(dòng)輪轉(zhuǎn)角，δ2為后主動(dòng)輪轉(zhuǎn)角。

44、另一方面，本技術(shù)還提供一種全向運(yùn)動(dòng)底盤(pán)裝置，所述全向運(yùn)動(dòng)底盤(pán)裝置包括：

45、動(dòng)力學(xué)模型構(gòu)建模塊，用于基于底盤(pán)的對(duì)角線(xiàn)兩主動(dòng)輪、兩從動(dòng)輪的運(yùn)動(dòng)方式，構(gòu)建底盤(pán)的二自由度底盤(pán)動(dòng)力學(xué)模型；

46、軌跡跟蹤模型構(gòu)建模塊，用于根據(jù)底盤(pán)目標(biāo)路徑和跟蹤的實(shí)際路徑的幾何關(guān)系，對(duì)所述底盤(pán)動(dòng)力學(xué)模型進(jìn)行轉(zhuǎn)換，得到底盤(pán)軌跡跟蹤誤差模型，根據(jù)所述底盤(pán)軌跡跟蹤誤差模型建立離散的狀態(tài)方程和lqr的動(dòng)態(tài)代價(jià)函數(shù)，根據(jù)離散的狀態(tài)方程和lqr的動(dòng)態(tài)代價(jià)函數(shù)建立黎卡提方程；

47、轉(zhuǎn)角控制量計(jì)算模塊，用于獲取底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息和目標(biāo)路徑，根據(jù)所述底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息對(duì)所述黎卡提方程進(jìn)行求解，根據(jù)所述底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息和目標(biāo)路徑得到跟蹤誤差，根據(jù)黎卡提方程求解得到的系數(shù)和跟蹤誤差得到底盤(pán)的驅(qū)動(dòng)輪的初步轉(zhuǎn)角控制量；

48、轉(zhuǎn)角控制量調(diào)節(jié)模塊，用于獲取底盤(pán)當(dāng)前的航向誤差，采用所述底盤(pán)當(dāng)前的航向誤差對(duì)所述底盤(pán)的初步轉(zhuǎn)角控制量進(jìn)行pid調(diào)節(jié)，得到調(diào)節(jié)后的轉(zhuǎn)角控制量；

49、跟蹤控制模塊，用于根據(jù)所述調(diào)節(jié)后的轉(zhuǎn)角控制量對(duì)底盤(pán)進(jìn)行軌跡跟蹤控制。

50、另一方面，本技術(shù)還提供一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其存儲(chǔ)有計(jì)算機(jī)程序，所述計(jì)算機(jī)程序在處理器上運(yùn)行時(shí)執(zhí)行如上所述的全向運(yùn)動(dòng)底盤(pán)的軌跡跟蹤控制方法。

51、另一方面，本技術(shù)還提供一種計(jì)算機(jī)程序產(chǎn)品，所述計(jì)算機(jī)程序產(chǎn)品包括：計(jì)算機(jī)程序代碼，當(dāng)所述計(jì)算機(jī)程序代碼在計(jì)算機(jī)上運(yùn)行時(shí)，使得所述計(jì)算機(jī)執(zhí)行如上所述的全向運(yùn)動(dòng)底盤(pán)的軌跡跟蹤控制方法。

52、依據(jù)本技術(shù)提供的全向運(yùn)動(dòng)底盤(pán)的軌跡跟蹤控制方法，主要用于對(duì)角線(xiàn)兩主動(dòng)輪、兩從動(dòng)輪的運(yùn)動(dòng)方式的全向運(yùn)動(dòng)底盤(pán)，首先基于底盤(pán)的對(duì)角線(xiàn)兩主動(dòng)輪、兩從動(dòng)輪的運(yùn)動(dòng)方式，構(gòu)建底盤(pán)的二自由度底盤(pán)動(dòng)力學(xué)模型，根據(jù)底盤(pán)目標(biāo)路徑和跟蹤的實(shí)際路徑的幾何關(guān)系，對(duì)底盤(pán)動(dòng)力學(xué)模型進(jìn)行轉(zhuǎn)換，得到底盤(pán)軌跡跟蹤誤差模型，根據(jù)底盤(pán)軌跡跟蹤誤差模型建立離散的狀態(tài)方程和lqr的動(dòng)態(tài)代價(jià)函數(shù)，根據(jù)離散的狀態(tài)方程和lqr的動(dòng)態(tài)代價(jià)函數(shù)建立黎卡提方程；獲取底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息和目標(biāo)路徑，根據(jù)底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息對(duì)黎卡提方程進(jìn)行求解，根據(jù)底盤(pán)的當(dāng)前運(yùn)動(dòng)狀態(tài)信息和目標(biāo)路徑得到跟蹤誤差，根據(jù)黎卡提方程求解得到的系數(shù)和跟蹤誤差得到底盤(pán)的驅(qū)動(dòng)輪的初步轉(zhuǎn)角控制量；獲取底盤(pán)當(dāng)前的航向誤差，采用底盤(pán)當(dāng)前的航向誤差對(duì)底盤(pán)的初步轉(zhuǎn)角控制量進(jìn)行pid調(diào)節(jié)，得到調(diào)節(jié)后的轉(zhuǎn)角控制量；根據(jù)調(diào)節(jié)后的轉(zhuǎn)角控制量對(duì)底盤(pán)進(jìn)行軌跡跟蹤控制，這樣提高了對(duì)底盤(pán)的控制精度。

53、本技術(shù)在上述各方面提供的實(shí)現(xiàn)方式的基礎(chǔ)上，還可以進(jìn)行進(jìn)一步組合以提供更多實(shí)現(xiàn)方式。