一種基于激光掃描儀的自主導(dǎo)航方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種自助主導(dǎo)航方法，特別是一種基于激光掃描儀的自主導(dǎo)航方法。
【背景技術(shù)】
[0002] 目前的電力巡檢導(dǎo)航方式有以下幾種，分別存在著相應(yīng)的優(yōu)缺點(diǎn)。
[0003] 電磁導(dǎo)航： 電磁導(dǎo)航系統(tǒng)中的金屬線由于是被埋設(shè)在地下的，具有隱蔽性好、不易被污染的優(yōu)點(diǎn)。 此外，該系統(tǒng)導(dǎo)航對(duì)外界信號(hào)干擾較小，成本較低，并且它的原理簡(jiǎn)單可靠。但是，電磁導(dǎo)航 系統(tǒng)也有很大的缺點(diǎn)存在：當(dāng)它的路徑一旦被確定，更改起來(lái)就會(huì)特別困難。因此該導(dǎo)航方 式不靈活，也不易于維護(hù)。并且在變電站的環(huán)境中，容易受到變電站強(qiáng)磁場(chǎng)的干擾。
[0004] 磁帶導(dǎo)航： 磁帶導(dǎo)航與電磁導(dǎo)航有著比較相近的原理。在磁帶導(dǎo)航中，磁帶是被鋪設(shè)在路面上的。 而在電磁導(dǎo)航中，是把導(dǎo)線預(yù)先埋設(shè)在地下的。相比較而言，磁帶導(dǎo)航的方式有不錯(cuò)的靈活 性，能夠更容易的改變導(dǎo)航路線，同時(shí)鋪設(shè)也會(huì)比較容易。但是，這種導(dǎo)航方式也存在著如 下缺點(diǎn)：因?yàn)閷⒋艓т佋O(shè)在地面上，所以它有被損壞或者被弄臟的風(fēng)險(xiǎn)；另外在草地和碎 石路上，導(dǎo)航磁帶是無(wú)法鋪設(shè)的。
[0005] 激光導(dǎo)航： 激光導(dǎo)航的原理是：在工作場(chǎng)地的四周，固定好最少三塊使用高反光性材料制做而成 的導(dǎo)引標(biāo)志。將激光掃描器安裝在巡檢車上，然后對(duì)脈沖激光發(fā)射器發(fā)射出的并且被四周 的導(dǎo)引標(biāo)志反射回來(lái)的激光進(jìn)行掃描，從而就可以計(jì)算出巡檢車的位置坐標(biāo)。但是激光導(dǎo) 航存在的問(wèn)題是，在變電站的環(huán)境中，需要鋪設(shè)反光標(biāo)識(shí)物，并且反光標(biāo)識(shí)物在室外的環(huán)境 中也容易弄臟、損壞、變形等，致使定位誤差變大或者失效。
[0006] 以上導(dǎo)航方式的缺點(diǎn)一是都需要在變電站進(jìn)行改造施工，可能影響現(xiàn)有變電站的 道路規(guī)劃結(jié)構(gòu)。二是巡檢路徑施工完成后，要變更巡檢路線十分困難。

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

