本發(fā)明屬于運(yùn)動(dòng)設(shè)備的運(yùn)動(dòng)技術(shù)領(lǐng)域，特別是涉及運(yùn)動(dòng)設(shè)備的里程計(jì)方法和里程計(jì)裝置。

背景技術(shù)：

移動(dòng)機(jī)器人在室內(nèi)環(huán)境中移動(dòng)時(shí)，里程計(jì)信息是機(jī)器人運(yùn)動(dòng)狀態(tài)的最基本信息。機(jī)器人的里程計(jì)是一種利用致動(dòng)器的移動(dòng)數(shù)據(jù)來(lái)估算機(jī)器人位姿隨時(shí)間改變量的方法。這個(gè)位姿變化可由編碼器和慣性導(dǎo)航傳感器來(lái)得到。傳統(tǒng)的編碼器估計(jì)歷程方法僅僅利用內(nèi)部傳感器信息的累計(jì)得到里程信息，例如一個(gè)機(jī)器人在其輪子或腿關(guān)節(jié)處配備有旋轉(zhuǎn)編碼器等設(shè)備，當(dāng)它向前移動(dòng)一段時(shí)間后，想要知道大致的移動(dòng)距離，借助旋轉(zhuǎn)編碼器，可以測(cè)量出輪子旋轉(zhuǎn)的圈數(shù)，如果知道了輪子的周長(zhǎng)，便可以計(jì)算出機(jī)器人移動(dòng)的距離。

但這類(lèi)里程計(jì)總是會(huì)遇到精度問(wèn)題。由于誤差是由多種因素混合產(chǎn)生，并且誤差隨時(shí)間的累積導(dǎo)致了里程計(jì)的讀數(shù)隨著時(shí)間的增加而變得越來(lái)越不可靠。

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

為了解決現(xiàn)有技術(shù)中里程計(jì)精度低誤差高的缺陷，本發(fā)明提供了一種運(yùn)動(dòng)設(shè)備的里程計(jì)方法以及里程計(jì)裝置。

本發(fā)明是通過(guò)以下技術(shù)方案來(lái)實(shí)現(xiàn)的：獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿、當(dāng)前時(shí)刻的第二激光幀以及局部點(diǎn)云地圖；

根據(jù)當(dāng)前時(shí)刻的位姿，以及第二激光幀與局部點(diǎn)云地圖，計(jì)算得到當(dāng)前時(shí)刻的最優(yōu)位姿。

進(jìn)一步的，以運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿為初值，對(duì)第二激光幀與局部點(diǎn)云地圖進(jìn)行點(diǎn)云匹配得到最優(yōu)位姿，即所述最優(yōu)位姿為根據(jù)以下公式獲得的最優(yōu)化解，所述公式為：

其中：為局部點(diǎn)云地圖構(gòu)成的曲面；pi為第二激光幀內(nèi)的激光點(diǎn)；符號(hào)代表旋轉(zhuǎn)-平移變換；符號(hào)代表點(diǎn)到局部點(diǎn)云地圖構(gòu)成的曲面的歐幾里得投影；p為計(jì)算過(guò)程中的位姿，以當(dāng)前時(shí)刻的位姿為初值。

進(jìn)一步的，所述第二激光幀為剔除動(dòng)態(tài)激光點(diǎn)后的第二激光幀。

進(jìn)一步的，剔除第二激光幀中的動(dòng)態(tài)激光點(diǎn)的步驟包括：根據(jù)當(dāng)前時(shí)刻的位姿獲得第二激光幀內(nèi)激光點(diǎn)在局部概率柵格地圖的位置坐標(biāo)；根據(jù)所述位置坐標(biāo)確定第二激光幀內(nèi)激光點(diǎn)在局部概率柵格地圖上的柵格坐標(biāo)；根據(jù)柵格坐標(biāo)所在柵格的概率值確定該激光點(diǎn)是否為動(dòng)態(tài)激光點(diǎn)，如果是則剔除動(dòng)態(tài)激光點(diǎn)。

進(jìn)一步的，所述當(dāng)前時(shí)刻的位姿為較優(yōu)位姿，具體的，所述較優(yōu)位姿的獲取包括步驟：獲取運(yùn)動(dòng)設(shè)備第一時(shí)刻到當(dāng)前時(shí)刻的初始相對(duì)運(yùn)動(dòng)量；獲取第一時(shí)刻的第一激光幀以及當(dāng)前時(shí)刻的第二激光幀；根據(jù)初始相對(duì)運(yùn)動(dòng)量，以及第一激光幀與第二激光幀，得到幀間相對(duì)運(yùn)動(dòng)量；根據(jù)第一時(shí)刻運(yùn)動(dòng)設(shè)備的位姿以及幀間相對(duì)運(yùn)動(dòng)量計(jì)算獲得當(dāng)前時(shí)刻的較優(yōu)位姿。

進(jìn)一步的，以相對(duì)運(yùn)動(dòng)量為初值，對(duì)第一激光幀與第二激光幀進(jìn)行幀間匹配得到幀間運(yùn)動(dòng)量。

進(jìn)一步地，所述幀間運(yùn)動(dòng)量具體通過(guò)求解以下公式的最優(yōu)化解獲得，公式為：

其中：為第一激光幀構(gòu)成的曲面；pi為第二激光幀的激光點(diǎn)；q為計(jì)算過(guò)程中運(yùn)動(dòng)設(shè)備的相對(duì)運(yùn)動(dòng)量，以相對(duì)運(yùn)動(dòng)量為初值；符號(hào)代表旋轉(zhuǎn)-平移變換；符號(hào)代表點(diǎn)到第一激光幀構(gòu)成的曲面的歐幾里得投影。

