本發(fā)明涉及機器人運動技術(shù)領(lǐng)域，具體而言，涉及一種移動機器人的行程校準(zhǔn)方法。

背景技術(shù)：

隨著機器人學(xué)的發(fā)展，室內(nèi)服務(wù)機器人的研究逐步成為熱點。在智能家居室內(nèi)定位感知系統(tǒng)中，機器人的自身定位能力對于路徑規(guī)劃極其重要，是機器人實現(xiàn)自主導(dǎo)航的關(guān)鍵，對于提高機器人的自動化水平具有重要的意義。

目前的移動機器人底盤，多有兩個或三個主動輪組成，里程計定位方法是一種重要的相對定位方法，它屬于航位推算法，是未知環(huán)境中移動機器人導(dǎo)航定位的主要方法，里程計定位方法能簡化確定位姿的基本問題，僅需要單一甚至無需外部傳感器信息，就可以實現(xiàn)對機器人位置和方向的估計，方法簡單。但會產(chǎn)生無界的誤差累積，甚至?xí)?dǎo)致移動機器人導(dǎo)航任務(wù)的失敗，因此里程計的誤差校正是實現(xiàn)機器人準(zhǔn)確位姿估計的前提。

現(xiàn)有的里程計校準(zhǔn)技術(shù)，如通過“雙向正方形路徑”方法校準(zhǔn)來自系統(tǒng)誤差的兩個“不相等的輪直徑”和“輪距的不確定”。如通過新的系統(tǒng)誤差模型，詳細糾正差分移動系數(shù)。

但以上方法都為數(shù)學(xué)方法，在應(yīng)對直線運動時較為準(zhǔn)確，若出現(xiàn)旋轉(zhuǎn)則會出現(xiàn)偏差。

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

有鑒于此，本發(fā)明實施例的目的在于提供一種移動機器人的行程校準(zhǔn)方法，實現(xiàn)了對里程計的誤差校正并提高了系統(tǒng)定位精度。

第一方面，本發(fā)明實施例提供了一種移動機器人的行程校準(zhǔn)方法，其中，包括：

獲取左輪實際路程和右輪實際路程；

根據(jù)所述左輪實際路程和所述右輪實際路程得到第一位移和轉(zhuǎn)角；

獲取左輪實際直徑和右輪實際直徑，通過所述左輪實際直徑和所述右輪實際直徑對里程計進行校準(zhǔn)；

通過慣性測量單元獲取第二位移；

獲取移動量閾值；

將移動距離與所述移動量閾值進行比較；

當(dāng)所述移動距離小于所述移動量閾值時，將所述第一位移和所述第二位移的平均值進行融合；

當(dāng)所述移動距離大于所述移動量閾值時，利用濾波融合。

結(jié)合第一方面，本發(fā)明實施例提供了第一方面的第一種可能的實施方式，其中：所述獲取左輪實際直徑和右輪實際直徑包括：

根據(jù)下式計算所述左輪實際直徑和所述右輪實際直徑：

其中，D'_l和D'_r分別為所述左輪直徑實際數(shù)據(jù)和所述右輪直徑實際數(shù)據(jù)，L為路程輸入值，τ為旋轉(zhuǎn)角度，ψ為所述轉(zhuǎn)角，N'_l和N'_r分別為左、右編碼器的脈沖增量，和分別為所述左輪直徑實際數(shù)據(jù)和所述右輪直徑實際數(shù)據(jù)，n為每圈脈沖量。

結(jié)合第一方面，本發(fā)明實施例提供了第一方面的第二種可能的實施方式，其中：所述獲取左輪實際路程和右輪實際路程包括：

根據(jù)下式計算所述左輪實際路程和所述右輪實際路程：

其中，L’_l和L’_r分別為所述左輪實際路程和所述右輪實際路程，N'_l和N'_r分別為左、右編碼器的脈沖增量，D'_l和D'_r分別為所述左輪實際直徑和所述右輪實際直徑，n為每圈脈沖量。

結(jié)合第一方面，本發(fā)明實施例提供了第一方面的第三種可能的實施方式，其中：所述根據(jù)所述左輪實際路程和所述右輪實際路程得到第一位移和轉(zhuǎn)角包括：

根據(jù)下式計算所述第一位移：

