機動車輛姿態(tài)的測量方法及測量裝置的制造方法
【專利摘要】本發(fā)明涉及機動車輛姿態(tài)的測量方法及測量裝置，該方法包括如下步驟：通過衛(wèi)星定位模塊獲取車輛的航向角和瞬時速度以得到地理坐標系上的加速度；根據(jù)地理坐標上的加速度換算得到載體水平坐標系上的加速度；根據(jù)該載體水平坐標系上的加速度獲取得到載體水平坐標系上的比力，并求取載體坐標系與載體水平坐標系之間的變換，之后求得理論姿態(tài)；根據(jù)公式Q＝Pi?1·D求取輸出姿態(tài)，其中，Q為陀螺儀積分出的姿態(tài)，D為角速度乘以時間間隔構(gòu)造的微旋轉(zhuǎn)，Pi為當前輸出姿態(tài)；將理論姿態(tài)和輸出姿態(tài)進行插值處理，并將輸出姿態(tài)以四元數(shù)的形式輸出。本發(fā)明將衛(wèi)星定位、加速度計和陀螺儀結(jié)合在一起作為傾角傳感器，使得輸出準確的傾角。
【專利說明】
機動車輛姿態(tài)的測量方法及測量裝置
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及機動車機動自動檢測技術(shù)，尤其設(shè)及機動車輛姿態(tài)測量的方法及測量 裝置。
【背景技術(shù)】
[0002] 駕駛?cè)嗽谌〉脵C動車駕駛證前，需要通過場地駕駛技能和道路駕駛技能的考試， 即俗稱的"科目二考試"和"科目Ξ考試"；前者用于考核駕駛?cè)嗽诔Ｒ姷缆贰⒊Ｒ娊煌ōh(huán)境、 常見氣候條件和突發(fā)交通狀況下對機動車的操控能力，W及對交通事件的處理能力；后者 用于考核駕駛?cè)嗽趯嶋H道路駕駛機動車時對機動車的操控能力、對交通法規(guī)的識別和遵守 能力、對道路交通情況的處理能力、W及文明駕駛、安全駕駛的意識。
[0003] 在科目二和科目Ξ考試中已經(jīng)使用基于衛(wèi)星定位系統(tǒng)的解決方案。為了判斷車輛 行駛是否符合考試規(guī)則，需要獲得車輛的位置和姿態(tài)。雙天線衛(wèi)星定位可W提供位置、航向 角和俯仰角，而橫滾角需要額外的傾角傳感器。傾角傳感器的物理原理是用加速度計測量 比力，當設(shè)備靜止時，傾角與比力相關(guān)。但是考試過程中車輛是變速運動的，比力包含運動 加速度，運時單獨使用傾角傳感器測出來的傾角并不準確。

【發(fā)明內(nèi)容】