進(jìn)一步的，根據(jù)最優(yōu)位姿與第二激光幀更新地圖，包括更新局部點(diǎn)云地圖和/或更新局部概率柵格地圖。

進(jìn)一步的，當(dāng)相對(duì)運(yùn)動(dòng)量的位移達(dá)到設(shè)定閾值時(shí)，重置柵格地圖，所述重置柵格地圖包括重置柵格地圖的概率值，重置地圖原點(diǎn)、更新局部概率柵格地圖。

本發(fā)明還提供一種里程計(jì)裝置，其技術(shù)方案是所述里程計(jì)裝置包括

第一獲取模塊，獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿；

第二獲取模塊，獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的第二激光幀；

第三獲取模塊，獲取局部點(diǎn)云地圖；

第一運(yùn)算模塊，根據(jù)當(dāng)前時(shí)刻的位姿，以及第二激光幀與局部點(diǎn)云地圖，計(jì)算得到當(dāng)前時(shí)刻的最優(yōu)位姿；

進(jìn)一步的，所述裝置還包括剔除模塊，用于剔除第二激光幀內(nèi)的動(dòng)態(tài)激光點(diǎn)；所述第二獲取模塊獲取剔除動(dòng)態(tài)激光點(diǎn)后的第二激光幀。

進(jìn)一步的,所述剔除模塊包括

第一坐標(biāo)單元，根據(jù)當(dāng)前時(shí)刻的位姿獲得第二激光幀內(nèi)激光點(diǎn)在局部概率柵格地圖的位置坐標(biāo)；

第二坐標(biāo)單元，根據(jù)所述位置坐標(biāo)確定第二激光幀內(nèi)激光點(diǎn)在局部概率柵格地圖上的柵格坐標(biāo)；

判斷單元，根據(jù)柵格坐標(biāo)所在柵格的概率值判斷該激光點(diǎn)是否為動(dòng)態(tài)激光點(diǎn)；

剔除單元，判斷單元的判斷結(jié)果剔除動(dòng)態(tài)激光點(diǎn)，即判斷結(jié)果為動(dòng)態(tài)激光點(diǎn)則剔除。

進(jìn)一步的,所述裝置還包括地圖更新模塊，用于根據(jù)最優(yōu)位姿與第二激光幀更新局部點(diǎn)云地圖和/或更新局部概率柵格地圖。

進(jìn)一步的,還包括地圖重置模塊，用于根據(jù)運(yùn)動(dòng)設(shè)備的移動(dòng)距離重置局部概率柵格地圖。

進(jìn)一步的，所述第一獲取模塊具體用于獲取當(dāng)前時(shí)刻的較優(yōu)位姿，包括

第一獲取單元，獲取運(yùn)動(dòng)設(shè)備第一時(shí)刻到當(dāng)前時(shí)刻的初始相對(duì)運(yùn)動(dòng)量；

第二獲取單元，獲取第一時(shí)刻的第一激光幀以及當(dāng)前時(shí)刻的第二激光幀；

第一計(jì)算單元，根據(jù)初始相對(duì)運(yùn)動(dòng)量，以及第一激光幀與第二激光幀，得到幀間相對(duì)運(yùn)動(dòng)量；

第二計(jì)算單元，根據(jù)第一時(shí)刻運(yùn)動(dòng)設(shè)備的位姿以及幀間相對(duì)運(yùn)動(dòng)量計(jì)算獲得當(dāng)前時(shí)刻的較優(yōu)位姿；

本發(fā)明還提供另一種里程計(jì)裝置，包括處理器以及用于存儲(chǔ)處理器可執(zhí)行的指令的存儲(chǔ)器；

所述處理器被配置為：

獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿、當(dāng)前時(shí)刻的第二激光幀以及局部點(diǎn)云地圖，

根據(jù)當(dāng)前時(shí)刻的位姿，以及第二激光幀與局部點(diǎn)云地圖，得到當(dāng)前時(shí)刻的最優(yōu)位姿。本發(fā)明的有益效果如下：

本發(fā)明提供的里程計(jì)方法，通過(guò)碼盤(pán)轉(zhuǎn)數(shù)獲得初始相對(duì)運(yùn)動(dòng)量及位姿，通過(guò)迭代近鄰算法等進(jìn)行激光幀間匹配，以此獲得較準(zhǔn)確的相對(duì)運(yùn)動(dòng)量和位姿。以上一步獲得的位姿為初值，對(duì)激光幀與局部點(diǎn)云地圖進(jìn)行匹配，獲得更準(zhǔn)確的位姿及運(yùn)動(dòng)量，并對(duì)外輸出。本方法無(wú)需先驗(yàn)全局地圖，能有效抑制里程計(jì)的累積誤差，對(duì)動(dòng)態(tài)環(huán)境有較好的適應(yīng)性；大范圍閉環(huán)(200米)誤差在1米之內(nèi)。

本發(fā)明的技術(shù)方案本質(zhì)上或者說(shuō)對(duì)現(xiàn)有技術(shù)做出貢獻(xiàn)的部分可以以軟件產(chǎn)品的形式體現(xiàn)出來(lái)，該計(jì)算機(jī)軟件產(chǎn)品可以存儲(chǔ)在存儲(chǔ)介質(zhì)中，如rom/ram、磁碟、光盤(pán)等，包括若干指令用以使得一臺(tái)計(jì)算機(jī)設(shè)備(可以是個(gè)人計(jì)算機(jī)，服務(wù)器，或者網(wǎng)絡(luò)設(shè)備等)。