其中，L'為所述第一位移，L’_l和L’_r分別為所述左輪實際路程和所述右輪實際路程，N'_l和N'_r分別為左、右編碼器的脈沖增量，D'_l和D'_r分別為所述左輪實際直徑和所述右輪實際直徑，n為所述每圈脈沖量。

結(jié)合第一方面，本發(fā)明實施例提供了第一方面的第四種可能的實施方式，其中：所述根據(jù)所述左輪實際路程和所述右輪實際路程得到第一位移和轉(zhuǎn)角還包括：

根據(jù)下式計算所述轉(zhuǎn)角：

其中，ψ為所述轉(zhuǎn)角，N'_l和N'_r分別為所述左、右編碼器的脈沖增量，和分別為所述左輪直徑實際數(shù)據(jù)和所述右輪直徑實際數(shù)據(jù)，B_a為輪距，n為所述每圈脈沖量。

結(jié)合第一方面，本發(fā)明實施例提供了第一方面的第五種可能的實施方式，其中：所述通過慣性測量單元獲取第二位移包括：

使IMU(Inertial Measurement Unit，慣性測量單元)模塊的XYZ三軸方向與差分輪運動正方向同向；

通過所述濾波去除系統(tǒng)噪聲；

對曲線運動時的加速度值進行二次積分，獲得第二位移。

結(jié)合第一方面，本發(fā)明實施例提供了第一方面的第六種可能的實施方式，其中：所述獲取移動量閾值包括：

獲取左輪測量直徑、右輪測量直徑和所述輪距；

根據(jù)所述左輪測量直徑和所述右輪測量直徑得到所述移動量閾值。

結(jié)合第一方面的第五種可能的實施方式，本發(fā)明實施例提供了第一方面的第七種可能的實施方式，其中：所述濾波為卡爾曼濾波、粒子濾波或高斯濾波。

結(jié)合第一方面，本發(fā)明實施例提供了第一方面的第八種可能的實施方式，其中：所述方法還包括：

根據(jù)所述第一位移、所述左輪實際路程和所述右輪實際路程獲取偏移距離；

根據(jù)下式計算所述偏移距離：

其中，C為所述偏移距離，L'為所述第一位移，L’_l和L’_r分別為所述左輪實際路程和所述右輪實際路程。

結(jié)合第一方面的第一種至第四種可能的實施方式，本發(fā)明實施例提供了第一方面的第九種可能的實施方式，其中：所述每圈脈沖量和所述左、右編碼器的脈沖增量通過編碼器獲得。

本發(fā)明提供的一種移動機器人的行程校準(zhǔn)方法，包括獲取左輪實際路程和右輪實際路程；根據(jù)左輪實際路程和右輪實際路程得到第一位移和轉(zhuǎn)角；獲取左輪實際直徑和右輪實際直徑，通過左輪實際直徑和右輪實際直徑對里程計進行校準(zhǔn)；通過慣性測量單元獲取第二位移；獲取移動量閾值；將移動距離與移動量閾值進行比較；當(dāng)移動距離小于移動量閾值時，將第一位移和第二位移的平均值進行融合；當(dāng)移動距離大于移動量閾值時，利用濾波融合。本發(fā)明實現(xiàn)了對里程計的誤差校正并提高了系統(tǒng)定位精度。

附圖說明

為了更清楚地說明本發(fā)明實施例的技術(shù)方案，下面將對實施例中所需要使用的附圖作簡單地介紹，應(yīng)當(dāng)理解，以下附圖僅示出了本發(fā)明的某些實施例，因此不應(yīng)被看作是對范圍的限定，對于本領(lǐng)域的普通技術(shù)人員來講，在不付出創(chuàng)造性勞動的前提下，還可以根據(jù)這些附圖獲得其他相關(guān)的附圖。

圖1示出了本發(fā)明實施例所提供的一種移動機器人的行程校準(zhǔn)方法流程圖。

具體實施方式

為使本發(fā)明實施例的目的、技術(shù)方案和優(yōu)點更加清楚，下面將結(jié)合本發(fā)明實施例中附圖，對本發(fā)明實施例中的技術(shù)方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發(fā)明一部分實施例，而不是全部的實施例。通常在此處附圖中描述和示出的本發(fā)明實施例的組件可以以各種不同的配置來布置和設(shè)計。因此，以下對在附圖中提供的本發(fā)明的實施例的詳細描述并非旨在限制要求保護的本發(fā)明的范圍，而是僅僅表示本發(fā)明的選定實施例。基于本發(fā)明的實施例，本領(lǐng)域技術(shù)人員在沒有做出創(chuàng)造性勞動的前提下所獲得的所有其他實施例，都屬于本發(fā)明保護的范圍。

