本發(fā)明涉及一種軌道平整度測量方法。

背景技術：

軌道機器人日常巡檢監(jiān)控任務都是在軌道上進行的，因此機器人對于軌道的穩(wěn)定性和平整度存在著較高要求。為了保障巡檢監(jiān)控任務地順利運行，需要對前期軌道安裝工藝進行嚴格把關，軌道安裝過程中會存在三維空間的角度偏差，而常見的人工采取傾角儀測量的方法只能獲取軌道在二維角度上的偏差，并且測量誤差較大，過程繁瑣，當人工爬高去進行測量時存在安全隱患，效率較低。

技術實現(xiàn)要素：

為解決現(xiàn)有技術存在的問題，本發(fā)明提供一種軌道平整度測量方法，智能判斷軌道安裝的平整度，實現(xiàn)軌道安裝檢測過程操作的簡易性、安全性和效率。

本發(fā)明提供的軌道平整度測量方法，包括以下步驟：

(1)將具有加速度傳感器、角速度傳感器、磁傳感器的行走機構放置在軌道上，并移動到軌道相應位置；

(2)獲取加速度傳感器、角速度傳感器的原始數(shù)據(jù)，根據(jù)原始數(shù)據(jù)得出行走機構的俯仰角、偏航角和翻滾角；行走機構的俯仰角、偏航角和翻滾角與軌道的俯仰角、偏航角和翻滾角相同。

(3)判斷步驟(2)中獲得的俯仰角、偏航角、翻滾角是否位于標準閾值區(qū)間內(nèi)，如果位于標準閾值區(qū)間內(nèi)則說明軌道平整，否則，軌道不平整。

優(yōu)選地，所述步驟(2)具體包括；

(2.1)獲取加速度傳感器、角速度傳感器的原始數(shù)據(jù)作為四元素，利用該四元素作為初值初始化四元素q₀、q₁、q₂、q₃，將四元素q₀、q₁、q₂、q₃轉(zhuǎn)化為歐拉角θ、ψ、該組歐拉角作為第一組姿態(tài)角Q₀，θ、ψ、分別表示俯仰角、偏航角、翻滾角；

(2.2)對加速度傳感器的原始數(shù)據(jù)進行中值濾波，得到加速度數(shù)據(jù)A-data，A-data包含分別對應x軸，y軸，z軸的A_x,A_y,A_z三個分量，利用這三個分量進行姿態(tài)解算，得到俯仰角θ，翻滾角以及z軸與重力加速度的夾角γ，其中

(2.3)獲取磁傳感器的原始數(shù)據(jù)并進行中值濾波，得到磁數(shù)據(jù)M-data，

該數(shù)據(jù)包含三個分量M_x,M_y,M_z，然后進行傾斜補償：

根據(jù)傾斜補償結果得到偏航角其中θ、為步驟(2.2)中得到俯仰角、翻滾角。

(2.4)對角速度傳感器的原始數(shù)據(jù)進行均值濾波得到角速度數(shù)據(jù)ω-data，對ω-data積分運算后得到對應的角度Q_k＝(ω_k-ω_b)dt+Q_k-1，其中，Q_k為當前時刻的角度；ω_k為角速度傳感器當前時刻的角速度；ω_b為當前時刻角速度的偏移量；dt為采樣周期；Q_k-1為前一時刻的角度值。

(2.5)將步驟(2.2)、(2.3)、(2.4)獲得的俯仰角θ、翻滾角偏航角ψ、當前時刻的角度Q_k通過卡爾曼濾波算法進行融合得到第二組姿態(tài)角Q₁；

(2.6)將第一組姿態(tài)角Q₀和第二組姿態(tài)角Q₁通過一階互補濾波進行融合，得到融合后數(shù)據(jù)Q_t＝αQ₀+βQ₁，根據(jù)融合后數(shù)據(jù)Q得到最終準確的俯仰角、翻滾角、偏航角，其中，α，β對應Q₀、Q₁置信權重，α+β＝1。α取值范圍為0.3-0.5，β取值范圍：0.7～0.5。