本發(fā)明提供的里程計(jì)裝置，解決了現(xiàn)有的現(xiàn)有里程計(jì)不精確以及累計(jì)誤差的不足，可用于眾多通用或?qū)Ｓ玫挠?jì)算系統(tǒng)環(huán)境或配置中。例如：個(gè)人計(jì)算機(jī)、服務(wù)器計(jì)算機(jī)、手持設(shè)備或便攜式設(shè)備、平板型設(shè)備、多處理器系統(tǒng)、基于微處理器的系統(tǒng)、機(jī)頂盒、可編程的消費(fèi)電子設(shè)備、網(wǎng)絡(luò)pc、小型計(jì)算機(jī)、大型計(jì)算機(jī)、包括以上任何系統(tǒng)或設(shè)備的分布式計(jì)算環(huán)境等等。

附圖說(shuō)明

圖1是本發(fā)明里程計(jì)方法實(shí)施例1的流程示意圖；

圖2是本發(fā)明里程計(jì)方法實(shí)施例1獲得較優(yōu)當(dāng)前位姿的流程示意圖；

圖3是本發(fā)明里程計(jì)方法實(shí)施例2的流程示意圖；

圖4是本發(fā)明里程計(jì)裝置的模塊構(gòu)成示意圖。

具體實(shí)施方式

結(jié)合附圖以及具體實(shí)施方式，對(duì)本發(fā)明做進(jìn)一步描述，需要說(shuō)明的是，在不相沖突的前提下，以下描述的各實(shí)施例之間或各技術(shù)特征之間可以任意組合形成新的實(shí)施例：

實(shí)施例一

圖1是本實(shí)施例提供的里程計(jì)方法的流程示意圖。

本方法應(yīng)用的場(chǎng)景中的設(shè)備包括運(yùn)動(dòng)設(shè)備，運(yùn)算設(shè)備，以及設(shè)置在運(yùn)動(dòng)設(shè)備上的激光掃描設(shè)備與物理里程設(shè)備。運(yùn)動(dòng)設(shè)備可以是移動(dòng)機(jī)器人、掃地機(jī)器人、機(jī)器玩具或者需要在室內(nèi)移動(dòng)并定位的智能家居等。運(yùn)算設(shè)備用于對(duì)各種數(shù)據(jù)進(jìn)行計(jì)算，數(shù)據(jù)來(lái)源為運(yùn)動(dòng)設(shè)備、激光掃描設(shè)備、物理里程設(shè)備等。物理里程設(shè)備用于檢測(cè)獲得運(yùn)動(dòng)設(shè)備的運(yùn)動(dòng)數(shù)據(jù)來(lái)估算運(yùn)動(dòng)設(shè)備位姿隨時(shí)間改變量，包括位置改變量、角度改變量等，一般設(shè)置于運(yùn)動(dòng)設(shè)備上，例如碼盤(pán)，其他光電編碼器，或者其他可以測(cè)量在一段時(shí)間內(nèi)運(yùn)動(dòng)設(shè)備的位移量的檢測(cè)元件。

一般對(duì)運(yùn)動(dòng)設(shè)備如移動(dòng)機(jī)器人設(shè)定有一定的活動(dòng)區(qū)域。根據(jù)不同的運(yùn)動(dòng)設(shè)備的可以建立不同的運(yùn)動(dòng)模型，常見(jiàn)的有差分驅(qū)動(dòng)的運(yùn)動(dòng)模型、全向移動(dòng)的運(yùn)動(dòng)模型。在特定的實(shí)施例中具有相鄰的兩個(gè)時(shí)刻，即前一時(shí)刻和當(dāng)前時(shí)刻，一般根據(jù)不同的運(yùn)動(dòng)模型以及已知的運(yùn)動(dòng)設(shè)備前一時(shí)刻的位姿(xt-1，yt-1，θt-1)，可以獲得運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿(xt，yt，θt)，并且可以根據(jù)兩個(gè)時(shí)刻的位姿計(jì)算得到相對(duì)運(yùn)動(dòng)量(δxt，δyt，δθt)。例如對(duì)于雙輪差分的運(yùn)動(dòng)模型，左右兩輪的驅(qū)動(dòng)電機(jī)均安裝有碼盤(pán)作為物理里程設(shè)備，可通過(guò)如下公式獲得到進(jìn)行當(dāng)前時(shí)刻的位姿(xt，yt，θt)

δdt＝(δdl+δdr)/2

δθt＝(δdl-δdr)/ω

θt＝θt-1+δθt

其中，左、右兩輪之間的間距為ω，δdt為根據(jù)碼盤(pán)的信息計(jì)算獲得的運(yùn)動(dòng)設(shè)備的距離變化量，δθt為運(yùn)動(dòng)設(shè)備的角度變化量，δdl、δdr分別為通過(guò)左右碼盤(pán)獲得的左右兩輪的運(yùn)動(dòng)量；

再如全向移動(dòng)的運(yùn)動(dòng)模型的四個(gè)驅(qū)動(dòng)輪的驅(qū)動(dòng)電機(jī)均安裝有碼盤(pán)，可通過(guò)如下公式獲得到進(jìn)行當(dāng)前時(shí)刻的位姿(xt，yt，θt)

xt＝xt-1+δx

yt＝y(tǒng)t-1+δy

θt＝θt-1+δθ

其中，δd1，δd2，δd3，δd4分別為通過(guò)碼盤(pán)獲得的四個(gè)驅(qū)動(dòng)輪上的運(yùn)動(dòng)量，r為輪轂半徑，rr為輥?zhàn)影霃剑昂筝S距為2lx，左右輪距為2ly；

激光掃描設(shè)備的掃描方向?yàn)檫\(yùn)動(dòng)設(shè)備移動(dòng)方向的前方，激光掃描設(shè)備掃描獲得的每一激光幀數(shù)據(jù)通過(guò)現(xiàn)有技術(shù)以激光點(diǎn)云的形式呈現(xiàn)。設(shè)立全局坐標(biāo)系，運(yùn)動(dòng)設(shè)備的位姿、激光掃描設(shè)備的位置以及獲得的激光幀數(shù)據(jù)可以統(tǒng)一到全局坐標(biāo)系中。

