一種測繪儀桿底位置濾波估計方法
【專利摘要】本發(fā)明涉及測繪設(shè)備領(lǐng)域，尤其涉及一種測繪儀桿底位置濾波估計方法。本發(fā)明公開設(shè)計的一種測繪儀桿底位置濾波估計方法，先將測繪儀的支撐桿圍繞待測點傾斜旋轉(zhuǎn)若干圈自身傾斜若干獲取旋轉(zhuǎn)過程中多個位置點的位置坐標，然后測量測繪儀支撐桿的桿長，通過拓展卡爾曼濾波算法濾波估計求得測繪儀支撐桿的桿底位置坐標，這樣不僅可以解決地磁變化和磁場干擾對測繪儀傾斜量修正精度的影響，同時縮減了生產(chǎn)成本、減小了測繪儀桿底位置修正模塊的體積及重量、提高了測繪儀桿底位置的估計精度、簡化了估計測繪儀桿底位置的操作修正過程。
【專利說明】
一種測繪儀桿底位置濾波估計方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及測繪設(shè)備領(lǐng)域，尤其涉及一種測繪儀桿底位置濾波估計方法。
【背景技術(shù)】
[0002] 傳統(tǒng)的測繪儀每次測量需要調(diào)節(jié)測量桿垂直，測量耗時大，測量效率低，且一般氣 泡調(diào)零的精度與操作人員的細心程度和技術(shù)相關(guān)，且當測量員長時間高強度測量時，由于 測量員的重復操作與疲勞很容易出現(xiàn)調(diào)平不準，使得測量精度嚴重下降，從而影響工程施 工的精度，造成人力與物力的嚴重浪費。
[0003] 因此，隨著MEMS傾角測量技術(shù)的發(fā)展與應(yīng)用，近些年國內(nèi)外的各大測繪儀生產(chǎn)公 司紛紛推出了帶有傾斜測量補償?shù)臏y繪儀產(chǎn)品，從而將測繪人員從重復的調(diào)平操作中解放 了出來，其主要特點是允許測繪員在使用測繪儀測量時，不需要調(diào)平的情況下依然能夠提 供等效于精確調(diào)平后的打點測量精度，在有效提高測繪效率和測繪精度的同時，提高了測 繪數(shù)據(jù)的一致性，減小了人為調(diào)平引入的各種不可控制的誤差。
[0004] 但是現(xiàn)有使用的傾斜測量模塊由于會使用MEMS傳感器解算輸出的水平傾角和航 向角信息，這些角度信息的精度會受到MEMS傳感器性能的影響，造成測繪儀桿底位置的估 計精度下降，同時，原有測繪儀傾斜改正方法中使用的高性能的工業(yè)級MEMS傳感器，價格 貴、成本高、體積大。因此，從減小生產(chǎn)成本、縮小模塊體積，以及提高測繪儀桿底位置估計 精度的目的出發(fā)，提出了一種基于拓展卡爾曼濾波的新型測繪儀桿底位置濾波估計方法。

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

