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

背景技術(shù)：

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

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

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

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

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

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

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

3、基于底盤的對角線兩主動輪、兩從動輪的運動方式，構(gòu)建底盤的二自由度底盤動力學模型；

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

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

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

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

8、在一些可能的設計中，所述底盤的二自由度底盤動力學模型為：

9、

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

11、在一些可能的設計中，所述底盤軌跡跟蹤誤差模型為：

12、

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

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

15、其中，

16、在一些可能的設計中，全向運動底盤的離散化狀態(tài)方程為：xk+1＝adxk+bduk；

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

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

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

20、

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

22、

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

24、r1＝k1vx2+k2vx+k3

25、r2＝k4vx2+k5vx+k6

26、其中，q1為橫向誤差加權(quán)系數(shù)，q2為橫向誤差一階導數(shù)加權(quán)系數(shù)，q3為航向誤差加權(quán)系數(shù)，q4為航向誤差一階導數(shù)加權(quán)系數(shù)，r1為輸出前主動輪轉(zhuǎn)角加權(quán)系數(shù)，r2為輸出后主動輪轉(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)為底盤當前坐標，θ為底盤當前航向角，(xd,yd)為跟蹤目標點坐標，θd為跟蹤目標點航向角，

33、為目標點的切向量,為目標點的法向量，k為目標點曲率，為底盤航向角；

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

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

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

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

38、獲取全向運動底盤當前的航向誤差

39、通過以下公式計算出全向運動底盤裝置的轉(zhuǎn)角調(diào)節(jié)量δδ：

40、

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

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

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

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

45、動力學模型構(gòu)建模塊，用于基于底盤的對角線兩主動輪、兩從動輪的運動方式，構(gòu)建底盤的二自由度底盤動力學模型；

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

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

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

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

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

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

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

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