[0007] 在下文中給出關(guān)于本發(fā)明的簡(jiǎn)要概述，以便提供關(guān)于本發(fā)明的某些方面的基本理 解。應(yīng)當(dāng)理解，這個(gè)概述并不是關(guān)于本發(fā)明的窮舉性概述。它并不是意圖確定本發(fā)明的關(guān) 鍵或重要部分，也不是意圖限定本發(fā)明的范圍。其目的僅僅是以簡(jiǎn)化的形式給出某些概念， 以此作為稍后論述的更詳細(xì)描述的前序。
[0008] 本發(fā)明的目的是在于提供一種不用在變電站進(jìn)行施工，對(duì)變電站現(xiàn)有的道路也沒(méi) 有苛刻的條件，碎石路和坡度不大的草地上也可以正常運(yùn)行，還可以臨時(shí)、靈活的基于激光 掃描儀的自主導(dǎo)航方法。
[0009] -種基于激光掃描儀的自主導(dǎo)航方法，包括以下步驟： A、建立特征地圖和巡檢線路：通過(guò)人工的方式牽引巡檢機(jī)器人完成對(duì)巡檢路線的一次 巡視，依次巡視形成以下模塊：位置測(cè)量模塊一位置估計(jì)模塊一幾何特征抽象模塊一全局 特征地圖構(gòu)建模塊； B、巡檢：巡檢機(jī)器人自動(dòng)按照以下步驟進(jìn)行巡檢，直至結(jié)束：位置測(cè)量模塊一幾何特 征抽象模塊一特征匹配模塊一位置估計(jì)模塊一機(jī)器人位置調(diào)整模塊； 步驟A中所述位置測(cè)量模塊的建立：首先根據(jù)里程計(jì)得出巡檢機(jī)器人在T到T+1時(shí)刻 沿行進(jìn)方向運(yùn)動(dòng)的距離；然后根據(jù)陀螺儀測(cè)得行進(jìn)方向和水平X軸的夾角；根據(jù)T時(shí)刻的 全局坐標(biāo)（_，Υγ)計(jì)算得到T+1時(shí)刻的全局坐標(biāo)(編__， 步驟A中的所述位置估計(jì)模塊的建立：運(yùn)用卡爾曼濾波建立巡檢機(jī)器人的運(yùn)動(dòng)模型和 觀測(cè)模型； X (K) =A*X (K-1) +B*U (K) +W (K) (I) Z (K) =H*X (K) +V (K) (2) X(K)為K時(shí)刻的系統(tǒng)狀態(tài)，U(K)為K時(shí)刻對(duì)系統(tǒng)的控制量，A和B為系統(tǒng)參數(shù)；Z(K)為 K時(shí)刻的測(cè)量值，H為測(cè)量系數(shù);W(K)和V(K)分別表示系統(tǒng)和測(cè)量過(guò)程中的噪聲，噪聲滿足 高斯白噪聲模型，設(shè)W (K)和V (K)的協(xié)方差分別為Q和R ; 基于卡爾曼濾波的5個(gè)經(jīng)典核心公式： X(k|k-l)=A*X(k-l |k-l)+B*U(k) (3) 其中X(k|k-1)是利用上一狀態(tài)預(yù)測(cè)的結(jié)果，X(k-l|k-l)為上一時(shí)刻的最優(yōu)估計(jì)值， U (k)為現(xiàn)在狀態(tài)的控制量，在模型中，控制量U (k)為O ; 經(jīng)過(guò)公式（3)之后，系統(tǒng)結(jié)果已經(jīng)更新了，接下來(lái)計(jì)算X (k I k-Ι)的協(xié)方差，用P來(lái)表示 協(xié)方差，則為： P(k|k-1) = A*P(k-l|k-l)*^r + Q (4) 其中P(k|k-1)為X(k|k-1)對(duì)應(yīng)的協(xié)方差，P(k-I|k-1)為X(k-I|k-1)對(duì)應(yīng)的協(xié)方差， 滅『為A的轉(zhuǎn)置矩陣，Q為系統(tǒng)噪聲。
[0010] X(k|k) = x(k|k-l) + _ (k)*(Z(k) - H*x(k|k-1)) (5) 竭（k) = P(k|k-l)*|pV(H*P(k|k-l)*|p + R) (6) P(k|k) = (I - _ (k)*H)*P(k|k-l) (7); 步驟A中所述的幾何特征抽象模塊的建立：對(duì)于激光掃描儀采集的數(shù)據(jù)點(diǎn)，將其抽象 出幾何特征，以便在巡檢過(guò)程中進(jìn)行位置匹配；抽象出的幾何特征為直線段和圓弧，使用的 方法為最小二乘法擬合；對(duì)于擬合出的直線段，取線段的中心點(diǎn)（垮，&)為質(zhì)心，直線段 則標(biāo)識(shí)為（垮，_，L，| :)，L為線段的長(zhǎng)度，I為線段與X軸的夾角；對(duì)于擬合出的圓弧， 我們?nèi)∑鋱A心（釋，_ )為質(zhì)心，圓弧則標(biāo)識(shí)為，^，R)，R為圓弧半徑； 步驟A中所述的全局特征地圖構(gòu)建模塊的建立：對(duì)應(yīng)每一個(gè)擬合出的直線段和圓弧， 加上位置測(cè)量模塊中里程計(jì)給出的信息，確定其全局坐標(biāo)系中的坐標(biāo)（X，y)，其中每個(gè) 特征都有全局坐標(biāo)系中的坐標(biāo)，和其先后的順序，以及每一個(gè)時(shí)刻機(jī)器人的位置信息集 P ((S!:，我），（羯，顆）......（，)); 巡檢步驟B中所述的位置測(cè)量模塊：根據(jù)里程計(jì)得出巡檢機(jī)器人在T到T+1時(shí)刻沿行 進(jìn)方向運(yùn)動(dòng)的距離；根據(jù)陀螺儀測(cè)得行進(jìn)方向和水平X軸的夾角；根據(jù)T時(shí)刻的全局坐標(biāo) (_，需）計(jì)算得到T+1時(shí)刻的全局坐標(biāo)逸_，％·ρ); 巡檢步驟B中的所述幾何特征抽象模塊：對(duì)于激光掃描儀采集的數(shù)據(jù)點(diǎn)，使用最小二 乘法進(jìn)行擬合，得到直線段和圓?。淮藭r(shí)：對(duì)于擬合出的直線段，得到其（L，$ )，其中L為 線段的長(zhǎng)度，_為線段與X軸的夾角；對(duì)于擬合出的圓弧，得到其R，R為圓弧半徑； 巡檢步驟B中的特征匹配模塊：對(duì)于上一模塊中抽象出來(lái)的直線段或者圓弧，參考位 置測(cè)量模塊中里程計(jì)給出的位置信息以及匹配的先后順序，將抽象出來(lái)的幾何特征在全局 特征地圖中進(jìn)行匹配，以確認(rèn)該幾何特征對(duì)應(yīng)的全局地圖中的特征；選取匹配成功的兩個(gè) 特征，獲取其全局位置坐標(biāo)信息，并且計(jì)算質(zhì)心與巡檢機(jī)器人的距離d，得到（?, ^，屯 )和（5?:，，i2); 巡檢步驟B中的位置估計(jì)模塊：根據(jù)上一模塊特征匹配得到的兩個(gè)匹配成功特征的信 息（?, _，電）和（鮮，海，：％)，運(yùn)用平面三角定位的方法，計(jì)算當(dāng)時(shí)時(shí)刻巡檢機(jī)器 人的坐標(biāo)（％，^ )，在取得巡檢機(jī)器人的坐標(biāo)之后，運(yùn)用特征地圖和巡檢線路的建立中相 同的卡爾曼濾波方法計(jì)算得到巡檢機(jī)器人的位置最佳估計(jì)值; 巡檢步驟B中所述機(jī)器人位姿調(diào)整模塊：根據(jù)上一模塊得到的位置最佳估計(jì)值（藏:， i)，在位置信息集P中查找對(duì)應(yīng)的坐標(biāo)（％:，_)，調(diào)整機(jī)器人PID控制部分，實(shí)現(xiàn)機(jī)器人 從（，讀:）一? (?%::.，為::）的移動(dòng)。
[0011] 本發(fā)明與現(xiàn)有技術(shù)相比具有以下優(yōu)點(diǎn)： 減少制作成本，不用在變電站進(jìn)行施工，對(duì)變電站現(xiàn)有的道路也沒(méi)有苛刻的條件，碎 石路和坡度不大的草地上也可以正常運(yùn)行，運(yùn)行范圍寬泛，還可以臨時(shí)、靈活的調(diào)整巡檢路 線，使用更加靈活方便，抗干擾能力強(qiáng)。
【附圖說(shuō)明】
[0012] 圖1為本發(fā)明特征地圖和巡檢線路建立過(guò)程的流程圖； 圖2為本發(fā)明巡檢過(guò)程的流程圖； 圖3為平面三角定位的算法圖。
【具體實(shí)施方式】
[0013] 為了加深對(duì)本發(fā)明的理解，下面將結(jié)合實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳述，該實(shí)施例 僅用于解釋本發(fā)明，并不構(gòu)成對(duì)本發(fā)明保護(hù)范圍的限定。
[0014] 如圖1-3示出了本發(fā)明一種基于激光掃描儀的自主導(dǎo)航方法的兩種實(shí)施方式： 實(shí)施例1 : 一種基于激光掃描儀的自主導(dǎo)航方法，包括以下步驟： A、 建立特征地圖和巡檢線路：通過(guò)人工的方式牽引巡檢機(jī)器人完成對(duì)巡檢路線的一次 巡視，依次巡視形成以下模塊：位置測(cè)量模塊一位置估計(jì)模塊一幾何特征抽象模塊一全局 特征地圖構(gòu)建模塊； B、 巡檢： 巡檢機(jī)器人自動(dòng)按照以下步驟進(jìn)行巡檢，直至結(jié)束：位置測(cè)量模塊一幾何特征抽象模 塊一特征匹配模塊一位置估計(jì)模塊一機(jī)器人位置調(diào)整模塊； 步驟A中所述位置測(cè)量模塊的建立：首先根據(jù)里程計(jì)得出巡檢機(jī)器人在T到Τ+l時(shí)刻 沿行進(jìn)方向運(yùn)動(dòng)的距離；然后根據(jù)陀螺儀測(cè)得行進(jìn)方向和水平X軸的夾角；根據(jù)T時(shí)刻的 全局坐標(biāo)，_)計(jì)算得到Τ+l時(shí)刻的全局坐標(biāo); 步驟A中的所述位置估計(jì)模塊的建立：運(yùn)用卡爾曼濾波建立巡檢機(jī)器人的運(yùn)動(dòng)模型和 觀測(cè)模型； X (K) =A*X (K-1) +B*U (K) +W (K) (I) Z (K) =H*X (K)