為實現(xiàn)對不平整軌道位置的準確定位，本發(fā)明所述行走機構上還安裝有定位系統(tǒng)，該定位系統(tǒng)用于對行走機構進行定位，確定行走機構在軌道上的位置，當檢測到軌道不平整時，可以利用行走機構的位置確定不平整軌道的位置。當俯仰角、偏航角、翻滾角不在標準閾值區(qū)間內(nèi)時，行走機構停止走動并發(fā)出報警，直至該軌道被修復平整，將行走機構重新放回該處，測得相關數(shù)據(jù)符合要求后，行走機構繼續(xù)檢測下一段軌道。

為了實現(xiàn)前期軌道安裝工藝地嚴格把控，本發(fā)明使用陀螺儀對軌道穩(wěn)定性和平整性進行測量，使用ARM處理器對數(shù)據(jù)進行處理，使用手持數(shù)據(jù)分析液晶屏顯示軌道平整度信息，減少人工測量的誤差。為了實現(xiàn)對軌道三維角度偏差的測量精確反饋，使用AHRS擴展分時算法和卡爾曼濾波算法對測量數(shù)據(jù)進行處理，使用數(shù)據(jù)分析液晶屏的偏差量及偏差曲線作為反饋量，使用異常指示燈判斷軌道平整度，智能精準定位出異常傾斜的軌道區(qū)域。本發(fā)明使用行走機構裝載測量設備，將測量設備拓展成為一個移動平臺，使操作簡易安全，提高軌道安裝效率。本發(fā)明實現(xiàn)智能判斷軌道安裝的平整度，自動進行具體區(qū)域地標定，反饋偏差值，進行精準修正，使得機器人導軌的穩(wěn)定性和平整性得到保證。

附圖說明

圖1為本發(fā)明流程圖；

圖2為手持數(shù)據(jù)分析液晶屏的控制界面；

圖3為手持數(shù)據(jù)分析液晶屏的三維姿態(tài)角界面；

圖4為手持數(shù)據(jù)分析液晶屏的偏差角界面。

具體實施方式

本發(fā)明的行走機構可采用申請?zhí)枮?01610186221.1的發(fā)明專利提供的機器人行走機構，該行走機構采用一個高性能有刷電機為動力，在驅(qū)動輪的下面有定位系統(tǒng)，結合電機編碼器位置環(huán)能精準采集行走的距離，該行走機構具有八個導向輪，在行走機構與軌道接觸的兩端，各有4個導向輪，相互配合，緊貼軌道，不會產(chǎn)生晃動影響陀螺儀數(shù)據(jù)；導向模塊可在水平面上轉(zhuǎn)動一定角度以適應轉(zhuǎn)向調(diào)節(jié)，可實現(xiàn)無死角轉(zhuǎn)向，在轉(zhuǎn)彎處不會產(chǎn)生運動方向受力。行走機構上安裝有定位系統(tǒng)，采取9個磁鋼與霍爾傳感器配合，可實現(xiàn)無接觸性，軌道不需要安裝供電的情況下，準確定位機構行走的距離。行走機構上安裝有陀螺儀、ARM處理器、三軸MEMS模塊測量模塊，也可以直接使用MPU9250傳感器，該傳感器集成了陀螺儀、加速度傳感器和磁傳感器。

本發(fā)明整體方案描述如下：

1、將行走機構放在需要測試平整度的軌道上；

2、將行走機構的上的ARM處理器、三軸MEMS模塊測量模塊上電；

3、在手持數(shù)據(jù)分析液晶屏設置軌道平整度的標準閾值區(qū)間(-0.6°～+0.6°)，該標準閾值區(qū)間是根據(jù)機器在軌道上運行保證水平姿態(tài)而設定，通過無線模塊發(fā)送給軌道上的行走機構；

