專利名稱：一種基于陀螺全站儀－激光標靶的盾構機自動導向系統(tǒng)的在線標定方法
技術領域：
本發(fā)明涉及一種基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的在線標定方法，可用于在線修正基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的陀螺漂移。

背景技術：
盾構機自動導向系統(tǒng)是一種集測量、儀器儀表和計算機軟硬件技術于一體，具有對盾構機位置姿態(tài)進行動態(tài)測量功能的系統(tǒng)，基于陀螺全站儀-激光標靶自動導向系統(tǒng)主要由陀螺全站儀、激光標靶、后視參考棱鏡和自動導向系統(tǒng)計算機組成，其中陀螺全站儀主要由捷聯式陀螺尋北儀和激光全站儀組成。該系統(tǒng)具有精度高、自主尋北、操作簡便、僅需要一個后視已知坐標點的優(yōu)點。捷聯式陀螺尋北儀是自動導向系統(tǒng)的核心部件之一，其精度直接決定了系統(tǒng)自尋北精度，因此必須通過標定試驗確定出陀螺尋北儀的各項誤差系數，并在系統(tǒng)中進行補償。
相關研究表明，陀螺尋北儀的各項誤差系數并不是固定不變的，尤其是陀螺漂移，每次啟動都不相同，隨著系統(tǒng)的使用或存放時間的推移而變化。因此，通常需要對盾構機自動導向系統(tǒng)進行半年或三個月一次的定期標定，標定的工作繁瑣而復雜，需要專業(yè)人員使用專用的試驗設備才能完成，這為使用單位帶來了工程應用中的難題。


發(fā)明內容
本發(fā)明的技術解決問題是克服現有技術的不足，提供一種基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的在線標定方法，該方法實現了在自動導向系統(tǒng)作業(yè)過程中的在線標定，免去了盾構機自動導向系統(tǒng)進行半年或三個月一次的定期標定，同時也提高了系統(tǒng)的自動導向精度。
本發(fā)明的技術解決方案為一種基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的在線標定方法，其特點在于包括下列步驟 (1)捷聯式陀螺尋北儀進行初始對準，確定捷聯式陀螺尋北儀的初始水平方位角_0、初始俯仰角θ0和初始橫滾角γ0； (2)具有自動鎖定目標功能的激光全站儀發(fā)射激光到激光標靶下部的反射棱鏡，然后自動鎖定激光標靶下部的反射棱鏡，并連續(xù)地測量捷聯式陀螺尋北儀的水平方位角_，測量結果用符號_l表示； (3)捷聯式陀螺尋北儀進入捷聯姿態(tài)解算狀態(tài)，利用步驟(1)計算得到的初始水平方位角_0、初始俯仰角θ0和初始橫滾角γ0初始化捷聯姿態(tài)解算狀態(tài)，然后根據陀螺尋北儀中陀螺的輸出實時連續(xù)解算捷聯式陀螺尋北儀的水平方位角_、俯仰角θ和橫滾角γ； (4)采用捷聯式陀螺尋北儀中的兩個加速度的輸出計算出俯仰角θ和橫滾角γ，計算結果用θα和γα表示； (5)將步驟(3)計算得到的_、θ和γ分別與步驟(2)計算得到的_l、步驟(4)計算得到的θα和γα相減，然后將_-_l、θ-θα和γ-γα作為觀測量，采用卡爾曼濾波器在線估計陀螺漂移，并在捷聯式陀螺尋北儀中進行補償。
本發(fā)明的原理是捷聯式陀螺尋北儀的各項誤差系數并不是固定不變的，尤其是陀螺漂移，每次啟動都不相同，隨著系統(tǒng)的使用或存放時間的推移而變化。因此，通常需要對盾構機自動導向系統(tǒng)進行半年或三個月一次的定期標定。由于盾構機自動導向系統(tǒng)作業(yè)時，陀螺全站儀固定在隧道管壁上，并且跟蹤固定在盾構機上的激光標靶，其水平姿態(tài)不隨時間變化，其水平方位角隨著盾構機的掘進非常緩慢的變化(該變化可通過激光全站儀精確測量)，因此捷聯式陀螺尋北儀工作過程中的水平方位角、俯仰角和橫滾角都可精確測量得到，將這三個測量值作為觀測量，采用卡爾曼濾波器可以精確估計捷聯式陀螺尋北儀的陀螺漂移。同時，采用卡爾曼濾波器估計捷聯式陀螺尋北儀的陀螺漂移，必須要求捷聯式陀螺尋北儀進行捷聯姿態(tài)解算，但由于捷聯式陀螺尋北儀只能測量X向和Y向的角速度，無法進行捷聯姿態(tài)解算。同樣利用捷聯式陀螺尋北儀處于靜止狀態(tài)這一特點，利用精確測量到的捷聯式陀螺尋北儀的水平方位角、俯仰角和橫滾角可以計算出Z軸角速度增量，這樣利用X、Y和Z三個方向的角速度就可實現了捷聯姿態(tài)解算，最終實現了利用卡爾曼濾波器在線估計陀螺漂移。
本發(fā)明與現有技術相比的優(yōu)點在于本發(fā)明實現了陀螺全站儀-激光標靶自動導向系統(tǒng)的在線標定，免去了復雜的定期標定，同時也提高了自動導向系統(tǒng)的精度。