在本發(fā)明的描述中，需要說明的是，術(shù)語“中心”、“上”、“下”、“左”、“右”、“豎直”、“水平”、“內(nèi)”、“外”等指示的方位或位置關(guān)系為基于附圖所示的方位或位置關(guān)系，僅是為了便于描述本發(fā)明和簡化描述，而不是指示或暗示所指的裝置或元件必須具有特定的方位、以特定的方位構(gòu)造和操作，因此不能理解為對本發(fā)明的限制?；诒景l(fā)明的實施例，本領(lǐng)域技術(shù)人員在沒有做出創(chuàng)造性勞動的前提下所獲得的所有其他實施例，都屬于本發(fā)明保護的范圍。

圖1為本發(fā)明實施例提供的一種移動機器人的行程校準(zhǔn)方法流程圖。

參照圖1，移動機器人的行程校準(zhǔn)方法，包括：

步驟S110，獲取左輪實際路程和右輪實際路程；

具體地，根據(jù)編碼器讀取左、右編碼器的脈沖增量分別為N'_l和N'_r，同時讀取編碼器的每圈脈沖量為n，并結(jié)合左輪實際直徑D'_l和右輪實際直徑D'_r進行計算，獲得左輪實際路程L’_l和右輪實際路程L’_r：

步驟S120，根據(jù)左輪實際路程和右輪實際路程得到第一位移和轉(zhuǎn)角；

具體地，根據(jù)差動機器人運動與模型，通過已經(jīng)獲得的獲得左輪實際路程L’_l和右輪實際路程L’_r，計算獲得第一位移L'：

同時，再結(jié)合輪距B_a，可以計算獲得轉(zhuǎn)角ψ：

步驟S130，獲取左輪實際直徑和右輪實際直徑，通過左輪實際直徑和右輪實際直徑對里程計進行校準(zhǔn)；

具體地，影響機器人旋轉(zhuǎn)運動的主要系統(tǒng)誤差來源是“輪距的不確定”和“兩輪直徑的實際平均值與標(biāo)準(zhǔn)平均值不相等”，所以進一步將轉(zhuǎn)差進行考慮，實現(xiàn)對里程計的校準(zhǔn)。針對低運動控制模型精度的移動機器人，假設(shè)尺寸上，兩輪存在尺寸誤差，左輪直徑比右輪直徑大，機器人低速直線運動，路程輸入值為L，機器人運動中實際轉(zhuǎn)過的角度為ψ，旋轉(zhuǎn)角度為τ，結(jié)合在編碼器上讀取的左、右編碼器的脈沖增量分別為N'_l和N'_r，和每圈脈沖量為n，可以計算獲得左輪實際直徑D'_l和右輪實際直徑D'_r：

步驟S140，通過慣性測量單元獲取第二位移；

具體地，使IMU模塊的XYZ三軸方向與差分輪運動正方向同向；

通過濾波去除系統(tǒng)噪聲；

對曲線運動時的加速度值進行二次積分，獲得第二位移。

也就是說，將IMU模塊正向朝上，XYZ三軸方向與差分輪運動正方向通向，IMU陀螺儀可以對角角速度響應(yīng)，一般由于在使用中存在系統(tǒng)噪聲，都必須經(jīng)過高級濾波，如粒子濾波或卡爾曼濾波，推算位移時，需要對曲線運動時的加速度值進行二次積分，獲得第二位移。

這里，IMU模塊內(nèi)置單軸陀螺儀和三軸加速計。IMU模塊也可以與RGB+D、MARG等傳感器融合。

步驟S150，獲取移動量閾值；

具體地，移動量閾值根據(jù)經(jīng)驗，一般設(shè)置為輪子行走一圈的路程，即可通過計算輪子的周長來獲取移動量閾值：

獲取左輪測量直徑、右輪測量直徑和輪距；

根據(jù)左輪測量直徑和右輪測量直徑得到移動量閾值。

步驟S160，將移動距離與移動量閾值進行比較；

具體地，當(dāng)移動機器人不發(fā)生移動時，不進行融合；當(dāng)移動量不為零時，討論移動距離與移動量閾值的大小關(guān)系。