4、輸入需要測試的軌道的距離，點擊運行，如圖2所示，為手持數(shù)據(jù)分析液晶屏的控制界面；

5、軌道的行走機構會在軌道上移動，當遇到某一段平整度不在標準閾值區(qū)間內(nèi)的區(qū)域時會自動停止行走并發(fā)出紅色閃爍燈；該段軌道平整度數(shù)據(jù)傳輸給手持數(shù)據(jù)分析液晶屏；由于測得翻滾角、俯仰角、偏航角三個數(shù)據(jù)，只要任意一個數(shù)據(jù)不再標準閾值區(qū)間內(nèi)，都會停止行走。

6、手持數(shù)據(jù)分析液晶屏顯示軌道平整度異常信息，在手持數(shù)據(jù)分析液晶屏觀察偏差量及偏差曲線，如圖3所示為手持數(shù)據(jù)分析液晶屏的三維姿態(tài)角界面，顯示的是該段軌道的翻滾角、俯仰角、偏航角數(shù)據(jù)，圖4所示為手持數(shù)據(jù)分析液晶屏的偏差角界面，顯示偏差量及偏差曲線；

7、工人在異常軌道區(qū)域進行人工修正，直至異常閃爍紅色變?yōu)檎?表示數(shù)據(jù)正常)，該段軌道平整度達到要求。

如圖1所示，本發(fā)明獲得準確的俯仰角、翻滾角、偏航角的具體步驟如下：

①第一組姿態(tài)角Q₀

使用InvenSense公司生產(chǎn)的MPU9250，MPU9250自帶DMP(數(shù)字運動處理器)和MPL(嵌入式運動處理庫)對MPU9250的加速度傳感器和角速度傳感器的原始數(shù)據(jù)進行處理，得到四元數(shù)并輸出，用這個四元數(shù)作為初值初始化四元數(shù)q₀、q₁、q₂、q₃，由于歐拉角比較直觀，x_b、y_b、z_b為該模塊載體三軸的坐標，x_n、y_n、z_n:為參考坐標系下的坐標，需要將四元數(shù)轉(zhuǎn)換成歐拉角：

首先，從四元素q₀、q₁、q₂、q₃轉(zhuǎn)換成方向余弦矩陣：

然后再通過方向余弦矩陣轉(zhuǎn)換成歐拉角：

式中T_ij(i,j＝1,2,3)表示方向余弦矩陣的第i行、第j列元素。這樣就可以得到第一組姿態(tài)角Q₀(包括俯仰角、翻滾角、偏航角)。

②第二組姿態(tài)角Q₁

1)加速度數(shù)據(jù)處理

得到加速度計原始數(shù)據(jù)后，進行一次中值濾波，得到處理后的加速度數(shù)據(jù)A-data，A-data包含A_x,A_y,A_z(對應x軸，y軸，z軸)三個分量，然后利用這三個分量進行姿態(tài)解算：

式中，θ,分別對應俯仰角和翻滾角，γ為z軸與重力加速度的夾角，這樣通過加速度計可以得到俯仰角和翻滾角，而偏航角沒有經(jīng)過校準，會出現(xiàn)明顯漂移，需要使用磁傳感器進行校正。

2)磁傳感器數(shù)據(jù)處理

在得到磁傳感器數(shù)據(jù)后，先進行中值濾波，得到處理后的磁數(shù)據(jù)M-data，該數(shù)據(jù)包含三個分量M_x,M_y,M_z，然后進行傾斜補償：

其中θ、為步驟1)中得到俯仰角、翻滾角，偏航角可以通過如下三角函數(shù)關系計算：

3)角速度數(shù)據(jù)處理

得到角速度ω原始數(shù)據(jù)后，需要先進行一次均值濾波，去除噪聲干擾，得到處理后數(shù)據(jù)ω-data，然后通過積分就可以得到對應的角度：

