本發(fā)明涉及車輛控制領域，尤其是涉及一種基于卡爾曼濾波的多列車鉸接角視覺測量優(yōu)化方法。
背景技術：
：鉸接角是多列車的重要動力學狀態(tài)，準確的測量多列車輛鉸接角可以有利于此類車輛主動安全系統(tǒng)的應用欲開發(fā)，如多列車主動轉向與多列車倒車系統(tǒng)等?，F(xiàn)有的鉸接角測量技術分為如下兩類：1.接觸式測量傳感器：如旋轉電位器安裝在重型車第五輪位置。2.非接觸式測量傳感器：如超聲波傳感器和視覺傳感器等。接觸式測量傳感器或超聲波傳感器只適用于某種特定的鉸接形式(如第五輪鉸接)。由于視覺系統(tǒng)的圖像質量易受諸多因素影響(系統(tǒng)振動、天氣等)，視覺傳感系統(tǒng)的測量量可靠性會受到影響。技術實現(xiàn)要素：本發(fā)明主要是解決現(xiàn)有技術所存在的受限于特定的鉸接形式或測量量可靠性不足的技術問題，提供一種不受鉸接形式限制、不容易受系統(tǒng)振動或天氣干擾的基于卡爾曼濾波的多列車鉸接角視覺測量優(yōu)化方法。本發(fā)明針對上述技術問題主要是通過下述技術方案得以解決的：一種基于卡爾曼濾波的多列車鉸接角視覺測量優(yōu)化方法，包括以下步驟：S1、建立三自由度線性列車車輛系統(tǒng)模型；S2、設定初始值；S3、預測狀態(tài)；S4、更新卡爾曼增益；S5、修正狀態(tài)。視覺系統(tǒng)的鉸接角測量值以及駕駛員方向盤轉角傳感器測量值為本系統(tǒng)的輸入，拖掛車輛鉸接角為本系統(tǒng)的輸出。作為優(yōu)選，所述三自由度線性列車車輛系統(tǒng)模型為：xk+1＝Axk+Buk+wkzk+1＝Cxk+vk其中，k為離散時間序列，xk為狀態(tài)量，xk為四維向量，包括牽引車側向速度、橫擺角速度、鉸接角以及鉸接角速度；zk為鉸接角的觀測量；uk為系統(tǒng)輸入量，即方向盤轉角；wk為過程噪聲；vk為觀測噪聲；狀態(tài)空間矩陣A、B和C詳細信息如下：M=m1+m2(a-b-d)m2-m2d0m1(b-a)J100-m2dJ2+m2d(b-a+d)J2+m2d200001;]]>N=C1+C2+C3v-(m1+m2)v+a(C1+C2+C3)-C2c-C3(b+e)v-C3ev-C3bC1-(c-b)C2v-m1(b-a)v+abC1+C2(c-b)(c-a)v00-C3evb-a+evC3e+m2dvC3e2vC3e0010;]]>E＝[C1b00]T；A＝M-1N；B＝M-1E；C＝[0001]；其中，m1為牽引車質量,m2為掛車質量；J1為牽引車橫擺轉動慣量,J2為掛車橫擺轉動慣量；a為牽引車前軸與牽引車質心的距離，b為牽引車前軸與第五輪的距離，c為牽引車前軸與后軸的距離；d為掛車質心到第五輪的距離，e為后軸到第五輪的距離；C1為牽引車前軸對應的輪胎的側偏剛度，C2為牽引車后軸對應的輪胎的側偏剛度，C3為掛車后軸對應的輪胎的側偏剛度；v為縱向車速，為觀測器的系統(tǒng)參數(shù)。作為優(yōu)選，步驟S3中，預測狀態(tài)依據(jù)以下公式確定：x^(k+1|k)=Ax^(k|k)+Buk]]>P(k+1|k)＝AP(k|k)AT+Q其中，為系統(tǒng)狀態(tài)的先驗估計；P(k+1|k)為系統(tǒng)協(xié)方差矩陣的先驗估計；Q為系統(tǒng)過程協(xié)方差。作為優(yōu)選，步驟S4中，由以下公式實現(xiàn)卡爾曼增益更新：Kk+1＝P(k+1|k)CT(CP(k+1|k)CT+R)-1其中，Kk+1為卡爾曼增益；R為系統(tǒng)測量協(xié)方差。作為優(yōu)選，步驟S5中，狀態(tài)修正由以下公式確定：x^(k+1|k+1)=x^(k+1|k)+Kk+1(zk+1-Cx^(k+1|k))]]>P(k+1|k+1)＝P(k+1|k)-Kk+1CP(k+1|k)其中，為系統(tǒng)狀態(tài)的后驗估計；P(k+1|k+1)為系統(tǒng)協(xié)方差矩陣的后驗估計。作為優(yōu)選，所述步驟S2中，x0的初始值設定為[0000]，狀態(tài)協(xié)方差矩陣初始值P0的初始值設定為0。作為優(yōu)選，系統(tǒng)過程協(xié)方差Q為diag(0.2,0.05,0.1,0.2)；系統(tǒng)測量協(xié)方差R為0.1。本發(fā)明采用離散卡爾曼濾波算法，可在低速工況下(0-30km/h)優(yōu)化視覺傳感系統(tǒng)鉸接角測量輸出。本發(fā)明利用常見方向盤轉角傳感器信號修正基于視覺系統(tǒng)的鉸接角測量值，能夠優(yōu)化視覺傳感系統(tǒng)的鉸接角測量精度與可靠性，融合后系統(tǒng)在視覺系統(tǒng)短暫出現(xiàn)錯誤或無效測量輸出的情況下仍能繼續(xù)有效工作。本方案不增加額外傳感設備，且線性卡爾曼濾波算法效率較高，實時性強，適用于車載系統(tǒng)。本發(fā)明帶來的實質性效果是，不需要額外增加成本，可以有效降低視覺系統(tǒng)測量誤差，提升視覺系統(tǒng)對鉸接角的測量可靠性。附圖說明圖1是本發(fā)明的一種流程圖。具體實施方式下面通過實施例，并結合附圖，對本發(fā)明的技術方案作進一步具體的說明。實施例：本實施例的一種基于卡爾曼濾波的多列車鉸接角視覺測量優(yōu)化方法，流程圖如圖1所示。視覺系統(tǒng)的鉸接角測量值以及駕駛員方向盤轉角傳感器測量值為本系統(tǒng)的輸入，拖掛車輛鉸接角為本系統(tǒng)的輸出。本發(fā)明采用離散卡爾曼濾波算法，可在低速工況下(0-30km/h)優(yōu)化視覺傳感系統(tǒng)鉸接角測量輸出，詳細的算法步驟介紹如下：1.三自由度線性列車車輛系統(tǒng)模型：xk+1＝Axk+Buk+wkzk+1＝Cxk+vk其中，xk為狀態(tài)量，為四維向量，包括牽引車側向速度，橫擺角速度，鉸接角以及鉸接角速度；zk為觀測量(鉸接角)；uk為系統(tǒng)輸入(方向盤轉角)；wk為過程噪聲；vk為觀測噪聲；狀態(tài)空間矩陣A,B,C詳細信息如下：M=m1+m2(a-b-d)m2-m2d0m1(b-a)J100-m2dJ2+m2d(b-a+d)J2+m2d200001;]]>N=C1+C2+C3v-(m1+m2)v+a(C1+C2+C3)-C2c-C3(b+e)v-C3ev-C3bC1-(c-b)C2v-m1(b-a)v+abC1+C2(c-b)(c-a)v00-C3evb-a+evC3e+m2dvC3e2vC3e0010;]]>E＝[C1b00]T；A＝M-1N；B＝M-1E；C＝[0001]；其中，m1,m2為牽引車與掛車質量；J1,J2為牽引車與掛車橫擺轉動慣量；a,b,c分別為牽引車前軸與牽引車質心，第五輪以及后軸的距離；d,e分別為掛車質心以及后軸到第五輪的距離；C1,C2,C3為輪胎側偏剛度；v為縱向車速設定值(由于此類傳感器的適用工況多以低速工況為主，因而此處車速參數(shù)設定為15km/h，在車速范圍為0-30km/h的情況下，濾波結果均可接受)。2.初始值設定：x0＝[0000]P0＝03.狀態(tài)預測：x^(k+1|k)=Ax^(k|k)+Buk]]>P(k+1|k)＝AP(k|k)AT+Q其中，為系統(tǒng)狀態(tài)的先驗估計；P(k+1|k)為系統(tǒng)協(xié)方差矩陣的先驗估計；Q為系統(tǒng)過程協(xié)方差；4.卡爾曼增益更新:Kk+1＝P(k+1|k)CT(CP(k+1|k)CT+R)-1其中，Kk+1為卡爾曼增益R為系統(tǒng)測量協(xié)方差5.狀態(tài)修正：x^(k+1|k+1)=x^(k+1|k)+Kk+1(zk+1-Cx^(k+1|k))]]>P(k+1|k+1)＝P(k+1|k)-Kk+1CP(k+1|k)其中，為系統(tǒng)狀態(tài)的后驗估計P(k+1|k+1)為系統(tǒng)協(xié)方差矩陣的后驗估計觀測器參數(shù)主要有過程協(xié)方差Q以及測量協(xié)方差R，其設置如下：Q＝diag(0.2,0.05,0.1,0.2)R＝0.1本文中所描述的具體實施例僅僅是對本發(fā)明精神作舉例說明。本發(fā)明所屬
技術領域：
的技術人員可以對所描述的具體實施例做各種各樣的修改或補充或采用類似的方式替代，但并不會偏離本發(fā)明的精神或者超越所附權利要求書所定義的范圍。盡管本文較多地使用了鉸接角、側向速度、噪聲等術語，但并不排除使用其它術語的可能性。使用這些術語僅僅是為了更方便地描述和解釋本發(fā)明的本質；把它們解釋成任何一種附加的限制都是與本發(fā)明精神相違背的。當前第1頁1 2 3