本發(fā)明涉及一種多軸重載鉸接車輛的軌跡跟蹤控制方法及裝置，屬于無人駕駛領(lǐng)域中的軌跡跟蹤控制領(lǐng)域。
背景技術(shù)：
1、在現(xiàn)代工業(yè)運(yùn)輸領(lǐng)域，多軸重載鉸接車輛憑借其承載能力強(qiáng)、運(yùn)輸效率高、環(huán)境適應(yīng)性好等優(yōu)點(diǎn)，成為在礦區(qū)、隧道、港口等大型工業(yè)園區(qū)中不可或缺的運(yùn)輸工具。同時(shí)隨著無人駕駛技術(shù)的不斷突破，多軸重載鉸接車也正朝著智能化發(fā)展，其中軌跡跟蹤技術(shù)是實(shí)現(xiàn)無人駕駛的關(guān)鍵技術(shù)，在乘用車上已經(jīng)廣泛應(yīng)用，但多軸重載鉸接車在實(shí)際行駛過程中，由于其質(zhì)量大、車身長、結(jié)構(gòu)復(fù)雜，使用基于傳統(tǒng)的模型預(yù)測(cè)控制(model?predictivecontrol，mpc)以及線性二次型調(diào)節(jié)器(linear?quuadratic?regulator，lqr)等經(jīng)典控制算法時(shí)，會(huì)導(dǎo)致軌跡跟蹤時(shí)實(shí)際行駛軌跡與參考軌跡存在較大偏差，甚至出現(xiàn)不穩(wěn)定現(xiàn)象，如側(cè)翻、打滑等，影響穩(wěn)定性和安全性。因此，亟需一種多軸重載鉸接車輛的軌跡跟蹤控制方法來解決現(xiàn)有技術(shù)在多軸重載鉸接車輛上軌跡跟蹤時(shí)誤差較大，行駛過程中穩(wěn)定性較低的問題。
技術(shù)實(shí)現(xiàn)思路
1、本發(fā)明的目的是提供一種多軸重載鉸接車輛的軌跡跟蹤控制方法及裝置，用以現(xiàn)有技術(shù)在多軸重載鉸接車輛上軌跡跟蹤時(shí)誤差較大，行駛過程中穩(wěn)定性較低的問題。
2、為實(shí)現(xiàn)上述目的，本發(fā)明的方案包括：
3、本發(fā)明的一種多軸重載鉸接車輛的軌跡跟蹤控制方法，包括如下步驟：
4、步驟1)、構(gòu)建多軸重載鉸接車輛的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)耦合模型；
5、步驟2)、基于耦合模型，利用分層無跡卡爾曼濾波對(duì)車輛動(dòng)態(tài)特性以及車輛位置進(jìn)行估計(jì)，分別得到車輛的狀態(tài)參數(shù)估計(jì)值和位置參數(shù)估計(jì)值；
6、步驟3)、以各軸的轉(zhuǎn)向角為控制量，基于車輛的狀態(tài)參數(shù)估計(jì)值、位置參數(shù)估計(jì)值和參考軌跡的狀態(tài)空間表達(dá)式構(gòu)建車輛誤差狀態(tài)空間模型，將車輛誤差狀態(tài)空間模型離散化；
7、步驟4)、基于離散化的車輛誤差狀態(tài)空間模型構(gòu)建預(yù)測(cè)模型，利用預(yù)測(cè)模型對(duì)多軸重載鉸接車輛的軌跡進(jìn)行跟蹤控制。
8、進(jìn)一步地，多軸重載鉸接車輛的動(dòng)力學(xué)模型由如下公式確定：
9、
10、其中，多軸重載鉸接車輛包括前車和后車；mf表示前車的質(zhì)量；mr表示后車的質(zhì)量；βf表示前車質(zhì)心側(cè)偏角；βr表示后車質(zhì)心側(cè)偏角；ω1表示前車質(zhì)心橫擺角速度；ω2表示后車質(zhì)心橫擺角速度；u1表示前車的縱向速度；u2表示后車的縱向速度；a1表示前車的質(zhì)心到第一軸的距離；a2表示后車的質(zhì)心到第六軸的距離；b1表示前車的質(zhì)心到第二軸的距離；b2表示后車的質(zhì)心到第五軸的距離；c1表示前車的質(zhì)心到第三軸的距離；c2表示后車的質(zhì)心到第四軸的距離；d1表示前車的質(zhì)心到鉸接點(diǎn)的距離；d2表示后車的質(zhì)心到鉸接點(diǎn)的距離；δi表示第i軸的轉(zhuǎn)角；fyi表示作用在第i軸的側(cè)向力；i＝1、2、3、4、5、6；ft表示鉸接點(diǎn)處的作用力；izf表示前車?yán)@z軸的轉(zhuǎn)動(dòng)慣量；izr表示后車?yán)@z軸的轉(zhuǎn)動(dòng)慣量；為鉸接角；
11、其中，第一軸、第二軸和第三軸為前車的三個(gè)軸；第四軸、第五軸和第六軸為后車的三個(gè)軸。
12、進(jìn)一步地，多軸重載鉸接車輛的運(yùn)動(dòng)學(xué)模型包括：多軸重載鉸接車輛的前車的運(yùn)動(dòng)學(xué)模型和多軸重載鉸接車輛的后車的運(yùn)動(dòng)學(xué)模型；
13、其中，前車的運(yùn)動(dòng)學(xué)模型通過如下公式確定：
14、
15、后車的運(yùn)動(dòng)學(xué)模型通過如下公式確定：
16、
17、其中，xf表示前車在橫向的位置；yf表示前車在縱向的位置；xr表示后車在橫向的位置；yr表示后車在縱向的位置；ψf表示前車的航向角；ψr表示后車的航向角。
18、進(jìn)一步地，fyi需滿足如下公式所示的線性關(guān)系：
19、fyi＝kiαi
20、其中，ki表示第i軸的側(cè)偏剛度；αi表示第i軸的輪胎的側(cè)偏角。
21、進(jìn)一步地，步驟2)包括：在第一層，將各軸的轉(zhuǎn)向角δi、前車的橫向加速度af、后車的橫向加速度ar以及鉸接點(diǎn)處的作用力ft發(fā)送給多軸重載鉸接車輛的動(dòng)力學(xué)模型，計(jì)算出車輛的狀態(tài)參數(shù)估計(jì)值；然后將第一層的估計(jì)結(jié)果發(fā)送到第二層，基于多軸重載鉸接車輛的運(yùn)動(dòng)學(xué)模型，計(jì)算出車輛的位置參數(shù)估計(jì)值；
22、其中，前車的橫向加速度af通過如下公式確定：
23、
24、后車的橫向加速度ar通過如下公式確定：
25、
26、其中，車輛的狀態(tài)參數(shù)估計(jì)值包括：前車質(zhì)心側(cè)偏角的估計(jì)值βfg、后車質(zhì)心側(cè)偏角的估計(jì)值βrg、前車質(zhì)心橫擺角速度的估計(jì)值ω1g以及后車質(zhì)心橫擺角速度的估計(jì)值ω2g；車輛的位置參數(shù)估計(jì)值包括：多軸重載鉸接車輛的前車在橫向的位置的估計(jì)值xfg、前車在縱向的位置的估計(jì)值yfg、多軸重載鉸接車輛的后車在橫向的位置的估計(jì)值xrg以及后車在縱向的位置的估計(jì)值yrg。
27、進(jìn)一步地，通過角度傳感器獲取各軸的轉(zhuǎn)向角δi；通過速度傳感器獲取前車的縱向速度u1和后車的縱向速度u2；通過力傳感器獲取鉸接點(diǎn)處的作用力ft。
28、進(jìn)一步地，步驟3)包括：根據(jù)車輛的狀態(tài)參數(shù)估計(jì)值和車輛的位置參數(shù)估計(jì)值構(gòu)建系統(tǒng)狀態(tài)空間表達(dá)式；
29、系統(tǒng)狀態(tài)空間表達(dá)式的控制量為各軸的轉(zhuǎn)向角；系統(tǒng)狀態(tài)空間表達(dá)式減去參考軌跡的狀態(tài)空間表達(dá)式得到車輛誤差狀態(tài)空間模型。
30、進(jìn)一步地，使用前向歐拉法對(duì)車輛誤差狀態(tài)空間模型進(jìn)行離散化。
31、進(jìn)一步地，步驟4)中，基于離散化的車輛誤差狀態(tài)空間模型，利用模型預(yù)測(cè)控制算法構(gòu)建預(yù)測(cè)模型。
32、本發(fā)明的一種多軸重載鉸接車輛的軌跡跟蹤控制裝置，包括處理器，處理器執(zhí)行計(jì)算機(jī)程序，以實(shí)現(xiàn)如述的多軸重載鉸接車輛的軌跡跟蹤控制方法的方法步驟。
33、本發(fā)明的有益效果為：作為開拓性發(fā)明創(chuàng)造，本發(fā)明提供了一種多軸重載鉸接車輛的軌跡跟蹤控制方法及裝置，先通過構(gòu)建多軸重載鉸接車輛的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)耦合模型，可以同時(shí)掌握多軸重載鉸接車輛的狀態(tài)信息和位置信息；然后再基于分層無跡卡爾曼濾波方法對(duì)車輛動(dòng)態(tài)特性以及車輛位置進(jìn)行估計(jì)，分別得到車輛的狀態(tài)參數(shù)估計(jì)值和位置參數(shù)估計(jì)值，可以減少估計(jì)誤差，提高估計(jì)精度；隨后再以各軸的轉(zhuǎn)向角為控制量，基于車輛的狀態(tài)參數(shù)估計(jì)值、位置參數(shù)估計(jì)值和參考軌跡的狀態(tài)空間表達(dá)式構(gòu)建車輛誤差狀態(tài)空間模型，將車輛誤差狀態(tài)空間模型離散化；基于離散化的車輛誤差狀態(tài)空間模型構(gòu)建預(yù)測(cè)模型，利用預(yù)測(cè)模型對(duì)多軸重載鉸接車輛的軌跡進(jìn)行跟蹤控制，能夠全面的考慮車輛狀態(tài)和車輛位置信息，實(shí)現(xiàn)對(duì)下一時(shí)刻的精確預(yù)測(cè)和最優(yōu)控制，保證在復(fù)雜工況下的穩(wěn)定性、精確性與魯棒性。
1.一種多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，包括如下步驟：
2.根據(jù)權(quán)利要求1所述的多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，所述多軸重載鉸接車輛的動(dòng)力學(xué)模型由如下公式確定：
3.根據(jù)權(quán)利要求2所述的多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，所述多軸重載鉸接車輛的運(yùn)動(dòng)學(xué)模型包括：多軸重載鉸接車輛的前車的運(yùn)動(dòng)學(xué)模型和所述多軸重載鉸接車輛的后車的運(yùn)動(dòng)學(xué)模型；
4.根據(jù)權(quán)利要求3所述的多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，所述fyi需滿足如下公式所示的線性關(guān)系：
5.根據(jù)權(quán)利要求3所述的多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，所述步驟2)包括：在第一層，將各軸的轉(zhuǎn)向角δi、前車的橫向加速度af、后車的橫向加速度ar以及鉸接點(diǎn)處的作用力ft發(fā)送給所述多軸重載鉸接車輛的動(dòng)力學(xué)模型，計(jì)算出車輛的狀態(tài)參數(shù)估計(jì)值；然后將第一層的估計(jì)結(jié)果發(fā)送到第二層，基于所述多軸重載鉸接車輛的運(yùn)動(dòng)學(xué)模型，計(jì)算出車輛的位置參數(shù)估計(jì)值；
6.根據(jù)權(quán)利要求5所述的多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，通過角度傳感器獲取各軸的轉(zhuǎn)向角δi；通過速度傳感器獲取前車的縱向速度u1和后車的縱向速度u2；通過力傳感器獲取鉸接點(diǎn)處的作用力ft。
7.根據(jù)權(quán)利要求5所述的多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，步驟3)包括：根據(jù)所述車輛的狀態(tài)參數(shù)估計(jì)值和所述車輛的位置參數(shù)估計(jì)值構(gòu)建系統(tǒng)狀態(tài)空間表達(dá)式；
8.根據(jù)權(quán)利要求5所述的多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，使用前向歐拉法對(duì)所述車輛誤差狀態(tài)空間模型進(jìn)行離散化。
9.根據(jù)權(quán)利要求1-8任一項(xiàng)所述的多軸重載鉸接車輛的軌跡跟蹤控制方法，其特征在于，步驟4)中，基于離散化的車輛誤差狀態(tài)空間模型，利用模型預(yù)測(cè)控制算法構(gòu)建預(yù)測(cè)模型。
10.一種多軸重載鉸接車輛的軌跡跟蹤控制裝置，包括處理器，其特征在于，所述處理器執(zhí)行計(jì)算機(jī)程序，以實(shí)現(xiàn)如權(quán)利要求1-9任一項(xiàng)所述的多軸重載鉸接車輛的軌跡跟蹤控制方法的方法步驟。