[發(fā)明專(zhuān)利]基于八叉樹(shù)搜索反光柱的機(jī)器人全局定位方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202210496440.5
• 申請(qǐng)日: 2022-05-09
• 公開(kāi)（公告）號(hào): CN114593738B
• 公開(kāi)（公告）日: 2022-07-26
• 發(fā)明（設(shè)計(jì)）人: 周軍;楊旭浩;李文廣;李留昭;付周;宋凱
• 申請(qǐng)（專(zhuān)利權(quán)）人: 山東大學(xué)
• 主分類(lèi)號(hào): G01C21/20
• 分類(lèi)號(hào): G01C21/20;G01C21/00;G01S17/06;G06T7/70;G06K9/62;G06V10/74
• 代理公司: 濟(jì)南圣達(dá)知識(shí)產(chǎn)權(quán)代理有限公司 37221
• 代理人: 祖之強(qiáng)

1.一種基于八叉樹(shù)搜索反光柱的機(jī)器人全局定位方法，其特征在于：

包括以下過(guò)程：

對(duì)反光柱全局地圖中的反光柱進(jìn)行一次遍歷，提取滿(mǎn)足需求的反光柱三角形，并存儲(chǔ)到八叉樹(shù)數(shù)據(jù)結(jié)構(gòu)中；

遍歷當(dāng)前觀測(cè)到的反光柱局部地圖形成的反光柱三角形，利用八叉樹(shù)快速搜索全局地圖中與之相同的反光柱三角形；

根據(jù)八叉樹(shù)搜索的結(jié)果，利用三邊定位算法計(jì)算當(dāng)前機(jī)器人位姿，并根據(jù)當(dāng)前機(jī)器人位姿校驗(yàn)當(dāng)前觀測(cè)到的其余反光柱是否與全局地圖匹配，如是，則利用所有匹配成功的反光柱更新機(jī)器人位姿，否則，判定機(jī)器人位姿錯(cuò)誤；

滿(mǎn)足需求的反光柱三角形，包括：

對(duì)任一反光柱三角形中，任意一條邊的長(zhǎng)度都不大于激光雷達(dá)觀測(cè)反光柱的最大距離，任意兩條邊之間的長(zhǎng)度差大于或等于預(yù)設(shè)值；

存儲(chǔ)到八叉樹(shù)數(shù)據(jù)結(jié)構(gòu)中，包括：

激光雷達(dá)觀測(cè)反光柱的最大距離為邊長(zhǎng)建立一個(gè)正方體作為八叉樹(shù)的根節(jié)點(diǎn)；

將上述正方體以固定的規(guī)則分割成8個(gè)子正方體并標(biāo)號(hào)；

在插入反光柱三角形時(shí)，分別以反光柱三角形的三條邊的邊長(zhǎng)對(duì)應(yīng)X、Y和Z軸的坐標(biāo)，計(jì)算出反光柱三角形應(yīng)該位于哪個(gè)子正方體中，若該子正方體中已經(jīng)存在其他的反光柱三角形，則繼續(xù)將該子正方體分割并標(biāo)號(hào)，以此類(lèi)推，直至子正方體中不再含有其他反光柱三角形，插入完畢。

2.如權(quán)利要求1所述的基于八叉樹(shù)搜索反光柱的機(jī)器人全局定位方法，其特征在于：

遍歷當(dāng)前觀測(cè)到的反光柱局部地圖形成的反光柱三角形，利用八叉樹(shù)快速搜索全局地圖中與之相同的反光柱三角形，包括：

從當(dāng)前激光幀中提取反光柱，形成觀測(cè)的局部地圖，只有觀測(cè)到的反光柱數(shù)量大于3時(shí)才能進(jìn)行準(zhǔn)確的全局定位；

按照順序從局部地圖中獲取一個(gè)反光柱三角形，計(jì)算反光柱三角形三條邊的邊長(zhǎng)，確定其在八叉樹(shù)中的位置點(diǎn)，以位置點(diǎn)為中心，確定一個(gè)正方體作為搜索范圍。

3.如權(quán)利要求2所述的基于八叉樹(shù)搜索反光柱的機(jī)器人全局定位方法，其特征在于：

假設(shè)反光柱三角形位置點(diǎn)的坐標(biāo)為(S_lm，S_mn，S_nl)，則正方體的搜索范圍的最大坐標(biāo)為：(S_lm+match_error，S_mn+match_error，S_nl+match_error)，搜索范圍的最小坐標(biāo)為：(S_lm-match_error，S_mn-match_error，S_nl-match_error)，match_error為搜索反光柱三角形時(shí)的允許誤差。

4.如權(quán)利要求2所述的基于八叉樹(shù)搜索反光柱的機(jī)器人全局定位方法，其特征在于：

確定完搜索范圍之后，從八叉樹(shù)的根節(jié)點(diǎn)開(kāi)始搜索，首先遍歷根節(jié)點(diǎn)的8個(gè)子節(jié)點(diǎn)，使用Cmax和Cmin分別代表當(dāng)前子節(jié)點(diǎn)的最大和最小坐標(biāo)，判斷子節(jié)點(diǎn)是否與SearchBox相交；

若滿(mǎn)足Cmax.xBmin.x、Cmax.yBmin.y、Cmax.zBmin.z、Cmin.xBmax.x、Cmin.yBmax.y和Cmin.zBmax.z中的任意一個(gè)條件，則子節(jié)點(diǎn)不與SearchBox相交，跳過(guò)該子節(jié)點(diǎn)并遍歷下一個(gè)子節(jié)點(diǎn)；

否則，子節(jié)點(diǎn)與SearchBox相交，繼續(xù)判斷當(dāng)前子節(jié)點(diǎn)是否為葉子節(jié)點(diǎn)，若不是則遍歷當(dāng)前子節(jié)點(diǎn)的子節(jié)點(diǎn)，直到成為葉子節(jié)點(diǎn)，然后判斷葉子節(jié)點(diǎn)內(nèi)是否包含反光柱三角形且反光柱三角形坐標(biāo)位于SearchBox內(nèi)，若是則將該反光柱三角形記錄，若不是則繼續(xù)上述的遍歷操作，直至遍歷完成；

其中，SearchBox為以反光柱三角形在八叉樹(shù)中的位置點(diǎn)為中心確定的正方體。