[0005] 本發(fā)明的目的是提供一種測繪儀桿底位置濾波估計方法，鑒于上述問題，它彌補 了上述的缺陷并提供以下優(yōu)點：
[0006] -種測繪儀桿底位置濾波估計方法，其中，所述方法包括：
[0007] 將測繪儀的支撐桿的底端安放在待測點；
[0008] 將所述測繪儀的支撐桿圍繞所述待測點傾斜旋轉(zhuǎn)若干圈，記錄所述測繪儀旋轉(zhuǎn)過 程中多個位置點的位置坐標；
[0009] 測量所述測繪儀支撐桿長度1〇;
[0010] 利用所述測繪儀的支撐桿長度和所述測繪儀旋轉(zhuǎn)過程中的多個位置點的位置坐 標通過拓展卡爾曼濾波算法對測繪儀的桿底位置及支撐桿桿長誤差進行濾波估計，以計算 出所述測繪儀的桿底位置坐標。
[0011] 上述的方法，其中，所述方法包括：
[0012] 所述測繪儀的支撐桿圍繞所述支撐桿底端以8字型或圓形方式傾斜旋轉(zhuǎn)。
[0013] 上述的方法，其中，所述方法還包括：
[0014]將所述測繪儀的支撐桿傾斜旋轉(zhuǎn)過程中的采集的位置坐標存儲至所述測繪儀的 存儲單元中；
[0015]輸入所述測繪儀的支撐桿桿長，并于所述測繪儀的微型處理器中通過拓展卡爾曼 濾波算法對所述測繪儀的支撐桿長度和存儲在所述存儲單元中的位置坐標進行濾波估計。
[0016] 上述的方法，其中，所述方法包括：
[0017] 通過拓展卡爾曼濾波估計算法對所述測繪儀的支撐桿桿底位置坐標[x，y，z]及支 撐桿的桿長誤差A 1〇進行估計計算，根據(jù)拓展卡爾曼狀態(tài)方程XkiFXk-i和量測方程
+ Δ/σ計算得出；
[0018] 其中F為單位矩陣，Xk-iStk-i時刻的狀態(tài)量X，Xk為tk時刻的狀態(tài)量，狀態(tài)量Χ=[χ， y，ζ，△ 10]，包含支撐桿桿底位置坐標[X，y，ζ ]及支撐桿的桿長誤差△ Ιο; Ζ為觀測量，選取支 撐桿長度1〇為觀測量，[?,ζ1]為所述存儲單元中的第i個點位置坐標(01)。
[0019] 上述的方法，其中，所述方法還包括：
[0020] 按照 、
，
"五個迭代更新方程，通過拓展卡爾 曼濾波方法估計狀態(tài)量；
[0021 ]其中，Λ?為tk時刻狀態(tài)量的一步預測值，為^-:時刻狀態(tài)量的估計值，κ為tk時 刻的濾波增益矩陣，巧為tk時刻均方誤差矩陣的一步預測值，ΡΛ為時刻均方誤差矩陣 的估計值，iT為tk時刻均方誤差矩陣的估計值，H k為量測方程一階線性化后的量測矩陣， Μ 1為狀態(tài)量足對應(yīng)的量測方程，上標T表不矩陣的轉(zhuǎn)置，Rk為4時刻量測噪聲，Qk為燦寸 刻狀態(tài)噪聲陣。
[0022]上述的方法，其中，所述方法還包括：
[0023]將估計得到的桿底位置直角坐標[X，y，z ]和桿長誤差Λ 1 0代入