Q_k＝(ω_k-ω_b)dt+Q_k-1 (6)

其中，Q_k為當前時刻的角度；ω_k為陀螺儀當前時刻的角速度；ω_b為當前時刻角速度的偏移量；dt為采樣周期；Q_k-1為前一時刻的角度值。

4)卡爾曼濾波

使用卡爾曼濾波算法對角度姿態(tài)數(shù)據(jù)進行融合，濾波器的輸入量為系統(tǒng)觀測量，輸出量為系統(tǒng)狀態(tài)量的預測，利用系統(tǒng)噪聲和觀測噪聲的統(tǒng)計特性進行最優(yōu)估計。假設系統(tǒng)的狀態(tài)方程和測量方程描述如下：

其中，X_k，y_k分別對應狀態(tài)向量和觀測向量；A為第k-1時刻到第k時刻狀態(tài)轉(zhuǎn)移矩陣；B為輸入控制向量的增益矩陣；U_k為輸入控制向量；H為狀態(tài)量到觀測量的增益矩陣；W_k為系統(tǒng)輸入噪聲；v_k為觀測噪聲；假定系統(tǒng)輸入噪聲和觀測噪聲都服從正態(tài)分布，輸入噪聲的協(xié)方差為Q，觀測噪聲的協(xié)方差為R，則卡爾曼濾波可描述為以下5個公式：

狀態(tài)量預測：

誤差協(xié)方差預測：

P_k|k-1＝AP_k-1A^T+Q (9)

卡爾曼增益更新：

K_k＝P_k|k-1H^T[HP_k|k-1H^T+R]^-1 (10)

狀態(tài)更新：

誤差協(xié)方差更新：

P_k＝(1-K_kH)P_k|k-1 (12)

式中，為對當前狀態(tài)真實值的預測；為基于k-1時刻對k時刻狀態(tài)的預測；A為第k-1時刻到第k時刻狀態(tài)轉(zhuǎn)移矩陣；B為輸入控制向量的增益矩陣；H為狀態(tài)量到觀測量的增益矩陣；y_k為第k時刻的觀測量；K_k為卡爾曼增益；P_k為當前狀態(tài)估計值的誤差協(xié)方差矩陣；P_k|k-1為預測的誤差協(xié)方差矩陣；I為單位矩陣。

陀螺儀角度狀態(tài)用式(5)表示，加速度計和磁傳感器對應角度作為觀測反饋值。為了抑制陀螺儀漂移，需要對偏移量進行預測，所以系統(tǒng)狀態(tài)方程為：

式中T表示采樣周期；Q_k為當前時刻的角度；ω_b為當前時刻角速度的偏移量，ω_k-1、ω_k分別對應k時刻和k-1時刻的角速度。在優(yōu)化過程中，各個初始值設置如下：

通過卡爾曼濾波算法將陀螺儀的角速度數(shù)據(jù)、加速度數(shù)據(jù)和磁傳感器數(shù)據(jù)進行融合，可以得到第二組姿態(tài)角Q₁(包括俯仰角、翻滾角、偏航角)。

③一階互補濾波

為了進一步提高獲得姿態(tài)角的準確性，將第一組三個姿態(tài)角Q₀(俯仰角、翻滾角、偏航角)和第二組三個姿態(tài)角Q₁(俯仰角、翻滾角、偏航角)通過互補濾波進行融合得到融合數(shù)據(jù)Qt(俯仰角、翻滾角、偏航角)互補濾波公式如下：

Q_t＝αQ₀+βQ₁ (14)

α，β對應Q₀、Q₁置信權重，α+β＝1，一般設置α的范圍值：0.3～0.5，設置β的范圍值：0.7～0.5；Qt為融合數(shù)據(jù)，這樣就能得到一個非常準確的姿態(tài)數(shù)據(jù)，通過顯示屏進行顯示。