局部點(diǎn)云地圖通過(guò)用激光掃描設(shè)備掃描局部環(huán)境形成，存儲(chǔ)于運(yùn)算設(shè)備中。局部概率柵格地圖，則是把局部點(diǎn)云地圖分成一系列柵格，建立局部概率柵格地圖坐標(biāo)系，其中每一柵格給定一個(gè)可能值，表示該柵格被占據(jù)的概率。點(diǎn)云地圖坐標(biāo)系、柵格地圖坐標(biāo)系與全局坐標(biāo)系可以相互轉(zhuǎn)換，以確定運(yùn)動(dòng)設(shè)備在地圖坐標(biāo)系中的位姿。

坐標(biāo)系的設(shè)立、坐標(biāo)變換以及坐標(biāo)系內(nèi)的點(diǎn)映射到其他坐標(biāo)系均可以通過(guò)現(xiàn)有技術(shù)實(shí)現(xiàn)。

如圖1所示的里程計(jì)方法，包括以下步驟：

步驟s110：獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿、當(dāng)前時(shí)刻的第二激光幀以及局部點(diǎn)云地圖。

所述當(dāng)前時(shí)刻為t，所述當(dāng)前時(shí)刻的位姿是通過(guò)物理里程設(shè)備或者現(xiàn)有技術(shù)可獲得當(dāng)前時(shí)刻的初始位姿

同時(shí)在當(dāng)前時(shí)刻t，通過(guò)激光掃描設(shè)備掃描得到的第二激光幀，激光掃描設(shè)備將獲得的激光幀數(shù)據(jù)發(fā)送給運(yùn)算設(shè)備，具體的可以通過(guò)有線或無(wú)線的方式，也可以通過(guò)總控設(shè)備將激光幀數(shù)據(jù)轉(zhuǎn)發(fā)給運(yùn)算設(shè)備。

步驟s120：根據(jù)當(dāng)前時(shí)刻的位姿，以及第二激光幀與局部點(diǎn)云地圖，得到當(dāng)前時(shí)刻的最優(yōu)位姿。

具體地，所述局部點(diǎn)云地圖存儲(chǔ)在運(yùn)算設(shè)備中，運(yùn)算設(shè)備以當(dāng)前時(shí)刻的位姿為初值，對(duì)第二激光幀與局部點(diǎn)云地圖進(jìn)行點(diǎn)云匹配得到最優(yōu)位姿，即根據(jù)以下公式獲得的最優(yōu)化解即為最優(yōu)位姿所述公式為：

其中：為局部點(diǎn)云地圖構(gòu)成的曲面；pi為第二激光幀內(nèi)的激光點(diǎn)；符號(hào)代表旋轉(zhuǎn)-平移變換；符號(hào)代表點(diǎn)到局部點(diǎn)云地圖構(gòu)成的曲面的歐幾里得投影；p為計(jì)算過(guò)程中的位姿，以當(dāng)前時(shí)刻的位姿為初值，其最終的值即為最優(yōu)位姿

所述第二激光幀與局部點(diǎn)云地圖進(jìn)行點(diǎn)云匹配可以通過(guò)現(xiàn)有技術(shù)實(shí)現(xiàn)，例如迭代近鄰算法、暴力匹配等。所述最優(yōu)位姿是由運(yùn)算設(shè)備通過(guò)上述技術(shù)方案計(jì)算獲得的當(dāng)前時(shí)刻的位姿，比步驟s110直接通過(guò)現(xiàn)有技術(shù)或者物理里程設(shè)備獲得的位姿更準(zhǔn)確的。

在本實(shí)施例中，作為進(jìn)一步改進(jìn)，所述當(dāng)前時(shí)刻的位姿還可以是通過(guò)以下技術(shù)方案獲得的較優(yōu)位姿用于代替通過(guò)物理里程設(shè)備或者現(xiàn)有技術(shù)可獲得當(dāng)前時(shí)刻的初始位姿圖2是本發(fā)明里程計(jì)方法實(shí)施例1獲得較優(yōu)當(dāng)前位姿的流程示意圖；具體的，所述較優(yōu)位姿的獲取包括以下步驟：

步驟s111：獲取運(yùn)動(dòng)設(shè)備第一時(shí)刻到當(dāng)前時(shí)刻的初始相對(duì)運(yùn)動(dòng)量。

所述第一時(shí)刻為當(dāng)前時(shí)刻之前的某一時(shí)間點(diǎn)，一般為當(dāng)前時(shí)刻的前一時(shí)刻t-1。所述初始相對(duì)運(yùn)動(dòng)量用于描述運(yùn)動(dòng)設(shè)備從第一時(shí)刻t-1所處的位姿pt-1＝(xt-1，yt-1，θt-1)移動(dòng)到當(dāng)前時(shí)刻t所處的位姿的變化，包括運(yùn)動(dòng)設(shè)備的位置變化量和角度變化量，用表示。

其中，第一時(shí)刻t-1所處的位姿pt-1＝(xt-1，yt-1，θt-1)已知，優(yōu)選地，所述第一時(shí)刻t-1所處的位姿pt-1＝(xt-1，yt-1，θt-1)為上一次里程計(jì)方法獲得的最優(yōu)位姿。在一具體實(shí)施中，可以根據(jù)第一時(shí)刻t-1所處的位姿pt-1＝(xt-1，yt-1，θt-1)與當(dāng)前時(shí)刻t所處的初始位姿計(jì)算轉(zhuǎn)化為初始相對(duì)運(yùn)動(dòng)量計(jì)算方法為現(xiàn)有技術(shù)，且在前面已經(jīng)描述。

