本發(fā)明涉及車輛姿態(tài)測量系統(tǒng)，尤其涉及一種車輛姿態(tài)安全預(yù)警方法及系統(tǒng)。
背景技術(shù)：
市場原有的車輛姿態(tài)測量裝置是一種重力懸浮機(jī)械裝置，儀表盤內(nèi)懸浮圓球跟隨車體慣性發(fā)生延后轉(zhuǎn)動(dòng)。這種裝置存在諸多缺陷，如：顯示嚴(yán)重延遲，不利于觀察，會(huì)導(dǎo)致駕駛員分神，造成潛在安全隱患，市場接受程度低。
技術(shù)實(shí)現(xiàn)要素：
為克服現(xiàn)有技術(shù)存在的上述技術(shù)問題，本發(fā)明提供了一種車輛姿態(tài)安全預(yù)警方法，其通過穩(wěn)定的姿態(tài)解算處理過程，可真實(shí)準(zhǔn)確地反映車輛姿態(tài)。
本發(fā)明解決上述技術(shù)問題的技術(shù)方案如下：一種車輛姿態(tài)安全預(yù)警方法，其包括以下步驟：
S1：利用MEMS陀螺儀測量車輛的角速率信息，利用加速度計(jì)測量車輛的第一加速度信息，利用GPS輸出信息計(jì)算得到第二加速度信息；
S2：選取姿態(tài)誤差角、陀螺常值偏移和加速度計(jì)常值零偏作為狀態(tài)向量，利用卡爾曼濾波基本方程對所述狀態(tài)向量進(jìn)行估計(jì)，獲得估計(jì)的狀態(tài)向量；
S3：使用估計(jì)的狀態(tài)向量中的所述姿態(tài)誤差角修正姿態(tài)矩陣，獲得修正后的姿態(tài)矩陣，根據(jù)所述修正后的姿態(tài)矩陣解算出俯仰角、橫滾角和航向角；將所述角速度信息扣除所述估計(jì)的狀態(tài)向量中的所述陀螺常值偏移得到角速率值；根據(jù)所述第一加速度信息、第二加速度信息及所述估計(jì)的狀態(tài)向量中的所述加速度計(jì)常值零偏得到加速度值；
S4：根據(jù)所述俯仰角、橫滾角、航向角、角速率值及加速度值判斷車輛姿態(tài)，并輸出所述俯仰角、橫滾角、航向角、角速率值、加速度值及車輛姿態(tài)。
本發(fā)明的有益效果是：利用穩(wěn)定的姿態(tài)解算處理方法，修正姿態(tài)矩陣，達(dá)到提高姿態(tài)解算精度的目的；在陀螺采樣得到的角速率中扣除濾波估計(jì)得到的陀螺常值漂移可以減小姿態(tài)解算過程中使用的角速率數(shù)據(jù)的誤差，提高姿態(tài)解算精度；在加速度計(jì)輸出中扣除濾波估計(jì)得到的加速度計(jì)常值零偏可以減小加速度計(jì)輸出數(shù)據(jù)的誤差，從而提高濾波過程中使用的量測向量的精度。采用選定的狀態(tài)方程和量測方程，進(jìn)行相應(yīng)的參數(shù)配置，在執(zhí)行完濾波估計(jì)后對相應(yīng)的變量進(jìn)行修正可以有效提高導(dǎo)航過程中姿態(tài)解算的精度，從而可真實(shí)準(zhǔn)確地反映車輛姿態(tài)，進(jìn)行可進(jìn)行準(zhǔn)確的車輛姿態(tài)安全預(yù)警。
在上述技術(shù)方案的基礎(chǔ)上，本發(fā)明還可以做如下改進(jìn)。
進(jìn)一步，在所述步驟S2之前還包括以下步驟：
T1：對所述角速率信息和第一加速度信息進(jìn)行標(biāo)定處理，獲得標(biāo)定后的角速率信息和第一加速度信息；
T2：將所述標(biāo)定后的第一加速度信息與根據(jù)GPS輸出信息計(jì)算得到的所述第二加速度信息進(jìn)行對比，計(jì)算兩者的差值，獲得加速度匹配信息，并對所述標(biāo)定后的角速率信息進(jìn)行正交標(biāo)定；
T3：判斷GPS是否可用，若GPS不可用，則轉(zhuǎn)至步驟S2；若GPS可用，則判斷車輛是否轉(zhuǎn)彎，若轉(zhuǎn)彎則轉(zhuǎn)至步驟S2，若不轉(zhuǎn)彎則將此刻所述加速計(jì)測量得到的加速度信息與根據(jù)GPS輸出信息計(jì)算得到的加速度信息進(jìn)行對比，計(jì)算兩者的差值，獲得加速度補(bǔ)償信息，再轉(zhuǎn)至步驟S2。
進(jìn)一步，在所述步驟T3和步驟S2之間還包括以下步驟：
在所述步驟T3中，若判斷車輛轉(zhuǎn)彎，則消除轉(zhuǎn)彎時(shí)X軸陀螺輸出的角速率，同時(shí)消除轉(zhuǎn)彎時(shí)Y軸陀螺輸出的角速率；
在消除轉(zhuǎn)彎時(shí)X軸陀螺的輸出的角速率之后，再判斷車輛是否處于轉(zhuǎn)彎狀態(tài)，若車輛處于轉(zhuǎn)彎狀態(tài)，則對X軸陀螺輸出的角速率信息進(jìn)行積分，得到俯仰角，并輸出；若車輛沒有處于轉(zhuǎn)彎狀態(tài)，則轉(zhuǎn)至步驟S2；
在消除轉(zhuǎn)彎時(shí)Y軸陀螺輸出的角速率之后，對Y軸陀螺輸出的角速率進(jìn)行積分得到第一俯仰角，同時(shí)根據(jù)加速度計(jì)的輸出估計(jì)出第二俯仰角，之后計(jì)算所述第一俯仰角與所述第二俯仰角的差值；并判斷GPS是否可用，若GPS不可用且所述第一俯仰角與所述第二俯仰角的差值大于預(yù)設(shè)值時(shí)，則對Y軸陀螺輸出的角速率信息進(jìn)行積分，估計(jì)當(dāng)前的俯仰角，并輸出；若GPS可用，則轉(zhuǎn)至所述步驟S2。
進(jìn)一步，所述步驟S2具體包括：
S2-1：選定濾波狀態(tài)向量X(t)：姿態(tài)誤差角φ＝[φE φN φU]T、陀螺常值漂移εb＝[εx εy εz]T、加速度計(jì)常值零偏即
S2-2：確立系統(tǒng)狀態(tài)轉(zhuǎn)移方程：
其中，X&(t)為狀態(tài)向量X(t)的導(dǎo)數(shù)，φ&、ε&b和依次為姿態(tài)誤差角φ、陀螺常值漂移εb、加速度計(jì)常值零偏的導(dǎo)數(shù)；
是9×9維的連續(xù)系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；
0代表3×3維的零陣是3×3維的從機(jī)體坐標(biāo)系b系到平臺坐標(biāo)系n系的方向余弦矩陣；w(t)為系統(tǒng)的激勵(lì)白噪聲過程，是3維向量，G(t)是9×3維的噪聲轉(zhuǎn)移矩陣；
S2-3：對所述系統(tǒng)狀態(tài)轉(zhuǎn)移方程進(jìn)行離散化處理得到離散化后的9×9維的狀態(tài)一步轉(zhuǎn)移矩陣Φk,k-1，Φk,k-1＝I+TF(t)，其中，T為濾波周期，I為9×9維的對角線元素全部為1，其余元素全部為0的單位矩陣；
S2-4：根據(jù)姿態(tài)矩陣和所述加速度匹配，或者根據(jù)姿態(tài)矩陣和所述加速度補(bǔ)償信息確定量測向量，并根據(jù)所述量測向量與狀態(tài)向量確定量測方程；
S2-5：根據(jù)所述狀態(tài)向量、狀態(tài)轉(zhuǎn)移方程及量測方程，并利用所述濾波基本方程對被估計(jì)的所述狀態(tài)向量進(jìn)行求解，得到估計(jì)的狀態(tài)向量。
進(jìn)一步，所述步驟S2-4具體包括：
將機(jī)體坐標(biāo)系b系的所述加速度匹配信息或所述加速度補(bǔ)償信息fb＝[fx fy fz]T投影到平臺坐標(biāo)系n系，得到E為東向，N為北向，U為天向，即
其中，是3×3維的從機(jī)體坐標(biāo)系b系到平臺坐標(biāo)系n系的方向余弦矩陣，即選取的量測向量Zk為fn的前兩維，即描述量測向量Zk與狀態(tài)向量Xk之間關(guān)系的量測方程為：
其中，是量測矩陣，g為重力加速度的大小，T00,T01,T02,T10,T11,T12為姿態(tài)矩陣中前兩行的元素，Vk為2×1維量測噪聲序列，其均值為0。
進(jìn)一步，在所述步驟S2-5中，所述濾波基本方程如下：
狀態(tài)一步預(yù)測方程為其中，為狀態(tài)一步預(yù)測估計(jì)值，Φk,k-1為狀態(tài)一步轉(zhuǎn)移矩陣，為前一時(shí)刻狀態(tài)向量的估計(jì)值；
一步預(yù)測均方誤差陣其中，Pk/k-1為一步預(yù)測均方誤差陣，Pk-1為前一時(shí)刻狀態(tài)估計(jì)的均方誤差陣，Qk-1為系統(tǒng)噪聲方差陣，為狀態(tài)一步轉(zhuǎn)移矩陣Φk,k-1的轉(zhuǎn)置；
計(jì)算濾波增益陣其中，Kk為濾波增益陣，Hk為量測矩陣，為Hk的轉(zhuǎn)置；Rk為量測噪聲序列的方差陣，其中，zk0和zk1為量測向量中的兩個(gè)分量，w為權(quán)重系數(shù)；
狀態(tài)估計(jì)其中，為當(dāng)前時(shí)刻狀態(tài)向量的估計(jì)值，Zk為量測向量；
計(jì)算估計(jì)的均方誤差其中，Pk為當(dāng)前時(shí)刻被估計(jì)的狀態(tài)向量的均方誤差陣，I為單位矩陣；
根據(jù)所述基本濾波方程a)-e)計(jì)算得到k時(shí)刻被估計(jì)的狀態(tài)向量即狀態(tài)向量X(t)在tk時(shí)刻的估計(jì)值。
進(jìn)一步，在所述步驟S4中，還包括顯示所述俯仰角、橫滾角、航向角、角速率值及加速度值的步驟。
進(jìn)一步，還包括將所述步驟S4中判斷出的所述車輛姿態(tài)進(jìn)行語音播報(bào)的步驟。
本發(fā)明還提供了一種車輛姿態(tài)安全預(yù)警系統(tǒng)，其包括:
MEMS陀螺儀，用于測量車輛的角速率信息；
加速度計(jì)，用于實(shí)時(shí)測量車輛的第一加速度信息；
GPS，用于獲得車輛的第二加速度信息；
姿態(tài)解算單元，用于根據(jù)所述角速率信息、第一加速度信息和第二加速度信息進(jìn)行姿態(tài)解算，獲得車輛的姿態(tài)。
進(jìn)一步，還包括顯示單元和語音播報(bào)單元，所述顯示單元用于顯示所述俯仰角、橫滾角、航向角、角速率值、加速度值及車輛姿態(tài)；所述語音播報(bào)單元用于控制并播報(bào)所述車輛姿態(tài)。
附圖說明
圖1為本發(fā)明實(shí)施例一提供的車輛姿態(tài)安全預(yù)警方法的流程圖。
具體實(shí)施方式
以下結(jié)合附圖對本發(fā)明的原理和特征進(jìn)行描述，所舉實(shí)例只用于解釋本發(fā)明，并非用于限定本發(fā)明的范圍。需要說明的是，在不沖突的情況下，本申請的實(shí)施例及實(shí)施例中的特征可以相互組合。
實(shí)施例一
如圖1所示，本實(shí)施例提供了一種車輛姿態(tài)安全預(yù)警方法，其包括以下步驟：
S1：利用MEMS陀螺儀測量車輛的角速率信息，利用加速度計(jì)測量車輛的第一加速度信息，利用GPS輸出信息計(jì)算得到第二加速度信息；
S2：選取姿態(tài)誤差角、陀螺常值偏移和加速度計(jì)常值零偏作為狀態(tài)向量，利用卡爾曼濾波基本方程對所述狀態(tài)向量進(jìn)行估計(jì)，獲得估計(jì)的狀態(tài)向量；
S3：使用所述估計(jì)的狀態(tài)向量中的所述姿態(tài)誤差角修正姿態(tài)矩陣，獲得修正后的姿態(tài)矩陣，根據(jù)所述修正后的姿態(tài)矩陣解算出俯仰角、橫滾角和航向角；將所述角速度信息扣除所述估計(jì)的狀態(tài)向量中的所述陀螺常值偏移得到角速率值；根據(jù)所述第一加速度信息、第二加速度信息及扣除所述估計(jì)的狀態(tài)向量中的所述加速度計(jì)常值零偏得到加速度值；
S4：根據(jù)所述俯仰角、橫滾角、航向角、角速率值及加速度值判斷車輛姿態(tài)，并輸出所述俯仰角、橫滾角、航向角、角速率值、加速度值及車輛姿態(tài)。
本實(shí)施例提供的車輛姿態(tài)安全預(yù)警方法，利用穩(wěn)定的姿態(tài)解算處理方法，修正姿態(tài)矩陣，達(dá)到提高姿態(tài)解算精度的目的；在陀螺采樣得到的角速率中扣除濾波估計(jì)得到的陀螺常值漂移可以減小姿態(tài)解算過程中使用的角速率數(shù)據(jù)的誤差，提高姿態(tài)解算精度；在加速度計(jì)輸出中扣除濾波估計(jì)得到的加速度計(jì)常值零偏可以減小加速度計(jì)輸出數(shù)據(jù)的誤差，從而提高濾波過程中使用的量測向量的精度。采用選定的狀態(tài)方程和量測方程，進(jìn)行相應(yīng)的參數(shù)配置，在執(zhí)行完濾波估計(jì)后對相應(yīng)的變量進(jìn)行修正可以有效提高導(dǎo)航過程中姿態(tài)解算的精度，從而可真實(shí)準(zhǔn)確地反映車輛姿態(tài)，進(jìn)行可進(jìn)行準(zhǔn)確的車輛姿態(tài)安全預(yù)警。
實(shí)施例二
本實(shí)施例提供的車輛姿態(tài)安全預(yù)警方法，其在包含實(shí)施例一所述的步驟之外，還具體涉及到以下過程。
在步驟S2之前還包括以下步驟：
對X、Y、Z三軸上的陀螺儀獲得的角速率信息和三軸上的加速度計(jì)獲得的加速度信息進(jìn)行標(biāo)定處理，獲得標(biāo)定后的角速率信息和第一加速度信息；
延時(shí)大約0.6秒將標(biāo)定后的第一加速度信息與GPS獲得的加速度信息進(jìn)行匹配，即將標(biāo)定后的第一加速度信息與根據(jù)GPS輸出信息計(jì)算得到的所述第二加速度信息進(jìn)行對比，計(jì)算兩者的差值，獲得加速度匹配信息，再對標(biāo)定后的角速率信息進(jìn)行正交標(biāo)定，消除誤差；
判斷GPS是否可用，若GPS不可用，則轉(zhuǎn)至步驟S2,；若GPS可用，則判斷車輛是否轉(zhuǎn)彎，若轉(zhuǎn)彎則轉(zhuǎn)至步驟S2，若不轉(zhuǎn)彎則將此刻所述加速計(jì)測量得到的加速度信息與根據(jù)GPS輸出信息計(jì)算得到的第二加速度信息進(jìn)行對比，計(jì)算兩者的差值，獲得加速度補(bǔ)償信息，再轉(zhuǎn)至步驟S2。
理想情況下，車輛轉(zhuǎn)彎過程中只有Z軸向陀螺存在角速率輸出，但是由于轉(zhuǎn)彎時(shí)向心加速度的存在、路面不平整和轉(zhuǎn)彎過程中車胎發(fā)生形變，會(huì)導(dǎo)致X軸和Y軸方向陀螺存在角速度輸出，所以需要消除轉(zhuǎn)彎時(shí)X軸和Y軸陀螺的輸出。因此，在步驟T3和步驟S2之間還包括以下步驟：
在所述步驟T3中，若判斷車輛轉(zhuǎn)彎，則消除轉(zhuǎn)彎時(shí)X軸陀螺輸出的角速率，同時(shí)消除轉(zhuǎn)彎時(shí)Y軸陀螺輸出的角速率；
在消除轉(zhuǎn)彎時(shí)X軸陀螺輸出的角速率之后，再判斷車輛是否處于轉(zhuǎn)彎狀態(tài)，若車輛處于轉(zhuǎn)彎狀態(tài)，則對X軸陀螺輸出的角速率信息進(jìn)行積分，得到俯仰角，并輸出；若車輛沒有處于轉(zhuǎn)彎狀態(tài)，則轉(zhuǎn)至步驟S2；
在消除轉(zhuǎn)彎時(shí)Y軸陀螺輸出的角速率之后，對Y軸陀螺輸出的角速率進(jìn)行積分得到第一俯仰角，同時(shí)根據(jù)加速度計(jì)的輸出估計(jì)出第二俯仰角，之后計(jì)算所述第一俯仰角與所述第二俯仰角的差值；并判斷GPS是否可用，若GPS不可用且所述第一俯仰角與所述第二俯仰角的差值大于預(yù)設(shè)閾值時(shí)，則對Y軸陀螺輸出的角速率信息進(jìn)行積分，估計(jì)當(dāng)前的俯仰角，并輸出；若GPS可用，則轉(zhuǎn)至所述步驟S2。
步驟S2與S3為姿態(tài)解算過程，在姿態(tài)解算前首先進(jìn)行粗對準(zhǔn)，忽略晃動(dòng)干擾的影響，將陀螺的輸出近似看作對地球旋轉(zhuǎn)角速度的測量值，將加速度計(jì)的輸出近似看作對重力加速度的測量值。根據(jù)陀螺和加速度計(jì)的輸出直接解算出姿態(tài)角或姿態(tài)矩陣。
考慮到MEMS陀螺精度低，無法感測地球自轉(zhuǎn)角速率，在粗對準(zhǔn)的過程中不使用陀螺的測量值。根據(jù)加計(jì)的輸出計(jì)算得到了載體的初始俯仰角和橫滾角，同時(shí)給定航向角的初始值為0度。設(shè)加速度計(jì)輸出為fb＝[fx fy fz]T，重力加速度大小為g，則粗對準(zhǔn)得到的俯仰角θ和橫滾角γ如下：
對準(zhǔn)過程中要求載體處于水平靜止?fàn)顟B(tài)。在主函數(shù)中將粗對準(zhǔn)時(shí)間設(shè)定為10s；在中斷處理函數(shù)中，讀取加速度計(jì)輸出，在對準(zhǔn)時(shí)間內(nèi)將加計(jì)的輸出進(jìn)行累加求和，當(dāng)計(jì)數(shù)達(dá)到10s，求加速度計(jì)輸出的平均值，并使用平均值計(jì)算初始俯仰角和橫滾角；將初始航向角置0。
下述具體描述姿態(tài)解算的過程，其中的步驟S2具體包括：
選定濾波狀態(tài)向量X(t)：
姿態(tài)誤差角φ＝[φE φN φU]T、陀螺常值漂移εb＝[εx εy εz]T、加速度計(jì)常值零偏即
確立系統(tǒng)狀態(tài)轉(zhuǎn)移方程：
其中，X&(t)為狀態(tài)向量X(t)的導(dǎo)數(shù)，φ&、ε&b和依次為姿態(tài)誤差角φ、陀螺常值漂移εb、加速度計(jì)常值零偏的導(dǎo)數(shù)；，是9×9維的連續(xù)系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；0代表3×3維的零陣是3×3維的從機(jī)體坐標(biāo)系b系到平臺坐標(biāo)系n系的方向余弦矩陣；w(t)為系統(tǒng)的激勵(lì)白噪聲過程，是3維向量，G(t)是9×3維的噪聲轉(zhuǎn)移矩陣；
在使用卡爾曼濾波基本方程對系統(tǒng)的狀態(tài)向量進(jìn)行估計(jì)前，必須對系統(tǒng)狀態(tài)轉(zhuǎn)移方程進(jìn)行離散化處理。設(shè)濾波周期為T，當(dāng)濾波周期較短時(shí)，F(xiàn)(t)可以近似看作常值矩陣，得到離散化后的9×9維的狀態(tài)一步轉(zhuǎn)移矩陣Φk,k-1為Φk,k-1＝I+TF(t)，其中，I為9×9維的對角線元素全部為1，其余元素全部為0的單位矩陣；
將機(jī)體坐標(biāo)系b系的加速度匹配信息或者加速度補(bǔ)償信息fb＝[fx fy fz]T投影到平臺坐標(biāo)系n系，得到E為東向，N為北向，U為天向，即其中，是3×3維的從機(jī)體坐標(biāo)系b系到平臺坐標(biāo)系n系的方向余弦矩陣，即選取的量測向量Zk為fn的前兩維，即描述量測向量Zk與狀態(tài)向量Xk之間關(guān)系的量測方程為：
其中，是量測矩陣，g為重力加速度的大小，T00,T01,T02,T10,T11,T12為姿態(tài)矩陣中前兩行的元素，Vk為2×1維量測噪聲序列，其均值為0。量測噪聲序列的方差陣為Rk，濾波過程中Rk應(yīng)根據(jù)噪聲統(tǒng)計(jì)特性進(jìn)行設(shè)定。實(shí)際濾波解算過程中將量測噪聲序列方差陣Rk設(shè)定為對角陣，對角線元素的值根據(jù)給定的方差值zk0和zk1為量測向量中的兩個(gè)分量，其大小根據(jù)權(quán)重系數(shù)w進(jìn)行調(diào)整，即
采用上面的公式可以依據(jù)量測值的精度對量測序列方差陣Rk進(jìn)行調(diào)整，從而在濾波過程中自動(dòng)確定對量測信息的利用程度，提高對狀態(tài)向量的估計(jì)效果。而現(xiàn)有技術(shù)中的濾波設(shè)計(jì)過程中將Rk設(shè)定為常值矩陣，當(dāng)噪聲的統(tǒng)計(jì)特性發(fā)生變化時(shí)，無法根據(jù)量測噪聲的實(shí)際情況進(jìn)行調(diào)整，難以達(dá)到對量測信息的有效利用，對狀態(tài)向量估計(jì)的效果不佳。
經(jīng)過上述選定的濾波狀態(tài)向量、確立的系統(tǒng)狀態(tài)轉(zhuǎn)移方程及量測方程，可根據(jù)濾波基本方程對被估計(jì)的狀態(tài)向量進(jìn)行求解，其中的濾波基本方程如下：
狀態(tài)一步預(yù)測方程為其中，為狀態(tài)一步預(yù)測估計(jì)值，實(shí)際解算過程中為9維向量；Φk,k-1為狀態(tài)一步轉(zhuǎn)移矩陣，實(shí)際解算過程中為9×9維矩陣；為前一時(shí)刻狀態(tài)向量的估計(jì)值，實(shí)際解算過程中為9維向量。
一步預(yù)測均方誤差陣其中，Pk/k-1為一步預(yù)測均方誤差陣，實(shí)際解算過程中為9×9維矩陣；Pk-1為前一時(shí)刻狀態(tài)估計(jì)的均方誤差陣，實(shí)際解算過程中為9×9維矩陣；Qk-1為系統(tǒng)噪聲方差陣，實(shí)際解算過程中為9×9維矩陣；為狀態(tài)一步轉(zhuǎn)移矩陣Φk,k-1的轉(zhuǎn)置。
計(jì)算濾波增益陣其中，Kk為濾波增益陣，實(shí)際解算過程中為9×2維矩陣；Hk為量測矩陣，實(shí)際解算過程中為2×9維矩陣，為Hk的轉(zhuǎn)置；Rk為量測噪聲序列的方差陣，實(shí)際解算過程中為2×2維矩陣。
狀態(tài)估計(jì)其中，為當(dāng)前時(shí)刻狀態(tài)向量的估計(jì)值，實(shí)際解算過程中為9維向量；Zk為量測向量，實(shí)際解算過程中為2維向量。
計(jì)算估計(jì)的均方誤差其中，Pk為當(dāng)前時(shí)刻被估計(jì)的狀態(tài)向量的均方誤差陣，實(shí)際解算過程中為9×9維矩陣；I為單位矩陣，實(shí)際解算過程中為9×9維的對角線元素全部為1，其余元素全部為0的單位矩陣。
根據(jù)上述基本濾波方程可以計(jì)算得到k時(shí)刻被估計(jì)的狀態(tài)向量即狀態(tài)向量X(t)在tk時(shí)刻的估計(jì)值，使用中姿態(tài)誤差角誤差修正姿態(tài)矩陣從而達(dá)到提高姿態(tài)解算精度的目的；在陀螺采樣得到的角速率中扣除濾波估計(jì)得到的陀螺常值漂移可以減小姿態(tài)解算過程中使用的角速率數(shù)據(jù)的誤差，提高姿態(tài)解算精度；在加速度計(jì)輸出中扣除濾波估計(jì)得到的加速度計(jì)常值零偏可以減小加速度計(jì)輸出數(shù)據(jù)的誤差，從而提高濾波過程中使用的量測向量的精度。綜上所述，采用選定的狀態(tài)方程和量測方程，進(jìn)行相應(yīng)的參數(shù)配置，在執(zhí)行完濾波估計(jì)后對相應(yīng)的變量進(jìn)行修正可以有效提高導(dǎo)航過程中姿態(tài)解算的精度。
根據(jù)修正后的姿態(tài)矩陣計(jì)算解算出俯仰角、橫滾角和航向角，具體計(jì)算公式如下：
姿態(tài)四元數(shù)Q＝[q0 q1 q2 q3]T，即
由上述公式解算出姿態(tài)航向角，即俯仰角θ、橫滾角γ和航向角即
經(jīng)過上述姿態(tài)解算處理過程，獲得了車輛的俯仰角、橫滾角、航向角、角速率值及加速度值等表示車輛姿態(tài)的參數(shù)，并可將該姿態(tài)參數(shù)顯示在屏幕上，便于車輛駕駛?cè)藛T了解車輛的狀況，并可根據(jù)該姿態(tài)參數(shù)進(jìn)行語音播報(bào)，提示路況變化及當(dāng)前車輛狀態(tài)，避免駕駛員注意力分散，有效降低緊張感和疲勞感、避免視覺錯(cuò)覺，保障行車安全。
語音播報(bào)規(guī)則具體如下：
若GPS不可用，當(dāng)加速度值x<-3m/s2和角速率值Z<10度/秒時(shí)，給出車輛剎車較急，請小心駕駛的語音播報(bào)；若GPS可用，當(dāng)加速度輸出的加速度值和GPS算出的加速度值之和小于-2m/s2，且角速率值Z<10度/秒時(shí)，給出車輛剎車較急，請小心駕駛的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
若GPS不可用，當(dāng)加速度計(jì)輸出的加速度值x>3m/s2和角速率值Z<10度/秒時(shí)，給出車輛加速較急，請小心駕駛的語音播報(bào)；若GPS可用，當(dāng)加速度輸出的加速度值和GPS算出的加速度值之和大于2m/s2，且角速率值Z<10度/秒時(shí)，給出車輛加速較急，請小心駕駛的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
若角速率值連續(xù)4次在8度/秒到18度/秒和-8度/秒到-18度/秒之間，并配合GPS輸出的速度，速度越快，角速率值越小，則給出連續(xù)并道，請注意駕駛習(xí)慣的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
若加速度值x<-1.2m/s2和x>-1.5m/s2之間的次數(shù)大于4次，則給出剎車操作頻繁，請注意保持車距的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
若角速率值的絕對值連續(xù)3次在18度/秒以上，則給出多轉(zhuǎn)彎路段，請控制車速的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
若加速度值大于12.8m/s2的次數(shù)小于8次或者小于7.4的次數(shù)大于70次，且若GPS有效時(shí)，GPS測得的速度大于10m/s時(shí)，則給出路況較好，請保持注意力的語音播報(bào)，復(fù)位條件為：距上次播報(bào)10分鐘或者播報(bào)路面顛簸時(shí)復(fù)位或者GPS有效速度為零。
當(dāng)車輛左轉(zhuǎn)時(shí)，若俯仰角大于5度且角速率Z<-20度/秒時(shí)，則給出上坡左轉(zhuǎn)彎，注意控制車速的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
當(dāng)車輛右轉(zhuǎn)彎，俯仰角大于5度，且角速率Z<-20度/秒時(shí)，則給出上坡右轉(zhuǎn)彎，注意控制車速的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
當(dāng)車輛左轉(zhuǎn)時(shí)，若俯仰角小于-5度，且角速率Z<-20度/秒度時(shí)，則給出下坡左轉(zhuǎn)彎，注意控制車速的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
當(dāng)車輛右轉(zhuǎn)時(shí)，如俯仰角小于-5度，且角速率Z<-20度/秒時(shí)，則給出下坡右轉(zhuǎn)彎，注意控制車速的語音播報(bào)，在播報(bào)完之后進(jìn)行復(fù)位。
當(dāng)滿足上述任一判斷邏輯時(shí)，即會(huì)進(jìn)行相應(yīng)的播報(bào)，在播完上一條時(shí)就會(huì)復(fù)位。通過上述播報(bào)規(guī)則提醒駕駛員車體姿態(tài)的變化，提醒駕駛員提前做出正確操作，避免事故發(fā)生，有效保障車輛在復(fù)雜路面上的行駛安全。
實(shí)施例三
基于上述的車輛姿態(tài)安全預(yù)警方法，本實(shí)施例提供了一種車輛姿態(tài)安全預(yù)警系統(tǒng)，其包括：
MEMS陀螺儀，用于測量車輛的角速率信息；
加速度計(jì)，用于實(shí)時(shí)測量車輛的第一加速度信息；
GPS，用于獲得車輛的第二加速度信息；
姿態(tài)解算單元，用于根據(jù)所述角速率信息、第一加速度信息和第二加速度信息進(jìn)行姿態(tài)解算，獲得車輛的姿態(tài)。
經(jīng)過姿態(tài)解算處理過程后，獲得車輛的姿態(tài)參數(shù)，根據(jù)姿態(tài)參數(shù)判斷車輛的姿態(tài)，并通過顯示單元將姿態(tài)參數(shù)和車輛姿態(tài)顯示在界面上，并可給出相應(yīng)的語音播報(bào)，提醒駕駛?cè)藛T。例如，在雨雪天氣時(shí)，由于視野不佳，無法判斷路況，且路面濕滑，借助該車輛姿態(tài)預(yù)警系統(tǒng)可監(jiān)測車體姿態(tài)信息，預(yù)測前方路況，對車速過快進(jìn)行提醒。在大霧天氣時(shí)，由于視野不佳，無法判斷路況，可通過該車輛姿態(tài)預(yù)警系統(tǒng)連續(xù)預(yù)測路況，提示減速。當(dāng)持續(xù)上坡時(shí)，若坡度預(yù)估不準(zhǔn)確，易導(dǎo)致熄火，通過該車輛姿態(tài)預(yù)警系統(tǒng)可連續(xù)預(yù)測坡度。當(dāng)持續(xù)下坡時(shí)，由于速度過快易導(dǎo)致失速，通過該車輛姿態(tài)預(yù)警系統(tǒng)可連續(xù)預(yù)測坡度，提示減速。當(dāng)緊急轉(zhuǎn)彎時(shí)，由于速度過快，易發(fā)生轉(zhuǎn)彎側(cè)翻，滑下車道的風(fēng)險(xiǎn)，通過該車輛姿態(tài)預(yù)警系統(tǒng)可預(yù)測彎道，提示減速和路況變化。當(dāng)連續(xù)轉(zhuǎn)彎時(shí)，由于離心力增加、速度過快也易導(dǎo)致車輛側(cè)翻，滑下車道，另外轉(zhuǎn)彎視線障礙，無法判斷道路，通過該車輛姿態(tài)預(yù)警系統(tǒng)可連續(xù)預(yù)測彎道曲率，提示轉(zhuǎn)彎減速。當(dāng)持續(xù)下坡轉(zhuǎn)彎時(shí)，也可預(yù)測彎道曲率，提示下坡減速。
車輛姿態(tài)安全系統(tǒng)能夠?qū)崟r(shí)、直觀地檢測車輛行駛姿態(tài)信息，以視頻顯示、音頻警報(bào)方式提供給駕駛員，通過監(jiān)測車輛姿態(tài)信息，提醒駕駛員車體姿態(tài)的變化，及時(shí)進(jìn)行糾正，并對前方路況進(jìn)行預(yù)測，有效保障車輛在復(fù)雜路面上的行駛安全；避免駕駛員注意力分散，有效降低緊張感和疲勞感、避免視覺錯(cuò)覺，保障行車安全。
以上所述僅為本發(fā)明的較佳實(shí)施例，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。