一種測(cè)繪一體機(jī)的傾斜測(cè)量方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及測(cè)繪技術(shù)領(lǐng)域，尤其涉及一種測(cè)繪一體機(jī)的傾斜測(cè)量方法。
【背景技術(shù)】
[0002]傳統(tǒng)測(cè)繪一體機(jī)需要測(cè)繪人員利用對(duì)中桿的氣泡使一體機(jī)保持水平，一體機(jī)相位中心點(diǎn)的經(jīng)瑋度即桿子底部測(cè)量點(diǎn)的經(jīng)瑋度，測(cè)量點(diǎn)的高度則是一體機(jī)的相位中心減去已知的桿子底部到一體機(jī)相位中心距離d即可。但為保證精度，這種方法需要測(cè)繪人員維持對(duì)中的時(shí)間，約半分鐘左右，難度較大且影響測(cè)繪效率。最新的測(cè)繪一體機(jī)加入了傾斜測(cè)量的功能，測(cè)繪人員可以在傾斜最大約20度的范圍內(nèi)保持幾秒鐘后計(jì)算出桿底位置。要實(shí)現(xiàn)傾斜測(cè)量，需要有實(shí)時(shí)的一體機(jī)的傾角和方位角，需要角度測(cè)量系統(tǒng)。現(xiàn)有的做法是使用傾角傳感器和磁強(qiáng)傳感器組成的系統(tǒng)來分別直接測(cè)量姿態(tài)角和方位角。但這種做法存在不足:
[0003](I)傾角傳感器只在靜止時(shí)輸出的角度精度才可用，如果由于風(fēng)等外力因素使桿子有晃動(dòng)，則輸出的傾角精度有損失；
[0004](2)在磁環(huán)境碰到突然的變化，例如附近有汽車經(jīng)過時(shí)，方位角會(huì)有較大的誤差，并可能有較大的跳動(dòng)；
[0005](3)在用戶未知磁環(huán)境好壞的情況下，進(jìn)行磁標(biāo)定之后，可能會(huì)造成標(biāo)定結(jié)果帶有較大誤差，直接降低后邊使用時(shí)的精度；
[0006](4)為控制成本，傳感器一般采用低成本的器件，低成本器件的可靠性難以百分百保證。
[0007]綜上所述，現(xiàn)有技術(shù)中的傾斜測(cè)量方法存在輸出的傾角精度低以及方位角有較大的誤差的問題。

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