ζ中，即可得到測繪儀桿底位置的經(jīng)煒高估計值[A，L，h]; h = z Iq - Δ/q + h
[0024] 其中，r為地球半徑，λ1丄1 A1分別表示所述存儲單元中存儲的第一個點的經(jīng)度、煒 度和高度。
[0025] 綜上所述，本發(fā)明公開設(shè)計的一種測繪儀桿底位置濾波估計方法，通過將測繪儀 的支撐桿圍繞待測點傾斜旋轉(zhuǎn)若干圈獲取旋轉(zhuǎn)過程中多個位置點的位置坐標，結(jié)合測量的 測繪儀支撐桿的桿長，采用拓展卡爾曼濾波算法濾波估計求得測繪儀支撐桿的桿底位置坐 標，這樣不僅可以解決地磁變化和磁場干擾對測繪儀傾斜量修正精度的影響，同時縮減了 生產(chǎn)成本、減小了測繪儀桿底位置修正模塊的體積及重量、提高了測繪儀桿底位置的估計 精度、簡化了估計測繪儀桿底位置的操作修正過程。
【附圖說明】
[0026] 參考所附附圖，以更加充分的描述本發(fā)明的實施例。然而，所附附圖僅用于說明和 闡述，并不構(gòu)成對本發(fā)明范圍的限制。
[0027] 圖1是本發(fā)明的流程圖。
[0028]圖2是本發(fā)明的測繪儀的支撐桿傾斜旋轉(zhuǎn)示意圖。
【具體實施方式】
[0029]下面結(jié)合附圖和具體的實施例對本發(fā)明作進一步的說明，但是不作為本發(fā)明的限 定。
[0030] 目前的測繪儀的主要工作原理就是在測繪儀內(nèi)部安裝一個低成本的傾角測量模 塊，通過傾角測量模塊實時測量傾斜角和航向角，結(jié)合傾斜后的經(jīng)煒高，利用精確的算法補 償修正方法，改正傾斜導致的測量誤差，獲得精確的打點經(jīng)煒高測量參數(shù)。但該誤差改正算 法精度與航向角的測量精度直接相關(guān)，而由于低成本傾角測量模塊，一般使用地磁傳感器 獲取航向信息，而地磁傳感器的測量原理主要依賴于微弱的地磁場信息，所以該航向角的 精度很容易受測繪儀內(nèi)部的線圈、鐵磁物質(zhì)，以及測繪儀周邊的礦場分布、鋼鐵建筑、設(shè)備 的影響，所以一般的使用精度不是很理想。
[0031] 雖然，所有傾角計模塊都會針對這些地磁干擾以及磁傳感器的誤差進行校準或者 誤差補償研究，但由于地磁相對微弱，且磁干擾形式復雜、干擾源眾多、對環(huán)境依賴性強，即 使是先進的校準算法加上嚴格的校準流程，也很難保證磁航向的測量精度在所有使用情況 下都能滿足傾斜改正測量的應(yīng)用需求，一般在充分標定的情況下，磁航向角精度也很難達 到1度以下的精度。
[0032] 另外，由于補償算法中用到的航向是真航向而非磁航向，因此，我們需要對測量到 的磁航向進行修正，其中磁偏角的獲取一般通過如下兩類方法獲得：
[0033 ]方法一、通過當?shù)亟?jīng)煒高利用地磁偏角模型查詢。
[0034]方法二、傳統(tǒng)四位置或八位置羅差修正方法獲得。
[0035] 其中，方法一的特點是使用方便，只需要在程序中固化一個不太復雜的地磁模型， 但由于精確的地磁數(shù)據(jù)庫巨大，且更新困難，因此，一般固化使用的地磁偏角模型都較為粗 略，無法反映細小區(qū)域的細微變化，其精度也只能勉強達到1度左右，且隨著時間的推移，地 球磁場也在發(fā)生著變化，如無法及時更新數(shù)據(jù)庫，誤差也會更大。因此，利用此方法補償?shù)?磁航向獲得的航向角精度一般在全航向測量范圍內(nèi)只能達到2度左右的使用精度，且很難 進一步提尚。
[0036] 方法二一般利用RTK接收的精確位置測量特性，進行多位置差分，獲得精確的航向 參考，計算多個角度的航向偏差，可以獲得較高的真航向參考，一般RTK精度在2cm，水平基 線在lm左右，可以獲得0.5度航向的精確參考，同時可以修正磁標定后的磁場畸變殘差，提 高航向角在全范圍的精度和誤差均勻性，但該方法標定過程復雜，一般需要熟練的技術(shù)人 員利用半小時左右才能操作完成。且其標定精度也與操作人員的細心和耐心存在很大的關(guān) 系，因此，實際航向測量精度一般在1度左右。
[0037] 因此，為了消除由于磁航向測量不準或易受干擾導致的測繪儀位置修正誤差，需 要采用一定的改進方式來消除航向測量誤差，達到在不使用磁航向測量信息的條件下，實 現(xiàn)測繪儀位置修正的目的。本發(fā)明設(shè)計了一種測繪儀桿底位置濾波估計的方法。
[0038]如圖1所示，首先將測繪儀傾斜旋轉(zhuǎn)若干圈，但是測繪儀是固定在測繪儀支撐桿的 一端的，測繪儀支撐桿的另一端固定在待測點上，本申請中于待測點上設(shè)置一固定裝置，測 繪儀支撐桿的另一端通過旋轉(zhuǎn)裝置與固定裝置連接，這樣就可以將測繪儀的支撐桿以一端 為固定點進行傾斜旋轉(zhuǎn)。在傾斜旋轉(zhuǎn)過程中，測繪儀自動記錄每次傾斜旋轉(zhuǎn)后的位置坐標， 另外外部再量測測繪儀的支撐桿的長度，因為支撐桿的一端是固定的，這樣在傾斜旋轉(zhuǎn)過 程中記錄測繪儀所處的位置坐標，然后采用拓展卡爾曼濾波算法，結(jié)合這些位置坐標和測 繪儀支撐桿桿長，濾波估計出測繪儀支撐桿的桿底位置坐標。
[0039] 實施例一：
[0040] (1)事先測量測繪儀支撐桿長度1〇。
[0041] (2)將測繪儀的支撐桿底端安放在待測點，本申請中待測點上設(shè)置一個固定裝置， 測繪儀支撐桿底端則通過一個旋轉(zhuǎn)裝置與固定裝置連接，保證支撐桿在圍繞底端傾斜畫圈 旋轉(zhuǎn)時，桿底位置固定不動。
[0042] (3)將測繪儀的支撐桿底端固定后，如圖2所示，將支撐桿底端以8字型或圓形或螺 旋型進行傾斜畫圈旋轉(zhuǎn)；
[0043] (4)在傾斜畫圈旋轉(zhuǎn)過程中，測繪儀自動記錄時間段(t^U)中相等時間間隔的測 繪儀多個位置點的位置坐標[A (1)，L(1)，h(1)]~以")，1> )，，)]，其中人表示經(jīng)度丄表示煒度、 h表示高度，該位置是支撐桿上安裝有位置測量模塊和微型中央處理器的一端，如圖2中所 示的位置A。
[0044] (5)在測繪儀的存儲單元中讀取記錄的位置坐標數(shù)組[X?，L(1)，h (1)]~[A(n)，L(n)， h(n)]，于測繪儀的微型處理器中進行卡爾曼濾波估計，首先是選擇其中某一位置為直角坐 標系原點，將所有位置的經(jīng)煒高坐標轉(zhuǎn)換為直角坐標。實施例中選擇第一個位置坐標點為 坐標原點[0,0,0]，其余位置坐標轉(zhuǎn)換為直角坐標的過程如式（1)所示，其中r為地球半徑。 轉(zhuǎn)換后的位置坐標數(shù)組為[0,0,0]，[x( 2)，y(2)，z⑵]~[xw，yw， zw]。
[0045]
[0046] (6)基于轉(zhuǎn)換得到的各個位置點的直角坐標，構(gòu)建濾波估計方程。其中濾波估計方 程中狀態(tài)量設(shè)為乂=[1，7,2,410]，其中4, 7,2]為待估計的桿底直角坐標，41〇為桿長誤差 估計值;量測量選取為桿長測量值Z = l。。構(gòu)建的狀態(tài)方程和量測方程如式(2)和(3)所示。
[0047] Xk = FXk-i (2)
[0048]
(3)
[0049] 其中F為單位矩陣，Xk-iStk-i時刻的狀態(tài)量X，Xk為tk時刻的狀態(tài)量，狀態(tài)量X=[x， y，z，Alo]包含支撐桿桿底位置坐標[x，y，z]及支撐桿的桿長誤差Δ1ο;Ζ為觀測量，選取支 撐桿長度1〇為觀測量，[?,ζ 1]為所述存儲單元中的第i個點位置坐標(01)。
[0050] (7)基于構(gòu)建的濾波估計方程，通過拓展卡爾曼濾波方法對狀態(tài)量進行估計，估計 過程中用到的各個參量的更新過程如式(4)~(9)所示。其中R k為桿長測量噪聲，Qk為待修正 估計直角坐標[x，y，z]和桿長誤差Δ1〇的測量噪聲。在微型中央處理器中，通過帶入選取的 多個位置點直角坐標[0,0,0]，[x (2)，y(2)，ζ(2)]~[x(n)，y(n)，z (n)]，進行濾波估算得到桿底直 角坐標[x，y，z]以及桿長誤差的估計值Δ 1〇。
[0051]