步驟S171，當(dāng)移動距離小于移動量閾值時，將第一位移和第二位移的平均值進行融合；

具體地，根據(jù)實際情況，將第一位移和第二位移的平均值進行融合，可以是幾何平均、算術(shù)平均、平方平均、加權(quán)平均等。

步驟S172，當(dāng)移動距離大于移動量閾值時，利用濾波融合。

具體地，濾波為卡爾曼濾波、粒子濾波或高斯濾波。

根據(jù)本發(fā)明的示例性實施例，該方法還包括：

根據(jù)第一位移、左輪實際路程和右輪實際路程獲取偏移距離；

根據(jù)下式計算偏移距離：

其中，C為偏移距離，L'為第一位移，L’_l和L’_r分別為左輪實際路程和右輪實際路程。

根據(jù)本發(fā)明的示例性實施例，每圈脈沖量和左、右編碼器的脈沖增量通過編碼器獲得。

具體地，使用每圈脈沖量為3500的編碼器，直線運動速度為50mm/s，旋轉(zhuǎn)角速度15°/s，通過獲取左輪測量直徑、右輪測量直徑和輪距，在直線運動，讀取脈沖量和偏移距離，在旋轉(zhuǎn)運行中，讀取脈沖量和偏移距離最后結(jié)合上述所提到的各種算法，進行位置融合。

比如，左輪直徑35.640mm，右輪直徑35.620mm，輪距250.395mm，直線運動5m,旋轉(zhuǎn)650°。測量，實際位置為4.930m,旋轉(zhuǎn)642.5°，進一步融合修正后，運動5.001m，旋轉(zhuǎn)650°。

本發(fā)明實施例中提到的為2輪移動機器人，但并不限于此，3輪、4輪移動機器人都可用。

本發(fā)明提供的一種移動機器人的行程校準(zhǔn)方法，包括獲取左輪實際路程和右輪實際路程；根據(jù)左輪實際路程和右輪實際路程得到第一位移和轉(zhuǎn)角；獲取左輪實際直徑和右輪實際直徑，通過左輪實際直徑和右輪實際直徑對里程計進行校準(zhǔn)；通過慣性測量單元獲取第二位移；獲取移動量閾值；將移動距離與移動量閾值進行比較；當(dāng)移動距離小于移動量閾值時，將第一位移和第二位移的平均值進行融合；當(dāng)移動距離大于移動量閾值時，利用濾波融合。本發(fā)明基于數(shù)學(xué)方法和陀螺儀地磁角度傳感器，實現(xiàn)了對里程計的誤差校正并提高了系統(tǒng)定位精度。

此外，在本發(fā)明各個實施例中的各功能單元可以集成在一個處理模塊中，也可以是各個單元單獨物理存在，也可以兩個或兩個以上單元集成在一個模塊中。上述集成的模塊既可以采用硬件的形式實現(xiàn)，也可以采用軟件功能模塊的形式實現(xiàn)。上述集成的模塊如果以軟件功能模塊的形式實現(xiàn)并作為獨立的產(chǎn)品銷售或使用時，也可以存儲在一個計算機可讀取存儲介質(zhì)中。

上述提到的存儲介質(zhì)可以是只讀存儲器、磁盤或光盤等。

應(yīng)注意到：相似的標(biāo)號和字母在下面的附圖中表示類似項，因此，一旦某一項在一個附圖中被定義，則在隨后的附圖中不需要對其進行進一步定義和解釋。術(shù)語“第一”、“第二”、“第三”等僅用于區(qū)分描述，而不能理解為指示或暗示相對重要性。

最后應(yīng)說明的是：以上所述實施例，僅為本發(fā)明的具體實施方式，用以說明本發(fā)明的技術(shù)方案，而非對其限制，本發(fā)明的保護范圍并不局限于此，盡管參照前述實施例對本發(fā)明進行了詳細的說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解：任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，其依然可以對前述實施例所記載的技術(shù)方案進行修改或可輕易想到變化，或者對其中部分技術(shù)特征進行等同替換；而這些修改、變化或者替換，并不使相應(yīng)技術(shù)方案的本質(zhì)脫離本發(fā)明實施例技術(shù)方案的精神和范圍，都應(yīng)涵蓋在本發(fā)明的保護范圍之內(nèi)。因此，本發(fā)明的保護范圍應(yīng)以所述權(quán)利要求的保護范圍為準(zhǔn)。