通過(guò)物理里程設(shè)備或者其他現(xiàn)有技術(shù)獲得的數(shù)據(jù)發(fā)送給運(yùn)算設(shè)備，通過(guò)有線或無(wú)線的方式，也可以通過(guò)總控設(shè)備轉(zhuǎn)發(fā)給運(yùn)算設(shè)備進(jìn)行計(jì)算。

步驟s112：獲取第一時(shí)刻的第一激光幀以及當(dāng)前時(shí)刻的第二激光幀。

在第一時(shí)刻t-1，激光掃描設(shè)備掃描得到的激光幀為第一激光幀，在當(dāng)前時(shí)刻t，激光掃描設(shè)備掃描得到的激光幀為第二激光幀。

步驟s113：根據(jù)初始相對(duì)運(yùn)動(dòng)量，以及第一激光幀與第二激光幀，得到幀間相對(duì)運(yùn)動(dòng)量。

具體地，以步驟s111中獲得的初始相對(duì)運(yùn)動(dòng)量為初值，對(duì)第一激光幀與第二激光幀進(jìn)行幀間匹配得到幀間相對(duì)運(yùn)動(dòng)量，即根據(jù)以下公式獲得的最優(yōu)化解即為幀間運(yùn)動(dòng)量，所述公式為：

其中：為第一激光幀構(gòu)成的曲面；pi為第二激光幀的激光點(diǎn)；q為計(jì)算過(guò)程中運(yùn)動(dòng)設(shè)備的相對(duì)運(yùn)動(dòng)量，以相對(duì)運(yùn)動(dòng)量為初值，其最終的值即為幀間相對(duì)運(yùn)動(dòng)量；符號(hào)代表旋轉(zhuǎn)-平移變換；符號(hào)代表點(diǎn)到第一激光幀構(gòu)成的曲面的歐幾里得投影。

所述第一激光幀與第二激光幀的幀間匹配可以通過(guò)現(xiàn)有技術(shù)實(shí)現(xiàn)，例如迭代近鄰算法、暴力匹配等。

所述幀間相對(duì)運(yùn)動(dòng)量是由運(yùn)算設(shè)備在初始相對(duì)運(yùn)動(dòng)量的基礎(chǔ)上計(jì)算獲得的從第一時(shí)刻t-1所處的位姿移動(dòng)到當(dāng)前時(shí)刻t所處的位姿的變化量，相對(duì)于步驟s111中獲得的初始相對(duì)運(yùn)動(dòng)量更準(zhǔn)確。

步驟s114：根據(jù)第一時(shí)刻運(yùn)動(dòng)設(shè)備的位姿以及幀間相對(duì)運(yùn)動(dòng)量計(jì)算獲得當(dāng)前時(shí)刻的較優(yōu)位姿。

第一時(shí)刻運(yùn)動(dòng)設(shè)備的位姿已知，根據(jù)第一時(shí)刻t-1運(yùn)動(dòng)設(shè)備的位姿pt-1＝(xt-1，yt-1，θt-1)，以及幀間相對(duì)運(yùn)動(dòng)量可以計(jì)算轉(zhuǎn)化為當(dāng)前時(shí)刻的較優(yōu)位姿所述較優(yōu)位姿相對(duì)于直接通過(guò)物理里程設(shè)備或其他現(xiàn)有技術(shù)獲得的當(dāng)前時(shí)刻的位姿更加準(zhǔn)確。

進(jìn)一步地，本發(fā)明還包括步驟s131：根據(jù)第一時(shí)刻運(yùn)動(dòng)設(shè)備的位姿以及最優(yōu)位姿計(jì)算轉(zhuǎn)化為最優(yōu)相對(duì)運(yùn)動(dòng)量。

第一時(shí)刻運(yùn)動(dòng)設(shè)備的位姿已知，根據(jù)第一時(shí)刻t-1運(yùn)動(dòng)設(shè)備的位姿pt-1＝(xt-1，yt-1，θt-1)，以及最優(yōu)位姿可以計(jì)算獲得最優(yōu)相對(duì)運(yùn)動(dòng)量

本發(fā)明中，步驟s111中，可以根據(jù)第一時(shí)刻t-1的位姿pt-1＝(xt-1，yt-1，θt-1)與當(dāng)前時(shí)刻t的位姿的計(jì)算轉(zhuǎn)化為初始相對(duì)運(yùn)動(dòng)量，計(jì)算公式如下：

xt＝xt-1+cos(θt-1)δxt-sin(θt-1)δyt

yt＝y(tǒng)t-1+sin(θt-1)δxt+cos(θt-1)δyt

θt＝θt-1+δθt

其中，用當(dāng)前時(shí)刻的較優(yōu)位姿代替當(dāng)前時(shí)刻t的初始位姿可實(shí)現(xiàn)步驟s114中較優(yōu)位姿與幀間相對(duì)運(yùn)動(dòng)量的計(jì)算轉(zhuǎn)化；用最優(yōu)位姿代替當(dāng)前時(shí)刻t的位姿可實(shí)現(xiàn)步驟s131中最優(yōu)位姿與最優(yōu)相對(duì)運(yùn)動(dòng)量計(jì)算轉(zhuǎn)化。同時(shí)，通過(guò)本發(fā)明還可以上述公式根據(jù)運(yùn)動(dòng)設(shè)備第一時(shí)刻的位姿以及最優(yōu)位姿計(jì)算獲得最優(yōu)相對(duì)運(yùn)動(dòng)量。