[0057] K為tk時刻狀態(tài)量的一步預測值，XL為tk-^刻狀態(tài)量的估計值，K為tk時刻的 濾波增益矩陣，A為t k時刻均方誤差矩陣的一步預測值，刻均方誤差矩陣的估 計值，6+為tk時刻均方誤差矩陣的估計值，H k為量測方程一階線性化后的量測矩陣， 為狀態(tài)量4對應(yīng)的量測方程，上標T表不矩陣的轉(zhuǎn)置，Rk為tk時刻量測噪聲，Qk為tk時刻狀 態(tài)噪聲陣。
[0058] (8)將估計得到的桿底直角坐標[x，y，z]和Δ1〇代入式（10)，即可得到測繪儀桿底 位置的坐標估計值[A，L，h]。
[0059]
[0060] [A，L，h]為測繪儀桿底位置的經(jīng)煒高，λ1丄1 A1分別表示所述存儲單元中存儲的第 一個點的經(jīng)度、煒度和高度。
[0061] 綜上所述，本發(fā)明設(shè)計的一種新型測繪儀桿底位置濾波估計方法，先將測繪儀傾 斜畫圈旋轉(zhuǎn)，通過記錄畫圈旋轉(zhuǎn)過程中多個點的位置坐標以及測量的支撐桿桿長，通過拓 展卡爾曼濾波算法估計桿底位置坐標，這樣可以解決地磁變化和磁場干擾對測繪儀傾斜量 修正精度的影響，同時減少了傾斜測量模塊中傾角傳感器引入的測量誤差，提高了測繪儀 的傾斜修正精度、使用效率、同時減小了生產(chǎn)成本以及縮小了修正模塊的體積和重量。 [0062]通過說明和附圖，給出了【具體實施方式】的特定結(jié)構(gòu)的典型實施例，基于本實用新 型精神，還可作其他的轉(zhuǎn)換。盡管上述發(fā)明提出了現(xiàn)有的較佳實施例，然而，這些內(nèi)容并不 作為局限。
[0063] 對于本領(lǐng)域的技術(shù)人員而言，閱讀上述說明后，各種變化和修正無疑將顯而易見。 因此，所附的權(quán)利要求書應(yīng)看作是涵蓋本發(fā)明的真實意圖和范圍的全部變化和修正。在權(quán) 利要求書范圍內(nèi)任何和所有等價的范圍與內(nèi)容，都應(yīng)認為仍屬本發(fā)明的意圖和范圍內(nèi)。
【主權(quán)項】
1. 一種測繪儀桿底位置濾波估計方法，其特征在于，所述方法包括： 將測繪儀的支撐桿的底端安放在待測點； 將所述測繪儀的支撐桿圍繞所述待測點傾斜旋轉(zhuǎn)若干圈，記錄所述測繪儀旋轉(zhuǎn)過程中 多個位置點的位置坐標； 測量所述測繪儀支撐桿長度1〇; 通過拓展卡爾曼濾波算法對所述測繪儀的支撐桿長度和所述測繪儀旋轉(zhuǎn)過程中的多 個位置點的位置坐標進行濾波估計，W計算出所述測繪儀的桿底位置坐標。2. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述方法包括： 所述測繪儀的支撐桿圍繞所述支撐桿底端W8字型或圓形方式傾斜旋轉(zhuǎn)。3. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述方法還包括： 將所述測繪儀的支撐桿傾斜旋轉(zhuǎn)過程中的采集的位置坐標存儲至所述測繪儀的存儲 單元中； 輸入所述測繪儀的支撐桿桿長，并于所述測繪儀的微型處理器中通過拓展卡爾曼濾波 算法對所述測繪儀的支撐桿長度和存儲在所述存儲單元中的位置坐標進行濾波估計。4. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述方法包括： 通過拓展卡爾曼濾波估計算法對所述測繪儀的支撐桿桿底位置坐標及支撐桿的 桿長誤差A 1〇進行估計計算，根據(jù)拓展卡爾曼狀態(tài)方程Xk = FXk-i和量測方程計算得出； 其中F為單位矩陣，Xk-i為tk-i時刻的狀態(tài)量X，Xk為tk時刻的狀態(tài)量，狀態(tài)量X=[x，y，z, A 1〇]，包含支撐桿桿底位置坐標[x，y，z]及支撐桿的桿長誤差Δ l〇;Z為觀測量，選取支撐桿 長度1〇為觀測量，[xi，yi，zi]為所述存儲單元中的第i個點的位置坐標。5. 根據(jù)權(quán)利要求4所述的方法，其特征在于，所述方法還包括： 按照五個迭代更新方程，通過拓展卡爾 曼濾波方法估計狀態(tài)量； 其中，馬為tk時刻狀態(tài)量的一步預測值，式4為tk-i時刻狀態(tài)量的估計值，K為tk時刻的 濾波增益矩陣，巧^為tk時刻均方誤差矩陣的一步預測值，/色為tk-l時刻均方誤差矩陣的估 計值，巧為tk時刻均方誤差矩陣的估計值，化為量測方程一階線性化后的量測矩陣， 為狀態(tài)量馬對應(yīng)的量測方程，上標T表示矩陣的轉(zhuǎn)置，Rk為tk時刻量測噪聲，Qk為tk時刻狀 態(tài)噪聲陣。6. 根據(jù)權(quán)利要求5所述的方法，其特征在于，所述方法還包括： 將估計得到的桿底直角坐標[X，y，Z巧日桿長誤差Δ 1〇代入 L=x/(r+hi)+Li 入= y/( (r+hi)cosLi)+Ai h = z+l〇- Δ lo+hi 式中，即可得到測繪儀桿底位置的經(jīng)締高估計值[λ,L，h]; 其中，r為地球半徑，λ?,?Λκ?分別表示所述存儲單元中存儲的第一個點的經(jīng)度、締度和 高度。
【文檔編號】G01C15/00GK105973210SQ201610334404
【公開日】2016年9月28日
【申請日】2016年5月19日
【發(fā)明人】杭義軍, 邢麗, 賈文峰, 呂印新
【申請人】極翼機器人(上海)有限公司