本發(fā)明涉及一種兩輪平衡車的路徑校正方法，尤其是涉及一種基于里程計和磁傳感器的兩輪平衡車路徑校正裝置及方法。
背景技術(shù)：
：近年來，兩輪平衡車因其運動靈活、智能控制、操作簡單、節(jié)省能源、綠色環(huán)保等特點在現(xiàn)代交通工具中應(yīng)用越來越廣泛，如用于日常代步工具、警務(wù)人員治安巡查、廣告宣傳、輔助拍攝等。然而，除了其載人功能外，兩輪平衡車作為機器人家族中的一員，在其自動行走、姿態(tài)調(diào)節(jié)等基本功能的基礎(chǔ)上搭載某些傳感器后即可在很多場合下替代巡邏機器人完成很多任務(wù)，如電站巡檢、小區(qū)安保、日常巡邏、環(huán)境監(jiān)測、軍事偵察等。現(xiàn)階段，里程計、陀螺儀、磁羅盤等傳感器常用于室外環(huán)境下機器人定位，但因其存在明顯缺陷，一般不單獨使用，通常與其他傳感器共同使用，進行信息融合。巡邏機器人通常采用搭載里程計、視覺傳感器、超聲波傳感器、激光測距儀、磁導航傳感器、GPS、紅外線傳感器等方式實現(xiàn)其路徑規(guī)劃、自主導航、定位、檢測路線等功能。當機器人在避障后或因其他原因偏移巡邏路徑時，如不及時采取校正和調(diào)整措施，在行進過程中誤差不斷積累，不僅無法順利完成巡邏任務(wù)，還會增大機器人走入死路或發(fā)生碰撞的危險程度。因此在機器人巡邏過程中對其實際運行路線與預(yù)定路徑進行比較與評估后及時進行校正和調(diào)整是至關(guān)重要的。通常利用其上搭載的傳感器進行位置信息采集，通過位置估算及障礙物位置或軌道位置信息進行路徑的跟蹤及調(diào)整。目前，大部分輪式巡邏機器人主要是基于視覺傳感器、紅外傳感器或磁導航傳感器設(shè)計開發(fā)的，基于視覺傳感器的機器人識別精度高，但視覺定位算法復(fù)雜，實時性較差，且易受環(huán)境光線影響，運行不穩(wěn)定；基于紅外傳感器價格較低、抗干擾能力強，識別速度快，但識別精度不高，通常需要在全路段布置連續(xù)的路面色帶標志信息來引導機器人行進；磁導航傳感器也需要在全路段鋪設(shè)磁軌或磁條，而在很多實際應(yīng)用場合下，要求在巡邏全路段鋪設(shè)導軌、磁軌、色帶軌等是不現(xiàn)實的?，F(xiàn)有技術(shù)中，針對移動機器人巡邏過程中的路徑校正均采用立體視覺方式，成本較高、實時性差、且易受環(huán)境影響，也尚未有專門針對兩輪平衡車巡邏時使用的路徑校正方法供生產(chǎn)廠家使用。技術(shù)實現(xiàn)要素：本發(fā)明的目的就是為了克服上述現(xiàn)有技術(shù)存在的缺陷而提供一種能在兩輪平衡車避障或偏移路徑后使其返回預(yù)定標準路線的基于里程計和磁傳感器的兩輪平衡車路徑校正裝置及方法。本發(fā)明的目的可以通過以下技術(shù)方案來實現(xiàn)：一種基于里程計和磁傳感器的兩輪平衡車路徑校正裝置，包括相互連接的控制器和兩輪平衡車驅(qū)動機構(gòu)，還包括兩個里程計、兩個磁傳感器和多個校正區(qū)，所述的兩個里程計和兩個磁傳感器分別與控制器連接，兩輪平衡車的左輪和右輪各安裝一個里程計和一個磁傳感器，每個校正區(qū)包括兩個相互不平行也不相交的磁條，所述的磁條位置固定，并與兩輪平衡車的標準路徑交叉，兩輪平衡車從起點開始按記憶路徑行駛，記憶路徑按照標準路徑生成，當兩輪平衡車行駛至校正區(qū)，磁傳感器感應(yīng)到車輪壓過磁條時，控制器讀取里程計的實時數(shù)據(jù)，根據(jù)實時數(shù)據(jù)與該校正區(qū)的標準數(shù)據(jù)計算當前的實際路徑與標準路徑之間的偏差，并根據(jù)偏差進行路徑校正，使兩輪平衡車回到標準路徑，然后繼續(xù)行駛至下一個校正區(qū)或結(jié)束行駛，其中，每個校正區(qū)內(nèi)，有四個標準數(shù)據(jù)和四個實時數(shù)據(jù)，標準數(shù)據(jù)獲取方法包括：兩輪平衡車從起點嚴格沿標準路徑行駛，經(jīng)過校正區(qū)內(nèi)磁條時，由控制器記錄兩個里程計的數(shù)據(jù)作為該校正區(qū)的標準數(shù)據(jù)。所述的裝置還包括設(shè)置在兩輪平衡車路徑上的充電樁，所述的充電樁作為兩輪平衡車行駛的起點。一種使用所述的基于里程計和磁傳感器的兩輪平衡車路徑校正裝置進行路徑校正的方法，包括以下步驟：S1，兩輪平衡車從起點開始啟動，按記憶路徑行駛，兩輪平衡車行駛的實際路徑與標準路徑之間存在偏差；S2，車輪壓過磁條時，該車輪上的磁傳感器向控制器發(fā)送信號，控制器讀取該車輪上里程計的實時數(shù)據(jù)，并返回步驟S2，直到控制器讀取到四個實時數(shù)據(jù)，該校正區(qū)內(nèi)的實時數(shù)據(jù)讀取完畢；S3，控制器根據(jù)四個實時數(shù)據(jù)，計算兩輪平衡車當前的實際路徑與標準路徑之間的偏差，并判斷是否進行路徑校正，若是，則進入步驟S4，否則繼續(xù)按記憶路徑行駛并返回步驟S2，或者結(jié)束行駛；S4，兩輪平衡車暫停行駛，根據(jù)偏差計算校正參數(shù)并根據(jù)校正參數(shù)移動至標準路徑，然后繼續(xù)按記憶路徑行駛并返回步驟S2。所述的步驟S2中，當車輪經(jīng)過磁條時，磁傳感器檢測到連續(xù)變化的磁感應(yīng)強度，當磁感應(yīng)強度最大時，磁傳感器判斷車輪壓過磁條，并向控制器發(fā)送信號。控制器讀取對應(yīng)車輪上的里程計數(shù)據(jù)。所述的步驟S3中，若偏差小于設(shè)定值，則不進行路徑校正，兩輪平衡車繼續(xù)朝標準路徑行駛并返回步驟S2，若當前校正區(qū)為最后一個校正區(qū)，則不進行路徑校正，兩輪平衡車結(jié)束行駛，否則進入步驟S4。所述的步驟S4中，第i個校正區(qū)內(nèi)，當前實際路徑與標準路徑之間的偏差包括角度偏差θi和方位向偏差Δdi，計算式分別為：θi＝βAi-αAiΔdi=(1-ΔLi2sinβAiΔLi1sinαAi)ΔLi1sinαBisin(αBi-αAi)sinαAi]]>其中，βAi為第i個校正區(qū)內(nèi)車輪第一次壓過的磁條與當前實際路徑之間的夾角，αAi為第i個校正區(qū)內(nèi)車輪第一次壓過的磁條與標準路徑之間的夾角，αBi為第i個校正區(qū)內(nèi)車輪第二次壓過的磁條與標準路徑之間的夾角，ΔLi1為第i個校正區(qū)內(nèi)標準路徑與兩個磁條交點之間的距離，ΔLi2為第i個校正區(qū)內(nèi)實際路徑與兩個磁條交點之間的距離；校正參數(shù)Δyi和的計算式分別為：Δyi＝ΔLmisinθi-Δdiφi=π2+sgn(Δyi)θi]]>其中，ΔLmi為第i個校正區(qū)內(nèi)，當前實際路徑與車輪第二次壓過的磁條之間交點到實際路徑上的待校正點之間的距離，sgn(*)為符號函數(shù)；兩輪平衡車根據(jù)校正參數(shù)移動至標準路徑的具體方法為：兩輪平衡車方向逆時針旋轉(zhuǎn)φi角度后，沿方位向所在直線向標準路徑行進|Δyi|，再逆時針旋轉(zhuǎn)角度，回到標準路徑和正確姿態(tài)，所述的方位向與標準路徑方向垂直。所述的第i個校正區(qū)內(nèi)標準路徑與兩個磁條交點之間的距離ΔLi1計算式為：ΔLi1=LLi+LRi2]]>其中，LLi為平衡車左輪在經(jīng)過兩磁條之間這段時間內(nèi)左輪里程計的里程記錄值，LRi為平衡車右輪在經(jīng)過兩磁條之間這段時間內(nèi)右輪里程計的里程記錄值；第i個校正區(qū)內(nèi)實際路徑與兩個磁條交點之間的距離ΔLi2計算原理與ΔLi1相同。與現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點：(1)采用在校正區(qū)內(nèi)設(shè)置磁條的方式，利用磁傳感器和里程計判斷兩輪平衡車位置和姿態(tài)，從而與標準數(shù)據(jù)進行比較得出偏差，進行路徑校正，不需要在全路段鋪設(shè)磁軌或磁條，實時性好、易于安裝、成本低、不易受環(huán)境影響、不需要人機交互。(2)磁傳感器抗干擾能力強，不受外界環(huán)境光照、顏色等影響。(3)以充電樁作為兩輪平衡車行駛的起點，無需另設(shè)起點，節(jié)約成本，易于管理，且符合兩輪平衡車充電需要。(4)每個校正區(qū)內(nèi)，由于兩個磁條的特殊擺放方式，可保證里程計共讀取四個實時數(shù)據(jù)，從而保證計算的可靠性。(5)當磁感應(yīng)強度最大時，磁傳感器判斷車輪壓過磁條，此時讀取里程計數(shù)據(jù)，計算誤差小。(6)路徑校正時，只需經(jīng)過兩次旋轉(zhuǎn)和一次位移，且角度和位移數(shù)值明確，可回到標準路徑和正確姿態(tài)。附圖說明圖1為本實施例兩輪平衡車標準路徑生成及巡邏路徑校正流程圖；圖2為本實施例兩輪平衡車的行駛路徑及校正區(qū)位置示意圖；圖3為本實施例中，兩輪平衡車在經(jīng)過校正區(qū)時計算行駛路徑與磁條間夾角以及里程值的示意圖；圖4為本實施例中，計算當前巡邏路徑(路徑2)與預(yù)定標準路徑(路徑1)的角度偏差和方位向偏差的幾何示意圖；圖5為本實施例中，預(yù)定標準路徑(路徑1)和當前巡邏路徑(路徑2)相對位置處于第二種情況下，計算路徑2與路徑1的角度偏差和方位向偏差的幾何示意圖；圖6(a)、圖6(b)為本實施例中，預(yù)定標準路徑(路徑1)和當前巡邏路徑(路徑2)相對位置處于第三種情況下，計算路徑2與路徑1的角度偏差和方位向偏差的幾何示意圖；圖7(a)、圖7(b)為本實施例中，預(yù)定標準路徑(路徑1)和當前巡邏路徑(路徑2)相對位置處于第四種情況下，計算路徑2與路徑1的角度偏差和方位向偏差的幾何示意圖；圖8為本實施例中，在待校正點換算出校正參數(shù)的幾何示意圖；附圖標記：101為充電樁；10為起始區(qū)；1為第一個校正區(qū)；2為第二個校正區(qū)；i為第i個校正區(qū)；K為第K個校正區(qū)。具體實施方式下面結(jié)合附圖和具體實施例對本發(fā)明進行詳細說明。本實施例以本發(fā)明技術(shù)方案為前提進行實施，給出了詳細的實施方式和具體的操作過程，但本發(fā)明的保護范圍不限于下述的實施例。實施例一種基于里程計和磁傳感器的兩輪平衡車路徑校正裝置，包括相互連接的控制器和兩輪平衡車驅(qū)動機構(gòu)，還包括兩個里程計、兩個磁傳感器、充電樁和多個校正區(qū)，兩個里程計和兩個磁傳感器分別與控制器連接，兩輪平衡車的左輪和右輪各安裝一個里程計和一個磁傳感器，每個校正區(qū)包括兩個相互不平行也不相交的磁條，磁條固定在地面上，并與兩輪平衡車的標準路徑交叉，充電樁設(shè)置在兩輪平衡車路徑上，作為兩輪平衡車行駛的起點。兩輪平衡車從起點開始按記憶路徑行駛，記憶路徑按照標準路徑生成，當兩輪平衡車行駛至校正區(qū)，磁傳感器感應(yīng)到車輪壓過磁條時，控制器讀取里程計的實時數(shù)據(jù)，根據(jù)實時數(shù)據(jù)與該校正區(qū)的標準數(shù)據(jù)計算當前的實際路徑與標準路徑之間的偏差，并根據(jù)偏差進行路徑校正，使兩輪平衡車回到標準路徑，然后繼續(xù)行駛至下一個校正區(qū)或結(jié)束行駛，其中，每個校正區(qū)內(nèi)，有四個標準數(shù)據(jù)和四個實時數(shù)據(jù)，標準數(shù)據(jù)獲取方法包括：兩輪平衡車從起點嚴格沿標準路徑行駛，經(jīng)過校正區(qū)內(nèi)磁條時，由控制器記錄兩個里程計的數(shù)據(jù)作為該校正區(qū)的標準數(shù)據(jù)。一種使用基于里程計和磁傳感器的兩輪平衡車路徑校正裝置進行路徑校正的方法，包括以下步驟：S1，兩輪平衡車從起點開始啟動，按記憶路徑行駛，兩輪平衡車行駛的實際路徑與標準路徑之間存在偏差；S2，車輪壓過磁條時，該車輪上的磁傳感器向控制器發(fā)送信號，控制器讀取該車輪上里程計的實時數(shù)據(jù)，并返回步驟S2，直到控制器讀取到四個實時數(shù)據(jù)，該校正區(qū)內(nèi)的實時數(shù)據(jù)讀取完畢；S3，控制器根據(jù)四個實時數(shù)據(jù)，計算兩輪平衡車當前的實際路徑與標準路徑之間的偏差，并判斷是否進行路徑校正，若是，則進入步驟S4，否則繼續(xù)按記憶路徑行駛并返回步驟S2，或者結(jié)束行駛；S4，兩輪平衡車暫停行駛，根據(jù)偏差計算校正參數(shù)并根據(jù)校正參數(shù)移動至標準路徑后，然后繼續(xù)按記憶路徑行駛并返回步驟S2。如圖1所示，在預(yù)定標準行進路段(圖中由一圈條形框圍成的虛線路徑，帶有箭頭)設(shè)置一個起始區(qū)和K個校正區(qū)域，起始區(qū)內(nèi)安裝一個充電樁1，充電樁1位置作為平衡車預(yù)定標準路徑和巡邏路徑的起始點。每個校正區(qū)域內(nèi)的地面上粘貼兩條能被磁傳感器所感應(yīng)識別的直線磁條，分別記為Ai和Bi(i＝1,2,…,K)，如附圖2所示，磁條Ai的起始點與終止點分別位于路基兩側(cè)，磁條Bi的起始點與終止點也分別位于路基兩側(cè)，兩條磁條不平行也不相交，分別與兩側(cè)路基構(gòu)成不同的任意夾角即可。平衡車的左右兩輪分別搭載一部里程計和一部磁傳感器，當車輪行進至某條磁條時，該輪上的磁傳感器感應(yīng)識別出磁條，并返回給平衡車一個信號，此時該輪上的里程計將記錄下該時刻的里程數(shù)據(jù)。第一步，開機后，兩輪平衡車從起始點開始沿預(yù)定標準路徑行進，經(jīng)過每一個校正區(qū)時，記錄下相關(guān)數(shù)據(jù)，并通過計算得到平衡車行進方向分別與該校正區(qū)內(nèi)兩條磁條間的夾角以及平衡車在通過兩條磁條間行進的里程值，設(shè)置參數(shù)i＝1。具體方法如下。如附圖3所示，假設(shè)平衡車左右兩輪間距為DLR，平衡車行進方向始終垂直于兩輪輪軸方向。當平衡車經(jīng)過第i個校正區(qū)時，假設(shè)右輪先到達磁條Ai，即平衡車行進到位置xA1時，右輪上的磁傳感器感應(yīng)識別到磁條Ai，此時右輪上的里程計記錄當前里程值為LRi1，當平衡車繼續(xù)行進到左輪到達磁條Ai時，即平衡車行進到位置xA2時，左輪上的磁傳感器感應(yīng)識別到磁條Ai，此時左輪和右輪上的里程計分別記錄當前里程值LLi1和LRi2，在此期間右輪行進里程為LAi＝LRi2-LRi1根據(jù)圖中幾何關(guān)系，可以計算得到平衡車行進方向與磁條Ai之間的夾角αAi為αAi=π2+arctanLAiDLR]]>平衡車繼續(xù)行進，同理，當右輪到達磁條Bi時，即平衡車行進到位置xB1時，右輪上的磁傳感器感應(yīng)識別到磁條Bi，右輪上的里程計記錄當前里程值LRi3，當左輪到達磁條Bi時，即平衡車行進到位置xB2時，左輪上的磁傳感器感應(yīng)識別到磁條Bi，此時左輪和右輪上的里程計分別記錄當前里程值LLi2和LRi4，在此期間右輪行進里程為LBi＝LRi4-LRi3計算得到平衡車行進方向與磁條Bi之間的夾角αBi為αBi=π2+arctanLBiDLR]]>而平衡車在經(jīng)過兩磁條之間的行進距離為左右輪行進距離的平均值，即為ΔLi1=LLi+LRi2=LLi2-LLi1+LRi3-LRi12]]>因此，根據(jù)以上方法可以得到平衡車在經(jīng)過每一個校正區(qū)時其行進方向分別與該校正區(qū)內(nèi)兩條磁條間的夾角αAi、αBi以及平衡車在通過兩條磁條間行進的里程值ΔLi1。第二步，平衡車從起始點開始根據(jù)里程計記錄值按記憶路徑行進，到達第i個校正區(qū)時，如附圖4所示，圖中路徑1為預(yù)定標準路徑，根據(jù)第一步中的記錄結(jié)果，路徑1與磁條Ai、磁條Bi之間的夾角分別為αAi和αBi，路徑1在通過兩條磁條間行進的里程值|Mi1Mi2|為ΔLi1。路徑2為當前巡邏路徑，按照第一步中相似的計算方法可以得到平衡車行進方向與該校正區(qū)內(nèi)兩條磁條間的夾角以及平衡車在通過兩條磁條間行進的里程值。記路徑2與磁條Ai、磁條Bi之間的夾角分別為βAi和βBi，路徑2在通過兩條磁條間行進的里程值|Ni1Ni2|為ΔLi2。第三步，計算路徑2與路徑1之間的角度偏差和方位向偏差。具體方法如下。根據(jù)附圖4中幾何關(guān)系，路徑2與路徑1之間的角度偏差θi為θi＝βAi-αAi在△OiMi1Mi2中，∠Mi1OiMi2＝αBi－αAi，由正弦定理|Mi1Mi2|sin(αBi-αAi)=|OiMi1|sin(π-αBi)]]>即ΔLi1sin(αBi-αAi)=|OiMi1|sinαBi]]>hi1=|OiMi1|sin(π-αAi)=ΔLi1sinαBisin(αBi-αAi)sinαAi]]>在△RiNi1Ni2中，∠Ni1RiNi2＝αAi，∠RiNi1Ni2＝π－βAi，由正弦定理|Ni1Ni2|sinαAi=|RiNi2|sin(π-βAi)]]>即ΔLi2sinαAi=|RiNi2|sinβAi]]>又有hi2hi1=|OiRi||OiMi1|=|RiNi2||Mi1Mi2|=ΔLi2sinβAiΔLi1sinαAi]]>聯(lián)立以上式子可得平衡車在行進至磁條Bi時，路徑2與路徑1在方位向上的偏差Δdi為Δdi=hi1-hi2=(1-ΔLi2sinβAiΔLi1sinαAi)hi1=(1-ΔLi2sinβAiΔLi1sinαAi)ΔLi1sinαBisin(αBi-αAi)sinαAi]]>需要說明的是，附圖4中給出的是βAi＞αAi的情況，即θi＞0，規(guī)定角度逆時針方向為正方向，圖中Oy方向為方位向正方向，此時路徑2與路徑1之間方位向偏差Δdi＞0，即路徑2與磁條Bi的交點相比路徑1與磁條Bi的交點在方位軸上坐標值更大。如附圖5所示，如路徑2與路徑1之間關(guān)系使得βAi＜αAi的情況，即θi＜0，此時計算原理與公式與上一種情況相同，只是計算得到的路徑2與路徑1之間方位向偏差Δdi＜0，即路徑2與磁條Bi的交點相比路徑1與磁條Bi的交點在方位軸上坐標值更小。此外，還存在第三種情況：θi＜0，Δdi＞0，如圖6(a)、6(b)所示；以及第四總情況：θi＞0，Δdi＜0，如圖7(a)、7(b)所示。因此，根據(jù)以上方法可以得到平衡車路徑2與路徑1之間的角度偏差θi和方位向偏差Δdi。第四步，根據(jù)上步中得到的偏差值判斷是否需要路徑校正，如不需校正，則繼續(xù)行進；如需要校正，則根據(jù)平衡車當前位置換算出校正參數(shù)，校正位置和姿態(tài)后繼續(xù)行進。具體校正參數(shù)的計算及路徑校正方法如下。如附圖8所示，假設(shè)行進至Ei1時平衡車停止行進準備路徑校正，此時由里程計記下的|Ni2Ei1|為ΔLmi，則有ΔyiΔdi=|TiEi1||TiNi2|=|TiNi2|-ΔLmi|TiNi2|]]>|TiNi2|=Δdisinθi]]>聯(lián)立以上兩式可得Δyi＝Δdi-ΔLmisinθiφi=π2-θi]]>即校正過程為平衡車逆時針方向旋轉(zhuǎn)φi角度后沿方位向行進Δyi后再順時針旋轉(zhuǎn)90°，此時即回到預(yù)定標準路徑1上，且為正確姿態(tài)。假設(shè)平衡車行至Ei2時停止行進準備路徑校正，校正參數(shù)計算如下Δyi＝(|Ni2Ei2|-|Ni2Ti|)sinθi＝ΔLmisinθi-Δdiφi=π2+θi]]>校正過程為平衡車逆時針方向旋轉(zhuǎn)φi角度后沿方位向負方向行進Δyi后再逆時針旋轉(zhuǎn)90°，此時即回到預(yù)定標準路徑1上，且為正確姿態(tài)。依據(jù)上步中規(guī)定的方位向正方向和角度正方向，上述兩種情況可概括如下校正參數(shù)為Δyi=ΔLmisinθi-Δdiφi=π2+sgn(Δyi)θi]]>其中，sgn()表示符號函數(shù)，即括號內(nèi)為正，則函數(shù)值為1；括號內(nèi)為負，則函數(shù)值為-1；括號內(nèi)為0，則函數(shù)值為0。校正過程為平衡車方向旋轉(zhuǎn)φi角度后沿方位向行進Δyi后方向再旋轉(zhuǎn)角度，此時即回到預(yù)定標準路徑1上，且為正確姿態(tài)。針對附圖6與附圖7中的兩種情況上述校正方法仍成立。第五步，令i＝i+1，行進至下一個校正區(qū)時重復(fù)步驟二～步驟四，直至i＝K+1，完成巡邏路徑2中的所有路徑校正，結(jié)束本輪巡邏過程。當前第1頁1 2 3