本發(fā)明提供的里程計(jì)方法，通過(guò)碼盤(pán)轉(zhuǎn)數(shù)獲得初始相對(duì)運(yùn)動(dòng)量及位姿，通過(guò)迭代近鄰算法等進(jìn)行激光幀間匹配，以此獲得較準(zhǔn)確的相對(duì)運(yùn)動(dòng)量和位姿。以上一步獲得的位姿為初值，對(duì)激光幀與局部點(diǎn)云地圖進(jìn)行匹配，獲得更準(zhǔn)確的位姿及運(yùn)動(dòng)量，并對(duì)外輸出。本方法無(wú)需先驗(yàn)全局地圖，能有效抑制里程計(jì)的累積誤差，對(duì)動(dòng)態(tài)環(huán)境有較好的適應(yīng)性；大范圍閉環(huán)(200米)誤差在1米之內(nèi)。

實(shí)施例二

圖3是本發(fā)明里程計(jì)方法實(shí)施例2的流程示意圖。

本實(shí)施例的里程計(jì)方法包括

步驟s210：獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿、當(dāng)前時(shí)刻的剔除激光動(dòng)態(tài)點(diǎn)后的第二激光幀以及局部點(diǎn)云地圖。

引入局部概率柵格地圖，將概率柵格地圖的概率值作為刪除動(dòng)態(tài)點(diǎn)的依據(jù)。將第二激光幀映射到局部概率柵格地圖，若激光點(diǎn)所在柵格的概率值小于靜態(tài)柵格的最低概率值則為動(dòng)態(tài)柵格，即該柵格內(nèi)的激光點(diǎn)均為動(dòng)態(tài)激光點(diǎn)，具體地，包括以下步驟：

步驟s211：根據(jù)當(dāng)前時(shí)刻的位姿獲得第二激光幀內(nèi)激光點(diǎn)在局部概率柵格地圖的位置坐標(biāo)；

步驟s212：根據(jù)所述位置坐標(biāo)確定第二激光幀內(nèi)激光點(diǎn)在局部概率柵格地圖上的柵格坐標(biāo)；

步驟s213：根據(jù)柵格坐標(biāo)所在柵格的概率值確定該激光點(diǎn)是否為動(dòng)態(tài)激光點(diǎn)，如果是則剔除動(dòng)態(tài)激光點(diǎn)。

原理如下：

第二激光幀內(nèi)激光點(diǎn)為激光點(diǎn)i，通過(guò)旋轉(zhuǎn)-平移變換將第二激光幀映射到局部概率柵格地圖得到激光點(diǎn)i的位置坐標(biāo)li，即

局部概率柵格地圖的原點(diǎn)坐標(biāo)o，柵格大小reso，激光點(diǎn)i所在柵格的柵格坐標(biāo)(m，n)，則

則當(dāng)0≤bel(m，n)≤belthres時(shí)，判定該柵格內(nèi)激光點(diǎn)i為動(dòng)態(tài)激光點(diǎn)。其中，bel(m，n)為柵格的概率值，belthres為靜態(tài)柵格的最低概率值，bel(m，n)、belthres的值、動(dòng)態(tài)激光點(diǎn)的剔除均通過(guò)現(xiàn)有技術(shù)實(shí)現(xiàn)，本發(fā)明不再贅述。

步驟s220：根據(jù)當(dāng)前時(shí)刻的位姿，以及剔除動(dòng)態(tài)激光點(diǎn)后的第二激光幀與局部點(diǎn)云地圖，得到當(dāng)前時(shí)刻的最優(yōu)位姿。

具體地，以當(dāng)前時(shí)刻的位姿為初值，對(duì)剔除動(dòng)態(tài)點(diǎn)后的第二激光幀與局部點(diǎn)云地圖進(jìn)行點(diǎn)云匹配得到最優(yōu)位姿，即根據(jù)以下公式獲得的最優(yōu)化解即為最優(yōu)位姿所述公式為：

其中：為局部點(diǎn)云地圖構(gòu)成的曲面；為剔除動(dòng)態(tài)激光點(diǎn)后的第二激光幀內(nèi)的激光點(diǎn)；p為計(jì)算過(guò)程中的位姿，以當(dāng)前時(shí)刻的位姿為初值，其最終的值即為最優(yōu)位姿；符號(hào)代表旋轉(zhuǎn)-平移變換；符號(hào)代表點(diǎn)到局部點(diǎn)云地圖構(gòu)成的曲面的歐幾里得投影。

本實(shí)施例所述的里程計(jì)方法與實(shí)施例一中不同的是步驟s210與步驟s220中的第二激光幀為剔除動(dòng)態(tài)激光點(diǎn)后的激光幀，步驟s210、步驟s220與步驟s211中的當(dāng)前時(shí)刻的位姿也可以用通過(guò)步驟s111-步驟s114的方法獲得的當(dāng)前時(shí)刻的較優(yōu)位姿。

在另一實(shí)施例中，本發(fā)明的里程計(jì)方法還包括步驟s240：根據(jù)步驟s220獲得的最優(yōu)位姿與第二激光幀更新地圖，包括更新局部概率柵格地圖和更新局部點(diǎn)云地圖。此步驟獲得的局部概率柵格地圖和局部點(diǎn)云地圖應(yīng)用于下一次里程計(jì)方法實(shí)現(xiàn)的過(guò)程，以獲得更精確的里程數(shù)據(jù)，具體包括以下子步驟：

步驟s241：將第二激光幀內(nèi)的激光點(diǎn)映射到全局坐標(biāo)系中得到激光點(diǎn)在全局坐標(biāo)系的位置。其中，激光點(diǎn)i可以為剔除動(dòng)態(tài)激光點(diǎn)后的第二激光幀內(nèi)的激光點(diǎn)，激光點(diǎn)i在全局坐標(biāo)系的位置通過(guò)旋轉(zhuǎn)-平移變換將第二激光幀映射到全局坐標(biāo)系中，即

步驟s242：將所述位置插入到局部點(diǎn)云地圖，完成局部點(diǎn)云地圖的更新；此步驟可以通過(guò)現(xiàn)有技術(shù)實(shí)現(xiàn)，這里不再贅述；