[0004] 針對上述技術(shù)問題，本發(fā)明的目的在于提供一種機動車輛姿態(tài)的測量方法及測量 裝置，其能夠準確的得到傾角。
[0005] 為實現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案：
[0006] 方案一：
[0007] -種機動車輛姿態(tài)的測量方法，應(yīng)用于機動車輛，該機動車輛上設(shè)有巧螺儀、衛(wèi)星 定位模塊和加速度計;包括如下步驟：
[000引步驟A:通過衛(wèi)星定位模塊獲取車輛的航向角和瞬時速度W得到地理坐標系上的 加速度;定義地理坐標系上的向量右上角用G標記，載體坐標系上的向量右上角用C標記，載 體水平坐標系上的向量右上角用Η標記；
[0009] 步驟Β:根據(jù)地理坐標上的加速度換算得到載體水平坐標系上的加速度；
[0010] 步驟C:根據(jù)該載體水平坐標系上的加速度獲取得到載體水平坐標系上的比力，并 求取載體坐標系與載體水平坐標系之間的變換，之后求得理論姿態(tài)；
[OOW 步驟D:根據(jù)公式Q = Pi-i · D求取輸出姿態(tài)，其中，Q為巧螺儀積分出的姿態(tài)，D為角 速度乘W時間間隔構(gòu)造的微旋轉(zhuǎn)，Pi為當前輸出姿態(tài)，Pi-l為上一次輸出姿態(tài)；
[0012] 步驟E:將理論姿態(tài)和輸出姿態(tài)進行插值處理，并將輸出姿態(tài)W四元數(shù)的形式輸 出。
[0013] 優(yōu)選的，步驟A中，根據(jù)公式
化理坐標系上的加速度，其中，互6為地理 坐標系上的加速度，為瞬時速度。
[0014] 優(yōu)選的，步驟B中，根據(jù)公式= rH 'SG換算得到載體水平坐標系上的加速度， 其中，過Η為載體水平坐標系上的加速度，Rg人地理坐標系至載體水平坐標系的變換。
[0015] 優(yōu)選的，步驟C具體包括如下子步驟：
[0016] 步驟C1:根據(jù)公式;+ gH得到載體水平坐標系上的比力，其中，復Η為載 體水平坐標系上的比力，gH為重力等效加速度.
[0017] 步驟C2:通過加速度計獲取載體坐標系下的比力，記為gC;
[001引步驟C3:獲取載體坐標系至載體水平坐標系的變換，記為Rg，并根據(jù)公式
求得理論姿態(tài)，其中，民若為理論姿態(tài)。
[0019] 優(yōu)選的，步驟E具體包括如下子步驟：
[0020] 步驟E1:根據(jù)公式巧=.α+ Q.(l _ α)將理論姿態(tài)和輸出姿態(tài)進行插值處 理，其中，Ρ功輸出姿態(tài)，α為插值權(quán)重，且α的取值范圍為(0,1);
[0021] 步驟Ε2:將PiW四元數(shù)的形式輸出，即Pi=[w X y Ζ]。
[0022] 優(yōu)選的，還包括步驟F:將輸出姿態(tài)通過公式一、公式二和公式Ξ轉(zhuǎn)換成姿態(tài)角輸 出；
[00%]其中，yaw為航向角，Ρ i tch為俯仰角，ro 11為橫滾角。
[0027] 方案二：
[0028] -種機動車輛姿態(tài)的測量裝置，應(yīng)用于機動車輛，該機動車輛上設(shè)有巧螺儀、衛(wèi)星 定位模塊和加速度計;包括如下模塊：
[0029] 定位模塊:用于通過衛(wèi)星定位模塊獲取車輛的航向角和瞬時速度W得到地理坐標 系上的加速度;定義地理坐標系上的向量右上角用G標記，載體坐標系上的向量右上角用C 標記，載體水平坐標系上的向量右上角用Η標記；
[0030] 換算模塊:用于根據(jù)地理坐標上的加速度換算得到載體水平坐標系上的加速度； [0031 ]變換模塊:用于根據(jù)該載體水平坐標系上的加速度獲取得到載體水平坐標系上的 比力，并求取載體坐標系與載體水平坐標系之間的變換，之后求得理論姿態(tài)；
[0032] 求取模塊：用于根據(jù)公式Q = Pi-i . D求取輸出姿態(tài)，其中，Q為巧螺儀積分出的姿 態(tài)，D為角速度乘W時間間隔構(gòu)造的微旋轉(zhuǎn)，Pi為當前輸出姿態(tài)，Pi-i為上一次輸出姿態(tài)；
[0033] 輸出模塊：用于將理論姿態(tài)和輸出姿態(tài)進行插值處理，并將輸出姿態(tài)W四元數(shù)的 形式輸出。
[0034] 相比現(xiàn)有技術(shù)，本發(fā)明的有益效果在于：
[0035] 本發(fā)明將衛(wèi)星定位、加速度計和巧螺儀結(jié)合在一起作為傾角傳感器，使得輸出準 確的傾角。
【附圖說明】
[0036] 圖1為本發(fā)明的機動車輛姿態(tài)的測量方法的流程圖。
【具體實施方式】
[0037] 下面，結(jié)合附圖W及【具體實施方式】，對本發(fā)明做進一步描述：
[0038] 參見圖1，本發(fā)明提供一種機動車輛姿態(tài)的測量方法，其應(yīng)用于機動車輛，該機動 車輛上設(shè)有巧螺儀、衛(wèi)星定位模塊和加速度計，本發(fā)明的改進在于將運Ξ者聯(lián)合起來，求得 更精確的傾角。
[0039] 包括如下步驟：
[0040] 步驟S1:通過衛(wèi)星定位模塊獲取車輛的航向角和瞬時速度W得到地理坐標系上的 加速度;定義地理坐標系上的向量右上角用G標記，載體坐標系上的向量右上角用C標記，載 體水平坐標系上的向量右上角用Η標記；
[0041] 地理坐標系固定在地面上，可W看做慣性系，ΧΥΖ軸方向由東北向定義;載體坐標 系固定在車上，ΧΥΖ軸方向由車的右向、前向和上向定義;載體水平坐標系，其Υ軸方向由載 體坐標系的Υ軸在水平面的投影定義，Ζ軸方向與地理坐標系相同，X軸為Ζ、Υ軸正交右手方 向。因為只設(shè)及姿態(tài)，忽略各坐標系的原點位置，姿態(tài)W矩陣和四元數(shù)的形式運算。車輛姿 態(tài)可W用載體坐標系到地理坐標系的變化表示，包含航向角、俯仰角和橫滾角，而載體水平 坐標系與載體坐標系的區(qū)別是：W上變化不含俯仰角和橫滾角，僅有航向角。
[0042] 具體的，在步驟S1中，根據(jù)公
也理坐標系上的加速度，其中，gG為 地理坐標系上的加速度，為瞬時速度。
[0043] 步驟S2:根據(jù)地理坐標上的加速度換算得到載體水平坐標系上的加速度；
[0044] 具體的，在步驟S2中，根據(jù)公式
換算得到載體水平坐標系上的加速 度，其中，gH為載體水平坐標系上的加速度，Rg為地理坐標系至載體水平坐標系的變換。
[0045] 地理坐標系至載體水平坐標系的變換僅包含航向角，可W利用航向角反算出來， Rg的作用之一是把加速度從地理坐標系變換到載體水平坐標系，因為只包含航向角，因而 Rg就是繞Z軸轉(zhuǎn)航向角的旋轉(zhuǎn)。
[0046] 步驟S3:根據(jù)該載體水平坐標系上的加速度獲取得到載體水平坐標系上的比力， 并求取載體坐標系與載體水平坐標系之間的變換，之后求得理論姿態(tài)。
[0047] 具體的，步驟S3包括如下子步驟：
[004引步驟S31:根據(jù)公式
爵到載體水平坐標系上的比力，其中，為載 體水平坐標系上的比力，gH為重力等效加速度;重力等效加速度的取值為9.8m/s2，為物理 上的已知值；
[0049] 步驟S32:通過加速度計獲取載體坐標系下的比力，記為客C;
[0化0]步驟S33:獲取載體坐標系至載體水平坐標系的變換，記為R?并根據(jù)公式 Rf二(RS)-i.RS求得理論姿態(tài)，其中，R若為理論姿態(tài)。
[0化1]步驟S33中，先把gc旋轉(zhuǎn)得到:§H，得到第一個旋轉(zhuǎn)，用第一個旋轉(zhuǎn)變換Y軸單位向 量，把得到的向量投影到XY平面，再旋轉(zhuǎn)到Y(jié)軸，此為第二個旋轉(zhuǎn)，兩個旋轉(zhuǎn)合起來就是Rg 其中，第一個旋轉(zhuǎn)是主要的，把兩個比力弄成重合，第二個旋轉(zhuǎn)保證最終不破壞兩坐標系的 Y軸在水平面的投影重合的要求;變換可W用矩陣或者四元數(shù)表示，取決于具體需求，數(shù)學 上是等價的。
[0052] 由于車輛實際行駛中，會有各種振動和沖擊，直接用比力計算出的理論姿態(tài)有很 大的干擾，不利于考試評判，因而還需將巧螺儀的信息融合進來，W削弱振動和沖擊的影 響。
[0053] 步驟S4:根據(jù)公式Q = Pi-i · D求取輸出姿態(tài)，其中，Q為巧螺儀積分出的姿態(tài)，D為角 速度乘W時間間隔構(gòu)造的微旋轉(zhuǎn)，Pi為當前輸出姿態(tài)，Pi-l為上一次輸出姿態(tài)；
[0054] 步驟S5:將理論姿態(tài)和輸出姿態(tài)進行插值處理，并將輸出姿態(tài)W四元數(shù)的形式輸 出。
[0055] 步驟S5具體是包括如下子步驟：
[0化6]步驟E1:根據(jù)公式
隱理論姿態(tài)和輸出姿態(tài)進行插值處 理，其中，Pi為輸出姿態(tài)，α為插值權(quán)重，且α的取值范圍為（〇，1)，在實際應(yīng)用中取接近0的 數(shù)；
[0化7]步驟Ε2:將PiW四元數(shù)的形式輸出，即Pi=[w X y zLR^和Q對應(yīng)同一個量，只是 通過不同的方式計算，長期來說是準確的，但是波動大，Q短期很穩(wěn)定，但會慢慢累積誤 差，因此用插值的方法把兩者的優(yōu)點結(jié)合起來。插值的時候用四元數(shù)表示姿態(tài)更加方便和 平滑。
[0058] 姿態(tài)角包括航向角、俯仰角和橫滾角，Ξ個角度有旋轉(zhuǎn)順序，與由線-面角定義的 傾角不同。
[0059] 還可W包括步驟S6:將輸出姿態(tài)通過公式一、公式二和公式Ξ轉(zhuǎn)換成姿態(tài)角輸出；
[0063] 其中，yaw為航向角，pitch為俯仰角，roll為橫滾角。
[0064] 另一方面，本發(fā)明還提供一種機動車輛姿態(tài)的測量裝置，應(yīng)用于機動車輛，該機動 車輛上設(shè)有巧螺儀、衛(wèi)星定位模塊和加速度計;包括如下模塊：
[0065] 定位模塊:用于通過衛(wèi)星定位模塊獲取車輛的航向角和瞬時速度W得到地理坐標 系上的加速度;定義地理坐標系上的向量右上角用G標記，載體坐標系上的向量右上角用C 標記，載體水平坐標系上的向量右上角用Η標記;本模塊中，是根據(jù)公iS
I理 坐標系上的加速度，其中，這G為地理坐標系上的加速度，巧i6為瞬時速度。
[0066] 換算模塊：用于根據(jù)地理坐標上的加速度換算得到載體水平坐標系上的加速度。 具體是根據(jù)公式
矣算得到載體水平坐標系上的加速度，其中，忌Η為載體水平 坐標系上的加速度，民g為地理坐標系至載體水平坐標系的變換。
[0067] 變換模塊:用于根據(jù)該載體水平坐標系上的加速度獲取得到載體水平坐標系上的 比力，并求取載體坐標系與載體水平坐標系之間的變換，之后求得理論姿態(tài)；
[0068] 變化模塊可W包括如下子模塊：
[0069] 相加模塊:用于根據(jù)公式
尋到載體水平坐標系上的比力，其中，§H 為載體水平坐標系上的比力，gH為重力等效加速度；
[0070] 第一獲取模塊:用于通過加速度計獲取載體坐標系下的比力，記為s'E;
[0071] 第二獲取模塊：用于獲取載體坐標系至載體水平坐標系的變換，記為并根據(jù) 公
得理論姿態(tài)，其中，Rf為理論姿態(tài)。
[0072] 求取模塊：用于根據(jù)公式Q = Pi-i . D求取輸出姿態(tài)，其中，Q為巧螺儀積分出的姿 態(tài)，D為角速度乘W時間間隔構(gòu)造的微旋轉(zhuǎn)，Pi為當前輸出姿態(tài)，Pi-1為上一次輸出姿態(tài)；
[0073] 輸出模塊：用于將理論姿態(tài)和輸出姿態(tài)進行插值處理，并將輸出姿態(tài)W四元數(shù)的 形式輸出。
[0074] 輸出模塊具體可包括如下子模塊：
[00巧]處理子模塊：用于根據(jù)公式
將理論姿態(tài)和輸出姿態(tài)進 行插值處理，其中，Pi為輸出姿態(tài)，α為插值權(quán)重；
[0076] 輸出子模塊:用于將PiW四元數(shù)的形式輸出，即Pi=[w X y Ζ]。
[0077] 另外，還可W包括轉(zhuǎn)換模塊:用于將輸出姿態(tài)通過公式一、公式二和公式Ξ轉(zhuǎn)換成 姿態(tài)角輸出；
[0081] 其中，yaw為航向角，pitch為俯仰角，roll為橫滾角。
[0082] 對本領(lǐng)域的技術(shù)人員來說，可根據(jù)W上描述的技術(shù)方案W及構(gòu)思，做出其它各種 相應(yīng)的改變W及形變，而所有的運些改變W及形變都應(yīng)該屬于本發(fā)明權(quán)利要求的保護范圍 之內(nèi)。
【主權(quán)項】
1. 一種機動車輛姿態(tài)的測量方法，應(yīng)用于機動車輛，該機動車輛上設(shè)有陀螺儀、衛(wèi)星定 位模塊和加速度計;其特征在于，包括如下步驟： 步驟A:通過衛(wèi)星定位模塊獲取車輛的航向角和瞬時速度以得到地理坐標系上的加速 度;定義地理坐標系上的向量右上角用G標記，載體坐標系上的向量右上角用C標記，載體水 平坐標系上的向量右上角用Η標記； 步驟Β:根據(jù)地理坐標上的加速度換算得到載體水平坐標系上的加速度； 步驟C:根據(jù)該載體水平坐標系上的加速度獲取得到載體水平坐標系上的比力，并求取 載體坐標系與載體水平坐標系之間的變換，之后求得理論姿態(tài)； 步驟D:根據(jù)公式〇 = · D求取輸出姿態(tài)，其中，Q為陀螺儀積分出的姿態(tài)，D為角速度 乘以時間間隔構(gòu)造的微旋轉(zhuǎn)，Pi為當前輸出姿態(tài)，Ρκ為上一次輸出姿態(tài)； 步驟Ε:將理論姿態(tài)和輸出姿態(tài)進行插值處理，并將輸出姿態(tài)以四元數(shù)的形式輸出。2. 如權(quán)利要求1所述的機動車輛姿態(tài)的測量方法，其特征在于，步驟Α中，根據(jù)公式地理坐標系上的加速度，其中，gG為地理坐標系上的加速度，f為瞬時速度。3. 如權(quán)利要求2所述的機動車輛姿態(tài)的測量方法，其特征在于，步驟B中，根據(jù)公式算得到載體水平坐標系上的加速度，其中，gH為載體水平坐標系上的加速 度，Rg為地理坐標系至載體水平坐標系的變換。4. 如權(quán)利要求3所述的機動車輛姿態(tài)的測量方法，其特征在于，步驟C具體包括如下子 步驟： 步驟C1:根據(jù)公式= 5H + ^?得到載體水平坐標系上的比力，其中，為載體水平 坐標系上的比力，gH為重力等效加速度； 步驟C2:通過加速度計獲取載體坐標系下的比力，記為# , 步驟C3:獲取載體坐標系至載體水平坐標系的變換，記為Rg.片根據(jù)公式求得理論姿態(tài)，其中，R|為理論姿態(tài)。5. 如權(quán)利要求4所述的機動車輛姿態(tài)的測量方法，其特征在于，步驟E具體包括如下子 步驟： 步驟E1:根據(jù)公式巧二. Ct + - α)將理論姿態(tài)和輸出姿態(tài)進行插值處理，其 中，Pi為輸出姿態(tài)，α為插值權(quán)重，且α的取值范圍為(〇，1); 步驟Ε2:將Pi以四元數(shù)的形式輸出，即Pi=[w X y ζ]。6. 如權(quán)利要求4所述的機動車輛姿態(tài)的測量方法，其特征在于，還包括步驟F:將輸出姿 態(tài)通過公式一、公式二和公式三轉(zhuǎn)換成姿態(tài)角輸出；其中，yaw為航向角，pitch為俯仰角，roll為橫滾角。7. -種機動車輛姿態(tài)的測量裝置，應(yīng)用于機動車輛，該機動車輛上設(shè)有陀螺儀、衛(wèi)星定 位模塊和加速度計;其特征在于，包括如下模塊： 定位模塊:用于通過衛(wèi)星定位模塊獲取車輛的航向角和瞬時速度以得到地理坐標系上 的加速度;定義地理坐標系上的向量右上角用G標記，載體坐標系上的向量右上角用C標記， 載體水平坐標系上的向量右上角用Η標記； 換算模塊:用于根據(jù)地理坐標上的加速度換算得到載體水平坐標系上的加速度； 變換模塊：用于根據(jù)該載體水平坐標系上的加速度獲取得到載體水平坐標系上的比 力，并求取載體坐標系與載體水平坐標系之間的變換，之后求得理論姿態(tài)； 求取模塊:用于根據(jù)公式Q = Ph · D求取輸出姿態(tài)，其中，Q為陀螺儀積分出的姿態(tài)，D為 角速度乘以時間間隔構(gòu)造的微旋轉(zhuǎn)，Pi為當前輸出姿態(tài)，Pi-i為上一次輸出姿態(tài)； 輸出模塊：用于將理論姿態(tài)和輸出姿態(tài)進行插值處理，并將輸出姿態(tài)以四元數(shù)的形式 輸出。
【文檔編號】G01C9/00GK105823463SQ201610158753
【公開日】2016年8月3日
【申請日】2016年3月17日
【發(fā)明人】鄢尤帥, 禤永俊
【申請人】廣州展訊信息科技有限公司