圖1為基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的結構框圖； 圖2為本發(fā)明的原理框圖； 圖3為本發(fā)明的卡爾曼濾波基本算法的解算流程圖。

具體實施例方式 如圖1所示，基于陀螺全站儀和激光標靶的盾構機自動導向系統(tǒng)，主要由陀螺全站儀、激光標靶、自動導向系統(tǒng)計算機和后視參考棱鏡組成，其中陀螺全站儀由捷聯式陀螺尋北儀和激光全站儀組成；陀螺尋北儀進行自主尋北并確定陀螺全站儀的坡度角和滾轉角；激光全站儀發(fā)射激光到后視參考棱鏡和激光標靶以測量陀螺全站儀到后視參考棱鏡和激光標靶的距離，并根據后視參考棱鏡提供位置坐標，計算激光全站儀的坐標和激光標靶的坐標；激光標靶將激光全站儀發(fā)射的激光按入射方向反射，并測量其軸線與入射激光的夾角以及盾構機的滾轉角和坡度角；最終由自動導向系統(tǒng)計算機采集陀螺全站儀和激光標靶的輸出，并計算出盾構機的水平方位角。
如圖2所示，本發(fā)明的具體方法如下 (1)捷聯式陀螺尋北儀進行初始對準，確定捷聯式陀螺尋北儀的初始水平方位角_0、初始俯仰角_0和初始橫滾角γ0； (2)具有自動鎖定目標功能的激光全站儀發(fā)射激光到激光標靶下部的反射棱鏡，然后自動鎖定激光標靶下部的反射棱鏡，并連續(xù)地測量捷聯式陀螺尋北儀的水平方位角_，測量結果用符號_l表示； (3)捷聯式陀螺尋北儀進入捷聯姿態(tài)解算狀態(tài)，利用步驟(1)計算得到的初始水平方位角_0、初始俯仰角θ0和初始橫滾角γ0初始化捷聯姿態(tài)解算狀態(tài)，然后根據陀螺尋北儀中陀螺的輸出實時連續(xù)解算捷聯式陀螺尋北儀的水平方位角_、俯仰角θ和橫滾角γ，具體步驟如下 (a)根據捷聯式陀螺尋北儀初始對準得到的捷聯式陀螺尋北儀初始的水平方位角_0、俯仰角θ0和橫滾角γ0計算初始姿態(tài)矩陣Cbn(0)和四元數q(0)，計算公式如下
令 則有 (b)利用_l、θ0和γ0計算捷聯式陀螺尋北儀的z軸角速度增量Δθz Δθz＝ωiecoslatsin_lsinγ0-ωiecoslatsinθ0cos_lcosγ0+ωiesinlatcosθ0cosγ0 (c)更新四元數和姿態(tài)矩陣 其中， 姿態(tài)矩陣Cbn的更新公式如下 (d)計算捷聯式陀螺尋北儀的水平方位角_
主值判斷如下表 (e)計算捷聯式陀螺尋北儀的俯仰角θ θ＝sin-1(T23) (9) (f)計算捷聯式陀螺尋北儀的橫滾角γ 主值判斷如下表 (4)根據捷聯式陀螺尋北儀中的兩個加速度的輸出fx和fy，采用下面的公式計算出俯仰角θ和橫滾角γ，計算結果用θα和γα表示 其中，g表示重力加速度。
(5)將步驟(3)計算得到的_、θ和γ分別與步驟(2)計算得到的_l、步驟(4)計算得到的θα和γα相減，然后將_-_l、θ-θα和γ-γα作為觀測量，采用卡爾曼濾波器在線估計陀螺漂移，并在捷聯式陀螺尋北儀中進行補償，采用的卡爾曼濾波器為 狀態(tài)方程 其中，X為系統(tǒng)狀態(tài)矢量，W為系統(tǒng)噪聲矢量，F為系統(tǒng)轉移矩陣，G為噪聲轉換矩陣 X＝[φx φy φz εx εy εz]T 系統(tǒng)的量測方程 Z＝HX+η (14) 其中Z為觀測矢量，H為觀測矩陣，η為量測噪聲 Z＝[φx φy φz]T H＝[I3×3 03×3] η＝[ηφx ηφy ηφz]T 令有 φx＝sin-1(C23)(15) φz主值判斷如下表 其中，

