利用單軸陀螺儀、傾角儀和里程計的組合導航裝置及方法
【專利摘要】本發明公開了一種基于單軸陀螺儀、傾角儀和里程計的組合導航裝置及方法，該裝置包含單軸陀螺儀、傾角儀、里程計、空間投影變換模塊、航位推算模塊和卡爾曼濾波模塊；單軸陀螺儀與傾角儀安裝在同一載體平面，單軸陀螺儀提供自身轉動的角速率，傾角儀提供俯仰角，通過空間投影變換算法，將陀螺儀在斜面上的轉動角速率投影為水平面上的角速率。同時，將里程計提供的行進距離投影為平面距離；再利用航位推算算法，根據初始點的位置坐標及方位角，推算出當前坐標及方位角。本發明的優點是：系統結構簡單，算法簡潔，實現了在無GNSS信號時的導航定位；同時，在達到一定精度的情況下降低了裝備成本。
【專利說明】
利用單軸陀螺儀、傾角儀和里程計的組合導航裝置及方法
技術領域
[0001] 本發明專利設及組合導航系統及方法，特別地，設及一種綜合利用單軸巧螺儀、傾 角儀和里程計的組合導航裝置及方法。
【背景技術】
[0002] 常規的車輛等載體定位通常采用GNSS(Global化vigation Satellite System, 全球導航衛星系統)衛星導航定位，在地面上運種方式能夠取得很好的結果。但是在桐穴、 巷道、地下采空區等環境中，無法接收到GNSS衛星信號時，通常會采用慣導導航方式。常規 的慣導系統需要=軸巧螺儀、加速度計等設備實現定位，但是機械巧螺儀精度較低，=軸光 纖巧螺儀價格昂貴，基本上價格是單軸光纖巧螺儀的=倍左右。
[0003] 中國專利CN201210096869.1，單軸巧螺儀和單軸加速度計的車載組合導航系統及 方法公開的定位系統包括GNSS模塊、單軸巧螺儀傳感器、單軸加速度計傳感器、DR模塊、邏 輯控制模塊和卡爾曼濾波器，在GNSS信號有效時，GNSS模塊與D財莫塊協同定位；當GNSS信號 無效時，DR模塊單獨導航，算法非常復雜。中國專利CN20061011810209，利用GI^與巧螺儀、 里程計的組合定位裝置在GPS信號良好時，利用GPS信號進行定位，在GPS信號丟失時，切換 至MEMS巧螺儀、里程計定位。中國專利CN201520358904.1，基于加速度計和巧螺儀的起重機 運行行程和偏斜監測裝置在采用=軸加速度傳感器采集線性加速度信號，用MEMS =軸巧螺 儀采集起重機運行偏斜的角速度信號;再對采集數值進行處理后，轉化為相應的位移變化 量和角速度值。
[0004] 因此，為了在達到一定精度要求下盡可能的減少生產成本，市場需要一種在無法 接收到GNSS衛星信號的情況下，成本更低且能滿足低速運動載體的定位需求的導航設備。
[0005] 發明專利內容
[0006] 為了實現無 GNSS信號時車輛載體等的導航定位，本發明提出了一種綜合利用單軸 巧螺儀、傾角儀和里程計的組合導航裝置及方法，該方法算法簡單，設備成本相對較低，能 夠滿足低速運動載體的定位需求。
[0007] 按照本發明提供的技術方案，一種基于單軸巧螺儀、傾角儀和里程計的組合導航 裝置，包括水平面角速度計算模塊1、航位推算模塊2、里程計3、起始點坐標及方位角模塊4、 卡爾曼濾波器5;
[000引所述水平面角速率計算模塊1包括單軸巧螺儀11、傾角儀12、空間投影變換模塊 13;空間投影變換模塊13獲取單軸巧螺儀11在任意平面上測得的單位時間內的旋轉角度和 傾角儀12測得的俯仰角，獲得水平面上單位時間內旋轉的角度；
[0009] 所述空間投影變換模塊13、所述起始點坐標及方位角模塊4、所述里程計3均與航 位推算模塊2連接;航位推算模塊2利用水平面角速率計算模塊1獲得的單位時間內水平面 內旋轉的角度、里程計3提供的里程信息W及起始點坐標及方位角模塊4提供的起始點坐標 及方位角，推算出當前點的位置坐標；
[0010] 所述航位推算模塊2與所述卡爾曼濾波器5連接。
[0011] 優選的:所述單軸巧螺儀11為單軸光纖巧螺儀。
[0012] 優選的：卡爾曼濾波器5對計算出的位置坐標進行平滑。
[0013] 優選的：單軸巧螺儀11、傾角儀12安裝在某一載體的平面上，且傾角儀的俯仰角轉 動軸垂直于載體縱軸線。
[0014] 本申請還公開了利用上述的組合導航裝置的組合導航方法，包括步驟：
[0015] 空間投影變換模塊13獲取單軸巧螺儀11在任意平面上測得的單位時間內的旋轉 角度和傾角儀12測得的俯仰角，獲得水平面上單位時間內旋轉的角度；
[0016] 航位推算模塊2利用水平面角速率計算模塊1獲得的單位時間內水平面內旋轉的 角度、里程計3提供的里程信息W及起始點坐標及方位角模塊4提供的起始點坐標及方位 角，推算出當前點的位置坐標。
[0017] 本申請綜合利用單軸巧螺儀、傾角儀和里程計的組合導航裝置包括單軸巧螺儀、 傾角儀、里程計、空間投影變換、航位推算、卡爾曼濾波器等。單軸巧螺儀、傾角儀安裝在車 輛載體的某一水平面上，隨載體做剛性運動。且傾角儀俯仰角的旋轉軸垂直于載體平面的 縱軸。里程計提供車輛的里程信息，即相對于初始位置行進了多少距離。
[0018] 車輛載體作低速運動，無論是直線運動還是曲線運動，在足夠小的時間間隔內行 進路線均可W視為直線。單軸巧螺儀測量載體平面在該時間間隔內轉過的角度，傾角儀提 供載體的某一時刻的俯仰角。
[0019] 本方案綜合利用單軸巧螺儀、傾角儀和里程計的組合導航方法步驟如下：
[0020] 第一步:獲取前一時刻記錄的載體俯仰角ai和里程信息&。
[0021] 第二步:獲取當前時刻記錄的載體俯仰角02和里程信息S2。
[0022] 第=步:通過單軸巧螺儀，獲取時間間隔內載體平面轉動的角度O。通過空間投影 變換，將載體在任意斜面轉動的角度《投影在水平面上，得到角度0。
[0023]
[0024] 第四步:起始點平面坐標為^〇,7〇，11〇)，起始方位角為4〇。可獲得當前點坐標^1， yi, hi)及方位角Al為：
[0025] Al = Ao+白
[0026] xi = xo+S*Cosa2Cos Al
[0027] yi = y〇+S*Cosa2Sin Al [002引 hi = ho+S 巧 ina2
[0029] 第五步:重復上述步驟，不斷獲取載體的位置坐標及方位角。
[0030] 與現有技術相比，本發明的有益效果是：
[0031] 充分利用精度高的光纖巧螺儀，從而可W獲得相對準確的設備水平姿態變化值， 同時，利用傾角儀和里程計輔助，在沒有GNSS衛星信號的情況下，準確獲得當前位置坐標。 整個算法相對簡單，相應的設備造價相對較低。
【附圖說明】
[0032] 構成本申請的一部分的附圖用來提供對本發明專利的進一步理解，本發明專利的 示意性實施例及其說明用于解釋本發明專利，并不構成對本發明專利的不當限定。在附圖 中：
[0033] 圖I是本發明申請的整體結構示意圖；
[0034] 圖2是本發明申請的傾角儀俯仰角方向示意圖；
[0035] 圖3是本發明公式推導過程中使用的示意圖；
[0036] 圖4是圖3的右視圖；
[0037] 圖5是圓形切割成多邊形的示意圖；
[0038] 圖6是運動載體的軌跡示意圖；
[0039] 圖7是運動載體的方向變化示意圖；
[0040] 其中，1、水平面角速度計算模塊，11、單軸巧螺儀，12、傾角儀，13、空間投影變換模 塊，
[0041] 2、航位推算模塊，3、里程計，4、起始點坐標及方位角模塊，5、卡爾曼濾波器；
[0042] 6、運動載體，7、車輪。
【具體實施方式】
[0043] W下結合附圖對本發明專利的實施例進行詳細說明，但是本發明專利可W根據權 利要求限定和覆蓋的多種不同方式實施。
[0044] 實施例1:
[0045] -種綜合利用單軸巧螺儀、傾角儀和里程計的組合導航裝置，包括水平面角速度 計算模塊1、航位推算模塊2、里程計3、起始點坐標及方位角模塊4、卡爾曼濾波器5。水平面 角速率計算模塊1包括單軸巧螺儀11、傾角儀12、空間投影變換模塊13;空間投影變換模塊 13獲取單軸巧螺儀11在任意平面上測得的單位時間內的旋轉角度和傾角儀12測得的俯仰 角，獲得水平面上單位時間內旋轉的角度。
[0046] 空間投影變換模塊13、所述起始點坐標及方位角模塊4、所述里程計3均與航位推 算模塊2連接;航位推算模塊2利用水平面角速率計算模塊1獲得的單位時間內水平面內旋 轉的角度、里程計3提供的里程信息W及起始點坐標及方位角模塊4提供的起始點坐標及方 位角，推算出當前點的位置坐標。航位推算模塊2與所述卡爾曼濾波器5連接。
[0047] 單軸巧螺儀、傾角儀安裝在車輛載體的某一水平面上，隨運動載體6做剛性運動。 且傾角儀12俯仰角的旋轉軸垂直于運動載體6所在平面的縱軸，參見圖2,傾角儀12的放置 方向與運動載體6的車輪7所前進的方向一致，其俯仰角方向為圖上箭頭所示，WX軸為旋轉 軸，從紙面向紙面后方轉動。里程計提供車輛的里程信息，即相對于初始位置行進了多少距 離。
[0048] 在起始點位置獲取當前時刻的平面坐標為^〇,7〇，11〇)，載體縱軸起始方位角為八〇。 設置采樣間隔為50化，在獲取角速率、俯仰角、里程值時保證時間同步。
[0049] 車輛載體作低速運動，無論是直線運動還是曲線運動，在足夠小的時間間隔內行 進路線均可W視為直線。
[0050] 單軸巧螺儀可為單軸光纖巧螺儀。單軸光纖巧螺儀實時提供載體的角速率9^，在時 間間隔化內，載體在任意斜面轉動的角度仿=界*姑，對于該時間間隔，前一時刻獲取的載 體俯仰角為〇1，當前時刻的俯仰角為02,行進的斜面距離為S，對其組成的圖形進行幾何分 析，通過空間投影變換，可W將載體在斜面上轉動的角度變換到平面上轉動的角度
[0化1]當前點坐標(xi，yi，hi)及方位角Al可W通過下式計算得到：
[0化2] Al = Ao+白
[0053] xi = xo+S*Cosa2Cos Al
[0054] yi = y〇+S*Cosa2Sin Al [0化5] hi = ho+S 巧 ina2
[0056] 下一時刻的位置坐標可W通過重復上述步驟得到。
[0057] W上公式的推導過程可參見圖3和圖4, W-個普遍的過程為例，前一時刻運動在 位置在0點，行駛的方向為0A，下一時刻載體沿OB行駛到B點，由于當時間間隔盡可能小的時 候載體的形式軌跡可W視為直線，因此W上假設可行。
[005引 OCD所在平面為水平面，OAB所在平面為斜面，OA長度設為Sl，OB長度設為S2，ZAOC (設為a 1)為載體在0點處的垂直傾角，由傾角儀測量，ZBOD(設為a2)為載體在B點時的垂直 傾角，傾角儀測量。其中，C點是A點向水平面作垂線所得的焦點，D點同理。E點是在ABCD平 面中，從B點向AC作垂線所得的交點。BE丄ACdZAOB(設為W )是載體從0點運動到B點轉動的 角度，由巧螺儀測量，ZCOD(設為0)是CO在水平面上的投影，也是計算方位角是需要加上的 角度。
[0化9]由圖3中幾何關系，
[0060] AC = Sl*sinal BD = S2*sina2 0C = Sl*cosal 〇D = S2*cosal
[0061 ] 由余弦定理
[0062]
[0063] 由圖3關系可得
[0064] AE=AC-BD [00化]由勾股定理可得
[0066] CD2 = AB2-AE2
[0067] 將上述關系式代入，即可得到 [006引
[0069] W上即為空間投影變換的公式推導。
[0070] 在實際應用中，如果采用實際設備采集的數據，由于精度問題，無法驗證算法的正 確性，因此采用模擬數據。
[0071] 例如:載體在水平面上運行一個圓周。常規的圓周是一個光滑的弧形，在實際運行 中，可W將圓形盡可能多的切割成多邊形，如圖5。
[0072] 當多邊形邊數足夠多時，可W認為運動載體運行的軌跡是圓形。
[0073] 如圖6的軌跡，X軸指向北方向，y軸指向東方向，初始位置為原點，初始方位角為 0%指向北方向。假設圓形被切割成邊數為20000的多邊形，則多邊形內角和為180*(20000- 2) = 3599640°，每一個內角的補角為0.018°，從圖7中可W看到，即載體運動中每一多邊形 邊長轉動的角度為0.018°。假設每一邊長為1。
[0074] 在起始點0,坐標為北東高(0,0,0)，方位角為0°，經過第一個時刻t運動到A處，由 于在平面上，不需要空間投影變換，0.018°即為上一問題推導的0。由下述公式，
[0075] Al = Ao+白
[0076] xi = xo+S*Cosa2Cos Ai
[0077] yi = y〇+S*Cosa2Sin Al [007引 hi = ho+S 巧 ina2
[0079] A點處坐標為：
[0080] Al=0+0.018 = 0.018。
[0081 ] xl =0+l*cos0*cos0.018 = 0.999999
[0082] yl=0+l*cos0*sin0.018 = 0.00031416
[0083] hi =0+l*sin0 = 0
[0084] 繼續行進到B點：
[0085] A2 = 0.018+0.018 = 0.036。
[0086] x2 = 0.999999+l*cos0*cos0.036 = 1.9999999
[0087] y2 = 0.00094248
[0088] h2 = 0+l*sin0 = 0
[0089] 一直計算下去，最終能夠回到0點，證明算法正確。
[0090] W上所述僅為本發明專利的優選實施例而已，并不用于限制本發明專利，對于本 領域的技術人員來說，本發明專利可W有各種更改和變化。凡在本發明專利的精神和原則 之內，所作的任何修改、等同替換、改進等，均應包含在本發明專利的保護范圍之內。
【主權項】
1. 一種基于單軸陀螺儀、傾角儀和里程計的組合導航裝置，其特征在于:包括水平面角 速度計算模塊（1)、航位推算模塊(2)、里程計(3)、起始點坐標及方位角模塊(4)、卡爾曼濾 波器(5); 所述水平面角速率計算模塊(1)包括單軸陀螺儀（11)、傾角儀（12)、空間投影變換模塊 (13);空間投影變換模塊(13)獲取單軸陀螺儀(11)在任意平面上測得的單位時間內的旋轉 角度和傾角儀(12)測得的俯仰角，獲得水平面上單位時間內旋轉的角度； 所述空間投影變換模塊（13)、所述起始點坐標及方位角模塊(4)、所述里程計(3)均與 航位推算模塊(2)連接;航位推算模塊(2)利用水平面角速率計算模塊（1)獲得的單位時間 內水平面內旋轉的角度、里程計(3)提供的里程信息以及起始點坐標及方位角模塊(4)提供 的起始點坐標及方位角，推算出當前點的位置坐標； 所述航位推算模塊(2)與所述卡爾曼濾波器(5)連接。2. 根據權利要求1所述的一種基于單軸陀螺儀、傾角儀和里程計的組合導航裝置，其特 征在于:所述單軸陀螺儀(11)為單軸光纖陀螺儀。3. 根據權利要求1所述的一種基于單軸陀螺儀、傾角儀和里程計的組合導航裝置，其特 征在于:卡爾曼濾波器(5)對計算出的位置坐標進行平滑。4. 根據權利要求1所述的一種基于單軸陀螺儀、傾角儀和里程計的組合導航裝置，其特 征在于:單軸陀螺儀(11 )、傾角儀(12)安裝在某一載體的平面上，且傾角儀的俯仰角轉動軸 垂直于載體縱軸線。5. 根據權利要求1所述的一種基于單軸陀螺儀、傾角儀和里程計的組合導航裝置，其特 征在于： 載體運動過程中，單軸陀螺儀（11)在單位之間內轉動的角度為ω ;傾角儀（12)測量的 單位時間前后載體的俯仰角分別為α 1，α2; 根據空間投影變換（13)計算出單位時間內載體運動投影到水平面上轉動的角度6. 根據|乂利妥豕1所還的一柙單軸陀螺儀、傾角儀和里程計的組合導航裝置，其特 征在于:起始點平面坐標為(xq，yo，ho)，起始方位角為Αο;由里程計(3)測量的單位時間內載 體進行距離為S;由航位推算模塊(2)可獲得當前點坐標( xl，yi，h〇及方位角^為：7. 利用權利要求1至6任一項所述的組合導航裝置的組合導航方法，其特征在于，包括 步驟： 空間投影變換模塊（13)獲取單軸陀螺儀（11)在任意平面上測得的單位時間內的旋轉 角度和傾角儀(12)測得的俯仰角，獲得水平面上單位時間內旋轉的角度； 航位推算模塊(2)利用水平面角速率計算模塊（1)獲得的單位時間內水平面內旋轉的 角度、里程計(3)提供的里程信息以及起始點坐標及方位角模塊(4)提供的起始點坐標及方 位角，推算出當前點的位置坐標。8.根據權利要求7所述的組合導航方法，其特征在于，步驟具體為： 第一步:獲取前一時刻記錄的載體俯仰角^和里程信息Si; 第二步:獲取當前時刻記錄的載體俯仰角α2和里程信息S2; 第三步：通過單軸陀螺儀，獲取時間間隔內載體平面轉動的角度ω ;通過空間投影變 換，將載體在任意斜面轉動的角度ω投影在水平面上，得到角度Θ ;第四步:起始點平面坐標為(x〇，y〇,ho)，起始方位角為Αο;可獲得當前點坐標(XI，yi,hi) 及方位角Αι為：第五步:重復上述步驟，不斷獲取載體的位置坐標及方位角。
【文檔編號】G01C21/16GK105953797SQ201610564309
【公開日】2016年9月21日
【申請日】2016年7月16日
【發明人】謝翔, 杜年春, 金俊
【申請人】中國有色金屬長沙勘察設計研究院有限公司