步驟s243：以光線追蹤方式，更新局部概率柵格地圖；包括更新位置點(diǎn)附近的柵格的概率值以及激光掃描設(shè)備和位置點(diǎn)連線上的概率值。具體地，一般以方差較小的高斯分布，更新位置點(diǎn)附近柵格的概率值；將激光掃描設(shè)備和位置點(diǎn)連線上的柵格概率置零，零表示該柵格被占據(jù)的概率為0。

在另一實(shí)施例中，里程計(jì)不斷累積里程，作為本發(fā)明的又一種改進(jìn)，還包括步驟s250根據(jù)運(yùn)動(dòng)設(shè)備的移動(dòng)距離重置概率柵格地圖。具體的，包括子步驟：

步驟s251：計(jì)算運(yùn)動(dòng)設(shè)備的移動(dòng)距離，并判斷是否到達(dá)設(shè)定距離；若到達(dá)設(shè)定距離則進(jìn)行步驟s252、步驟s253、步驟s254進(jìn)行重置地圖，所述重置柵格地圖包括重置柵格地圖的概率值，重置地圖原點(diǎn)、更新局部概率柵格地圖。所述運(yùn)動(dòng)設(shè)備的移動(dòng)距離通過(guò)本發(fā)明的里程計(jì)方法實(shí)現(xiàn)的里程計(jì)獲得。

步驟s252：將所有柵格概率值均置為表示未知的概率值，一般-1表示該柵格的概率值未知，即該柵格不知道是否被占據(jù)；

步驟s253：根據(jù)最優(yōu)位姿在地圖中的位置確定地圖原點(diǎn)；具體的，以最優(yōu)位姿在地圖中的位置或者平移一定距離的位置為地圖原點(diǎn)；

步驟s254：用當(dāng)前時(shí)刻最近的n幀激光幀以光線追蹤方式更新局部概率柵格地圖。更新方法與步驟s243一致，以此在下一次里程計(jì)方法中可以獲得更準(zhǔn)確的運(yùn)動(dòng)設(shè)備的位姿。

實(shí)施例三

圖4是本發(fā)明里程計(jì)裝置的模塊構(gòu)成示意圖。

一種里程計(jì)裝置，包括

第一獲取模塊，獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿；

第二獲取模塊，獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的第二激光幀；

第三獲取模塊，獲取局部點(diǎn)云地圖；

第一運(yùn)算模塊，根據(jù)當(dāng)前時(shí)刻的位姿，以及第二激光幀與局部點(diǎn)云地圖，計(jì)算得到當(dāng)前時(shí)刻的最優(yōu)位姿。

在本實(shí)施例中，作為進(jìn)一步改進(jìn)，所述第一獲取模塊具體用于獲取當(dāng)前時(shí)刻的較優(yōu)位姿位姿，具體地，所述第一獲取模塊包括

第一獲取單元，獲取運(yùn)動(dòng)設(shè)備第一時(shí)刻到當(dāng)前時(shí)刻的初始相對(duì)運(yùn)動(dòng)量。

第二獲取單元，獲取第一時(shí)刻的第一激光幀以及當(dāng)前時(shí)刻的第二激光幀。

第一計(jì)算單元，根據(jù)初始相對(duì)運(yùn)動(dòng)量，以及第一激光幀與第二激光幀，得到幀間相對(duì)運(yùn)動(dòng)量。

第二計(jì)算單元，根據(jù)第一時(shí)刻運(yùn)動(dòng)設(shè)備的位姿以及幀間相對(duì)運(yùn)動(dòng)量計(jì)算獲得當(dāng)前時(shí)刻的較優(yōu)位姿。

進(jìn)一步地，本發(fā)明的里程計(jì)裝置還包括剔除模塊，用于剔除第二激光幀內(nèi)的動(dòng)態(tài)激光點(diǎn)，所述第二獲取模塊獲取剔除動(dòng)態(tài)激光點(diǎn)后的第二激光幀。具體的，所述剔除模塊包括：

第一坐標(biāo)單元，根據(jù)當(dāng)前時(shí)刻的位姿獲得第二激光幀內(nèi)激光點(diǎn)在局部概率柵格地圖的位置坐標(biāo)；

第二坐標(biāo)單元，根據(jù)所述位置坐標(biāo)確定第二激光幀內(nèi)激光點(diǎn)在局部概率柵格地圖上的柵格坐標(biāo)；

判斷單元，根據(jù)柵格坐標(biāo)所在柵格的概率值判斷該激光點(diǎn)是否為動(dòng)態(tài)激光點(diǎn)，

剔除單元，判斷單元的判斷結(jié)果剔除動(dòng)態(tài)激光點(diǎn)，即判斷結(jié)果為動(dòng)態(tài)激光點(diǎn)則剔除。

進(jìn)一步地，本發(fā)明的里程計(jì)裝置還包括

用于根據(jù)最優(yōu)位姿與第二激光幀更新局部點(diǎn)云地圖以及局部概率柵格地圖。具體地，所述地圖更新模塊包括：

映射單元，將第二激光幀內(nèi)的激光點(diǎn)映射到全局坐標(biāo)系中，得到激光點(diǎn)在全局坐標(biāo)系的位置。通過(guò)旋轉(zhuǎn)-平移變換，即通過(guò)公式將第二激光幀內(nèi)的激光點(diǎn)映射到全局坐標(biāo)系中。

第一更新單元，將位置插入到局部點(diǎn)云地圖，以此完成局部點(diǎn)云地圖的更新。

第二更新單元，以光線追蹤方式，更新局部概率柵格地圖。具體地，一般以方差較小的高斯分布，更新位置點(diǎn)附近柵格的概率值；將激光掃描設(shè)備和位置點(diǎn)連線上的柵格概率置零，零表示該柵格點(diǎn)未被占據(jù)的概率值。