卡爾曼濾波基本算法編排，該算法的流程圖如圖3所示 狀態(tài)一步預測方程 狀態(tài)估值計算方程 濾波增量方程 一步預測均方誤差方程 估計均方誤差方程
權利要求
1、一種基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的在線標定方法，其特征在于包括下列步驟
(1)捷聯式陀螺尋北儀進行初始對準，確定捷聯式陀螺尋北儀的初始水平方位角_0、初始俯仰角θ0和初始橫滾角γ0；
(2)具有自動鎖定目標功能的激光全站儀發(fā)射激光到激光標靶下部的反射棱鏡，然后自動鎖定激光標靶下部的反射棱鏡，并連續(xù)地測量捷聯式陀螺尋北儀的水平方位角_，測量結果用符號_l表示；
(3)捷聯式陀螺尋北儀進入捷聯姿態(tài)解算狀態(tài)，利用步驟(1)計算得到的初始水平方位角_0、初始俯仰角θ0和初始橫滾角γ0初始化捷聯姿態(tài)解算狀態(tài)，然后根據陀螺尋北儀中陀螺的輸出實時連續(xù)解算捷聯式陀螺尋北儀的水平方位角_、俯仰角θ和橫滾角γ；
(4)采用捷聯式陀螺尋北儀中的兩個加速度的輸出計算出俯仰角θ和橫滾角γ，計算結果用θa和γa表示；
(5)將步驟(3)計算得到的_、θ和γ分別與步驟(2)計算得到的_l、步驟(4)計算得到的θa和γa相減，然后將_-_l、θ-θa和γ-γa作為觀測量，采用卡爾曼濾波器在線估計陀螺漂移，并在捷聯式陀螺尋北儀中進行補償。
2、根據權利要求1所述的基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的在線標定方法，其特征在于步驟(3)所述的捷聯姿態(tài)解算狀態(tài)的具體步驟為
(1)根據捷聯式陀螺尋北儀初始對準得到的捷聯式陀螺尋北儀的初始水平方位角_0、初始俯仰角θ0和初始橫滾角γ0計算初始姿態(tài)矩陣Cbn(0)和四元數q(0)，計算公式如下
令
則有
(2)利用_l、θ0和γ0計算捷聯式陀螺尋北儀的z軸角速度增量Δθz
Δθz＝ωie coslat sin_l sinγ0-ωie coslat sinθ0 cos _l cosγ0+ωie sin lat cos θ0 cosγ0；
(3)更新四元數和姿態(tài)矩陣
其中，
姿態(tài)矩陣Cbn的更新公式如下
(4)計算捷聯式陀螺尋北儀的水平方位角_
主值判斷如下表
(5)計算捷聯式陀螺尋北儀的俯仰角θ
θ＝sin-1(T23)；
(6)計算捷聯式陀螺尋北儀的橫滾角γ
主值判斷如下表
3、根據權利要求1所述的一種基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的在線標定方法，其特征在于步驟(5)所述的卡爾曼濾波器為狀態(tài)方程
其中，X為系統(tǒng)狀態(tài)矢量，W為系統(tǒng)噪聲矢量，F為系統(tǒng)轉移矩陣，G為噪+聲轉換矩陣
X＝[φx φy φz εx εy εz]T
系統(tǒng)的量測方程
Z＝HX+η
其中Z為觀測矢量，H為觀測矩陣，η為量測噪聲
Z＝[φx φy φz]T
H＝[I3×3 03×3
η＝[ηφx ηφy ηφz]T
令有
φx＝sin-1(C23)
φz主值判斷如下表
其中，
4、根據權利要求1所述的一種基于陀螺全站儀-激光標靶的盾構機自動導向系統(tǒng)的在線標定方法，其特征在于所述步驟(4)中采用捷聯式陀螺尋北儀中的兩個加速度的輸出計算出俯仰角θ和橫滾角γ如下陀螺尋北儀中的兩個加速度的輸出fx和fy，采用下面的公式計算出俯仰角θ和橫滾角γ，計算結果用θa和γa表示
其中，g表示重力加速度。
全文摘要
本發(fā)明涉及一種基于陀螺全站儀－激光標靶的盾構機自動導向系統(tǒng)的在線標定方法，其特征是在盾構機掘進過程中，利用激光全站儀跟蹤激光標靶下部的棱鏡，測量捷聯式陀螺尋北儀的水平方位角變化，同時利用陀螺尋北儀中的兩個加速度計計算陀螺尋北儀的俯仰角和橫滾角，然后利用這三個精確測量得到的姿態(tài)角計算出捷聯式陀螺尋北儀無法測量的Z軸角速度，再與陀螺測量的X和Y軸角速度組合，即可進行捷聯姿態(tài)解算，最后將三個精確測量得到的姿態(tài)角作為觀測量，采用卡爾曼濾波器在線估計陀螺漂移，并在系統(tǒng)中進行補償。本發(fā)明具有在線消除陀螺漂移誤差的優(yōu)點，可用于提高自動導向系統(tǒng)的導向精度。
文檔編號G01C21/18GK101078627SQ20071011805
公開日2007年11月28日 申請日期2007年6月28日 優(yōu)先權日2007年6月28日
發(fā)明者劉百奇, 房建成, 全偉, 楊照華, 楊勝, 李金濤 申請人:北京航空航天大學