[0008]本發(fā)明的目的在于提供一種測(cè)繪一體機(jī)的傾斜測(cè)量方法，旨在解決針對(duì)現(xiàn)有技術(shù)中的傾斜測(cè)量方法存在輸出的傾角精度低以及方位角有較大的誤差的問題。
[0009]本發(fā)明是這樣實(shí)現(xiàn)的，一種測(cè)繪一體機(jī)的傾斜測(cè)量方法，所述傾斜測(cè)量方法包括:
[0010]對(duì)陀螺儀傳感器的測(cè)量值進(jìn)行冗余判斷和預(yù)處理后得到第一姿態(tài)角和第一方位角，同時(shí)對(duì)加速度計(jì)傳感器的測(cè)量值和磁強(qiáng)傳感器的測(cè)量值進(jìn)行冗余判斷和預(yù)處理后得到第二姿態(tài)角和第二方位角；
[0011]將所述第二姿態(tài)角與所述第一姿態(tài)角進(jìn)行相減后得到姿態(tài)角的差值，將所述第二方位角與所述第一方位角進(jìn)行相減后得到方位角的差值；
[0012]對(duì)所述姿態(tài)角的差值和所述方位角的差值進(jìn)行預(yù)判定和預(yù)處理后發(fā)送給卡爾曼濾波器進(jìn)行濾波，以得到修正后的姿態(tài)角和方位角。
[0013]對(duì)陀螺儀傳感器、加速度計(jì)傳感器以及磁強(qiáng)傳感器的測(cè)量值進(jìn)行冗余判斷和預(yù)處理的步驟具體為:
[0014]在所述陀螺儀傳感器、所述加速度計(jì)傳感器或所述磁強(qiáng)傳感器中任意選擇兩個(gè)傳感器，根據(jù)其中一個(gè)傳感器的三軸坐標(biāo)、另一個(gè)傳感器的任一軸坐標(biāo)以及所述兩個(gè)傳感器的三軸坐標(biāo)系之間的相對(duì)角度計(jì)算常系數(shù)；
[0015]根據(jù)所述常系數(shù)判斷所述兩個(gè)傳感器的每一個(gè)軸的數(shù)據(jù)是否合格，是，則采用該軸數(shù)據(jù)，否，則放棄該軸數(shù)據(jù)。
[0016]所述根據(jù)其中一個(gè)傳感器的三軸坐標(biāo)、另一個(gè)傳感器的任一軸坐標(biāo)以及所述兩個(gè)傳感器的三軸坐標(biāo)系之間的相對(duì)角度計(jì)算常系數(shù)的步驟具體為:
[0017]根據(jù)以下關(guān)系式計(jì)算常系數(shù):
[0018]al.xl+bl.yl+cl.zl+dl.x2 = O
[0019]xl = [xl 0 0]
[0020]yl = [0 yl 0]
[0021]zl = [0 0 zl]
[0022]x2 = [x2.sin a I.cos β I x2.sin a I.sin β I x2.cos a I]
[0023]其中，al、bl、cl以及dl為常系數(shù)，xl、yl、zl為一個(gè)傳感器的三軸坐標(biāo)，x2為另一個(gè)傳感器的任一軸坐標(biāo)，α 1，β I為兩個(gè)傳感器的三軸坐標(biāo)系之間的相對(duì)角度。
[0024]所述根據(jù)所述常系數(shù)判所述兩個(gè)傳感器的每一個(gè)軸的數(shù)據(jù)是否合格，是，則采用該軸數(shù)據(jù)的步驟具體為:
[0025]根據(jù)以下關(guān)系式計(jì)算常系數(shù)對(duì)某一個(gè)傳感器的每一軸的數(shù)據(jù)進(jìn)行判斷:
[0026]al.xl+bl.yl+cl.zl+dl.x2 ^ 0...(I)
[0027]a2.xl+b2.yl+c2.zl+d2.y2 ^ Cl...(2)
[0028]a3.xl+b3.yl+c3.zl+d3.z2 ^ Cl...(3)
[0029]其中，xl、yl、zl為一個(gè)傳感器的三軸坐標(biāo)，x2、y2、z2為另一個(gè)傳感器的三軸坐標(biāo)；
[0030]當(dāng)公式(I)和公式⑵成立，公式(3)不成立時(shí)，判定z2軸出現(xiàn)故障；當(dāng)公式(I)和公式(3)成立，公式(2)不成立時(shí)，判定y2軸出現(xiàn)故障；當(dāng)公式(2)和公式(3)成立，公式(I)不成立時(shí)，判定x2軸出現(xiàn)故障。
[0031]所述將所述姿態(tài)角的差值和所述方位角的差值經(jīng)過預(yù)判定和預(yù)處理后發(fā)送給卡爾曼濾波器進(jìn)行濾波的步驟中將所述姿態(tài)角的差值進(jìn)行預(yù)判定和預(yù)處理的步驟具體為:
[0032]判斷所述加速度計(jì)傳感器的測(cè)量值與重力的差的絕對(duì)值是否大于第一預(yù)設(shè)值，是，則放棄所述姿態(tài)角差值，否，則判斷所述加速度計(jì)傳感器的測(cè)量值與重力的差的絕對(duì)值是否大于第二預(yù)設(shè)值，是，則放棄所述姿態(tài)角差值并將所述加速度計(jì)傳感器的測(cè)量值進(jìn)行降權(quán)后發(fā)送給卡爾曼濾波器進(jìn)行濾波，否，則將所述姿態(tài)角的差值發(fā)送給卡爾曼濾波器進(jìn)行濾波。
[0033]所述將所述姿態(tài)角的差值和所述方位角的差值經(jīng)過預(yù)判定和預(yù)處理后發(fā)送給卡爾曼濾波器進(jìn)行濾波的步驟中將所述方位角的差值進(jìn)行預(yù)判定和預(yù)處理的步驟具體為:
[0034]判斷卡爾曼濾波器進(jìn)行濾波時(shí)間是否大于第三預(yù)設(shè)值，否，則將所述方位角的差值發(fā)送給卡爾曼濾波器進(jìn)行濾波，是，則判斷所述磁強(qiáng)傳感器的測(cè)量值與當(dāng)?shù)卮拍Ｐ偷牟钪档慕^對(duì)值是否大于第四預(yù)設(shè)值，是，則放棄所述方位角的差值，否，則判斷所述方位角的差值的絕對(duì)值是否大于第五預(yù)設(shè)值，是，則放棄所述方位角的差值，否，則判斷所述方位角的差值的絕對(duì)值是否大于第六預(yù)設(shè)值，是，則放棄所述方位角差值并將所述磁強(qiáng)傳感器的測(cè)量值進(jìn)行降權(quán)后發(fā)送給卡爾曼濾波器進(jìn)行濾波，否，則將所述方位角的差值發(fā)送給卡爾曼濾波器進(jìn)行濾波。
[0035]所述傾斜測(cè)量方法還包括:
[0036]A.采集所述磁強(qiáng)傳感器測(cè)量的磁強(qiáng)數(shù)據(jù)并進(jìn)行保存，同時(shí)將所述磁強(qiáng)數(shù)據(jù)經(jīng)過濾波器濾波后得到修正后的磁強(qiáng)數(shù)據(jù)；
[0037]B.將所述修正后的磁強(qiáng)數(shù)據(jù)與保存的磁強(qiáng)數(shù)據(jù)進(jìn)行殘差計(jì)算后得到殘差統(tǒng)計(jì)量;
[0038]C.判斷所述殘差統(tǒng)計(jì)量是否小于第七預(yù)設(shè)值，否，則判定所述修正后的磁強(qiáng)數(shù)據(jù)無效數(shù)據(jù)，并返回執(zhí)行所述步驟A，是，則執(zhí)行步驟D ;
[0039]D.判斷所述殘差統(tǒng)計(jì)量是否小于第八預(yù)設(shè)值，是，則保存所述修正后的磁強(qiáng)數(shù)據(jù)，否，則刪除所述殘差統(tǒng)計(jì)量中大于第九預(yù)設(shè)值的數(shù)據(jù)，并將剩余的殘差統(tǒng)計(jì)量經(jīng)過濾波器濾波后得到標(biāo)定的磁強(qiáng)數(shù)據(jù)。
[0040]所述步驟A中將所述磁強(qiáng)數(shù)據(jù)經(jīng)過濾波器濾波后得到修正后的磁強(qiáng)數(shù)據(jù)的步驟具體為:
[0041]判斷所述磁強(qiáng)數(shù)據(jù)與所述修正后的磁強(qiáng)數(shù)據(jù)的差值是否小于第十預(yù)設(shè)值，是，則保存修正后的磁強(qiáng)數(shù)據(jù)，否，則重新采集所述磁強(qiáng)傳感器測(cè)量的磁強(qiáng)數(shù)據(jù)。
[0042]本發(fā)明提供一種測(cè)繪一體機(jī)的傾斜測(cè)量方法，采用卡爾曼濾波器來過濾測(cè)繪一體機(jī)的晃動(dòng)與磁場(chǎng)的突變，提高傾斜測(cè)量系統(tǒng)的穩(wěn)定度和可靠度，使用冗余的低成本傳感器進(jìn)行測(cè)量，在降低成本的同時(shí)提升了硬件可靠性，解決了現(xiàn)有技術(shù)中的傾斜測(cè)量方法存在輸出的傾角精度低以及方位角有較大的誤差的問題，并且提高傾斜測(cè)量系統(tǒng)的精度可靠性和可用性，另外在磁強(qiáng)傳感器標(biāo)定過程中，計(jì)算估計(jì)的參數(shù)的質(zhì)量，來標(biāo)示標(biāo)定參數(shù)的質(zhì)量是否合格。
【附圖說明】
[0043]為了更清楚地說明本發(fā)明實(shí)施例中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動(dòng)性的前提下，還可以根據(jù)這些附圖獲得其他的附圖。
[0044]圖1是本發(fā)明一種實(shí)施例提供的一種測(cè)繪一體機(jī)的傾斜測(cè)量方法的流程圖；
[0045]圖2是本發(fā)明另一種實(shí)施例提供的一種測(cè)繪一體機(jī)的傾斜測(cè)量方法的流程圖；
[0046]圖3是本發(fā)明一種實(shí)施例提供的一種測(cè)繪一體機(jī)的傾斜測(cè)量方法中的兩組正交的三軸正