進(jìn)一步的，本發(fā)明的里程計(jì)裝置還包括地圖重置模塊，用于根據(jù)運(yùn)動(dòng)設(shè)備的移動(dòng)距離重置局部概率柵格地圖。所述地圖重置模塊包括

距離計(jì)算單元，計(jì)算運(yùn)動(dòng)設(shè)備的移動(dòng)距離；所述計(jì)算方法為現(xiàn)有技術(shù)，或者通過(guò)本方法的里程計(jì)根據(jù)最優(yōu)位姿計(jì)算獲得。

第二判斷單元，判斷所述移動(dòng)距離是否到達(dá)設(shè)定距離；

若到達(dá)設(shè)定距離，所述地圖重置模塊通過(guò)概率值重置單元、原點(diǎn)重置單元、地圖重置單元重置局部概率柵格地圖。即概率值重置單元，根據(jù)判斷結(jié)果將所有柵格概率值均置為表示未知的概率值；原點(diǎn)重置單元，根據(jù)判斷結(jié)果將最優(yōu)位姿在地圖中的位置重置為地圖原點(diǎn)；地圖重置單元，根據(jù)判斷結(jié)果，以光線追蹤方式，利用當(dāng)前時(shí)刻最近的n幀激光幀更新局部概率柵格地圖。

本實(shí)施例中的裝置與前述實(shí)施例中的方法是基于同一發(fā)明構(gòu)思下的兩個(gè)方面，在前面已經(jīng)對(duì)方法實(shí)施過(guò)程作了詳細(xì)的描述，所以本領(lǐng)域技術(shù)人員可根據(jù)前述描述清楚地了解本實(shí)施中的系統(tǒng)的結(jié)構(gòu)及實(shí)施過(guò)程，為了說(shuō)明書(shū)的簡(jiǎn)潔，在此就不再贅述。

為了描述的方便，描述以上裝置時(shí)以功能分為各種模塊分別描述。當(dāng)然，在實(shí)施本發(fā)明時(shí)可以把各模塊的功能在同一個(gè)或多個(gè)軟件和/或硬件中實(shí)現(xiàn)。

通過(guò)以上的實(shí)施方式的描述可知，本領(lǐng)域的技術(shù)人員可以清楚地了解到本發(fā)明可借助軟件加必需的通用硬件平臺(tái)的方式來(lái)實(shí)現(xiàn)?；谶@樣的理解，本發(fā)明的技術(shù)方案本質(zhì)上或者說(shuō)對(duì)現(xiàn)有技術(shù)做出貢獻(xiàn)的部分可以以軟件產(chǎn)品的形式體現(xiàn)出來(lái)，該計(jì)算機(jī)軟件產(chǎn)品可以存儲(chǔ)在存儲(chǔ)介質(zhì)中，如rom/ram、磁碟、光盤(pán)等，包括若干指令用以使得一臺(tái)計(jì)算機(jī)設(shè)備(可以是個(gè)人計(jì)算機(jī)，服務(wù)器，或者網(wǎng)絡(luò)設(shè)備等)執(zhí)行本發(fā)明各個(gè)實(shí)施例或者實(shí)施例的某些部分所述的方法。

描述的裝置實(shí)施例僅僅是示意性的，其中所述作為分離部件說(shuō)明的模塊或單元可以是或者也可以不是物理上分開(kāi)的，作為模塊或單元示意的部件可以是或者也可以不是物理模塊，既可以位于一個(gè)地方，或者也可以分布到多個(gè)網(wǎng)絡(luò)模塊上?？梢愿鶕?jù)實(shí)際的需要選擇其中的部分或者全部單元來(lái)實(shí)現(xiàn)本實(shí)施例方案的目的。本領(lǐng)域普通技術(shù)人員在不付出創(chuàng)造性勞動(dòng)的情況下，即可以理解并實(shí)施。

本發(fā)明可用于眾多通用或?qū)Ｓ玫挠?jì)算系統(tǒng)環(huán)境或配置中。例如：個(gè)人計(jì)算機(jī)、服務(wù)器計(jì)算機(jī)、手持設(shè)備或便攜式設(shè)備、平板型設(shè)備、多處理器系統(tǒng)、基于微處理器的系統(tǒng)、機(jī)頂盒、可編程的消費(fèi)電子設(shè)備、網(wǎng)絡(luò)pc、小型計(jì)算機(jī)、大型計(jì)算機(jī)、包括以上任何系統(tǒng)或設(shè)備的分布式計(jì)算環(huán)境等等，如實(shí)施例四。

實(shí)施例四

如圖所示的里程計(jì)裝置，包括：處理器以及用于存儲(chǔ)處理器可執(zhí)行的指令的存儲(chǔ)器；

所述處理器被配置為：

獲取運(yùn)動(dòng)設(shè)備當(dāng)前時(shí)刻的位姿、當(dāng)前時(shí)刻的第二激光幀以及局部點(diǎn)云地圖，

根據(jù)當(dāng)前時(shí)刻的位姿，以及第二激光幀與局部點(diǎn)云地圖，得到當(dāng)前時(shí)刻的最優(yōu)位姿。

本發(fā)明實(shí)施例提供的里程計(jì)，解決了現(xiàn)有的現(xiàn)有里程計(jì)不精確以及累計(jì)誤差的不足。對(duì)于本領(lǐng)域的技術(shù)人員來(lái)說(shuō)，可根據(jù)以上描述的技術(shù)方案以及構(gòu)思，做出其它各種相應(yīng)的改變以及變形，而所有的這些改變以及變形都應(yīng)該屬于本發(fā)明權(quán)利要求的保護(hù)范圍之內(nèi)。