[發(fā)明專利]運(yùn)動裝置的軌跡數(shù)據(jù)處理方法及其處理系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201910615659.0
• 申請日: 2019-07-09
• 公開（公告）號: CN110345936B
• 公開（公告）日: 2021-02-09
• 發(fā)明（設(shè)計）人: 蔡龍生;趙明
• 申請（專利權(quán)）人: 上海有個機(jī)器人有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16;G01C21/20
• 代理公司: 上海光華專利事務(wù)所(普通合伙) 31219
• 代理人: 王華英

本發(fā)明提供一種運(yùn)動裝置的軌跡數(shù)據(jù)處理方法及其處理系統(tǒng)，所述獲取方法包括獲取運(yùn)動裝置外部環(huán)境輪廓的相繼兩幀點云，以及所述兩幀點云之間編碼器的相對平移量和慣性測量單元的角度改變量，其中，分別將所述兩幀點云標(biāo)記為參考幀點云和當(dāng)前幀點云；利用所述平移改變量和所述角度改變量構(gòu)建初始變換矩陣；利用所述初始變換矩陣對所述當(dāng)前幀點云進(jìn)行數(shù)據(jù)變換，以獲取過渡幀點云；利用所述參考幀點云和所述過渡幀點云得到對應(yīng)點對集合；利用所述對應(yīng)點對集合得到所述運(yùn)動裝置的軌跡數(shù)據(jù)。利用本發(fā)明，可以有效減小運(yùn)動裝置運(yùn)行軌跡推算時的累積誤差，提高運(yùn)動裝置運(yùn)行軌跡推算的準(zhǔn)確性。

技術(shù)領(lǐng)域

本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域，特別涉及一種運(yùn)動裝置的軌跡數(shù)據(jù)處理方法及其處理系統(tǒng)。

背景技術(shù)

基于激光掃描的自主移動機(jī)器人技術(shù)已經(jīng)有越來越成熟的應(yīng)用。激光掃描所得到的點云是對機(jī)器人外部環(huán)境輪廓的一種描述，該描述實質(zhì)上給出了機(jī)器人離環(huán)境邊緣的距離信息。根據(jù)這些距離信息，機(jī)器可以實現(xiàn)自身位置的確定，也可以實現(xiàn)實時避障的功能。因此充分利用點云的信息是機(jī)器人自主移動的一個基本能力。

點云匹配是機(jī)器人同步定位與建圖(Simultaneously Localization andMapping)的重要組成部分。在相繼的兩幀點云中，其有極其相似的形狀。由于環(huán)境輪廓的剛體性質(zhì)，點云匹配輸出的是兩幀點云之間的旋轉(zhuǎn)量和平移量。若給定其中一幀點云的位置坐標(biāo)，則可以計算出另一幀點云的位置。以此類推，通過機(jī)器人所搭載激光掃描的點云序列，基于點云匹配算法我們可以得到機(jī)器人的運(yùn)動軌跡。

點云匹配實質(zhì)上是一個優(yōu)化問題。一般采用迭代算法求解此類優(yōu)化問題，此時迭代過程非常強(qiáng)烈依賴初始值，如果初始值給得不好，則迭代算法容易陷入局部最優(yōu)解，此局部最優(yōu)解是機(jī)器人軌跡推算累積誤差的來源。因此如果給出一個比較接近全局最優(yōu)解的初始值就很重要。

另一方面，在點云匹配中，誤差函數(shù)是根據(jù)對應(yīng)點對建立起來的，而對應(yīng)點對是根據(jù)最近點規(guī)則計算得來的。此時，最近點的搜索將占用絕大部分的計算時間。如何減小最近點搜索范圍是點云匹配的一個基本問題。

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

鑒于以上所述現(xiàn)有技術(shù)的缺點，本發(fā)明的目的在于提供一種運(yùn)動裝置的軌跡數(shù)據(jù)處理方法及其處理系統(tǒng)，通過合理選擇初始變換矩陣，用于解決現(xiàn)有技術(shù)中運(yùn)動裝置運(yùn)行軌跡推算時累計誤差較大的技術(shù)問題。

為實現(xiàn)上述目的及其他相關(guān)目的，本發(fā)明提供一種運(yùn)動裝置的軌跡數(shù)據(jù)處理方法，所述運(yùn)動裝置的軌跡數(shù)據(jù)處理方法包括：

獲取運(yùn)動裝置外部環(huán)境輪廓的相繼兩幀點云，以及所述兩幀點云之間編碼器的相對平移量和慣性測量單元的角度改變量，其中，分別將所述兩幀點云標(biāo)記為參考幀點云和當(dāng)前幀點云；

利用所述相對平移量和所述角度改變量構(gòu)建初始變換矩陣；

利用所述初始變換矩陣對所述當(dāng)前幀點云進(jìn)行變換，以獲取過渡幀點云；

利用所述參考幀點云和所述過渡幀點云得到對應(yīng)點對集合；

利用所述對應(yīng)點對集合得到所述運(yùn)動裝置的軌跡數(shù)據(jù)。

在一實施例中，所述利用所述對應(yīng)點對集合得到所述運(yùn)動裝置的軌跡數(shù)據(jù)的步驟包括：

建立關(guān)于所述對應(yīng)點對集合中所有對應(yīng)點對誤差度量的目標(biāo)函數(shù)；

優(yōu)化求解所述目標(biāo)函數(shù)，以獲得目標(biāo)變量增量；

根據(jù)所述目標(biāo)變量增量更新所述初始變換矩陣，以獲取所述運(yùn)動裝置的軌跡數(shù)據(jù)。

在一實施例中，所述優(yōu)化求解所述目標(biāo)函數(shù)，以獲得目標(biāo)變量增量的步驟包括：

關(guān)于目標(biāo)變量線性優(yōu)化所述目標(biāo)函數(shù)，以得到關(guān)于所述目標(biāo)變量的方程組；

求解所述目標(biāo)變量的方程組，以獲得目標(biāo)變量增量。