[發(fā)明專利]一種激光雷達(dá)機器人重定位方法

在審

• 申請?zhí)枺?/td>: 202211628840.3
• 申請日: 2022-12-18
• 公開（公告）號: CN115902912A
• 公開（公告）日: 2023-04-04
• 發(fā)明（設(shè)計）人: 沈姚聞;張盛鍇;劉華平;劉孜捷
• 申請（專利權(quán)）人: 上海奧歐智能科技有限公司
• 主分類號: G01S17/06
• 分類號: G01S17/06;G01S17/86;G01S5/02
• 代理公司: 上海助動專利代理事務(wù)所(普通合伙) 31492
• 代理人: 周翠娟

1.一種激光雷達(dá)機器人重定位方法，其特征在于：機器人在收到重定位請求后，機器人的檢查標(biāo)簽提供相對基站的位置和置信度，給出一個搜索窗口，對柵格地圖事先進(jìn)行模糊化處理，相鄰的地圖像素灰度值調(diào)整為補償范圍內(nèi)最大的灰度值，依據(jù)補償范圍的大小給出多個補償?shù)貓D，在像素最低的補償?shù)貓D內(nèi)刪選高出給定閾值的候選點，候選點在高像素的地圖依據(jù)雷達(dá)擊中在柵格地圖的得分進(jìn)行分支定界搜索，最高得分即為重定位的結(jié)果。

2.根據(jù)權(quán)利要求1所述的一種激光雷達(dá)機器人重定位方法，其特征在于：還包括在重定位前機器人通過雷達(dá)對工作環(huán)境進(jìn)行掃描，以柵格地圖的方式存儲在機器人內(nèi)且基站已在柵格地圖上標(biāo)出。柵格地圖存有初始的磁力計指北信息。

3.根據(jù)權(quán)利要求1所述的一種激光雷達(dá)機器人重定位方法，其特征在于：還包括使用EKF融合射頻定位標(biāo)簽的觀測數(shù)據(jù)和里程計的運動模型，得到機器人相對定位基站的位置，所述射頻定位的觀測數(shù)據(jù)為標(biāo)簽相對基站的距離range，以及由磁力計得到相對基站的角度angle。

4.根據(jù)權(quán)利要求3所述的一種激光雷達(dá)機器人重定位方法，其特征在于：還包括由拓展卡爾曼濾波EKF可知基站定位機器人需預(yù)測步和更新步，其中

預(yù)測步為：

跟新步為：

(1)預(yù)測步

根據(jù)輪式運動模型得到的機器人可能在的位置；

其中x_k

預(yù)測步對應(yīng)的協(xié)方差矩陣G_t則為：

(2)跟新步

觀測矩陣z_t為機器人觀測到相對基站的距離和角度：

觀測矩陣的雅克比矩陣H_t為：

5.根據(jù)權(quán)利要求4所述的一種激光雷達(dá)機器人重定位方法，其特征在于，所述候選點在高像素的地圖依據(jù)雷達(dá)擊中在柵格地圖的得分進(jìn)行分支定界搜索具體為：

需要檢索到柵格地圖中與當(dāng)前雷達(dá)點云最匹配的點作為機器人的定位起始點，即如下式所示，其中W為搜索窗口內(nèi)所有點的集合；

其中W代表線性搜索窗口和角度搜索窗口：

在柵格地圖檢索過程中，通過算法定義節(jié)點Node,分支規(guī)則Branch,以及上界的計算方式Upperbounds。

6.根據(jù)權(quán)利要求5所述的一種激光雷達(dá)機器人重定位方法，其特征在于：所述Node定義具體為

算法使用Depth-first?Serach(DFS)深度優(yōu)先算法作為默認(rèn)的最優(yōu)解候選，DFS能快速評價葉節(jié)點，使用得分閾值裁剪掉得分差的節(jié)點，計算每個子節(jié)點的上節(jié)，優(yōu)先訪問最有可能的子節(jié)點的最大邊界。

7.根據(jù)權(quán)利要求5所述的一種激光雷達(dá)機器人重定位方法，其特征在于：所述Branch定義具體為

每一個節(jié)點由整數(shù)元組c＝(c_x,cy,c_θ,c_h)∈z⁴表示，節(jié)點代表了一個2^ch×2^ch個可能的平移位置以及角度：

8.根據(jù)權(quán)利要求5所述的一種激光雷達(dá)機器人重定位方法，其特征在于：所述計算上界Upper?bounds

對原始的柵格地圖分別采用不同的補償范圍，設(shè)置預(yù)先計算的網(wǎng)絡(luò)，將范圍內(nèi)的像素值設(shè)置為這個區(qū)域內(nèi)的最大值，保存于數(shù)組視為n個補償?shù)貓D，預(yù)先計算一個網(wǎng)格每個可能的高度c_h允許計算與掃描點的數(shù)量的分?jǐn)?shù)，為了方便計算，在搜索空間邊界附近的以上的的最大值，

計算上界時，先計算較低分辨率的概率網(wǎng)格，依次將分辨率減半，直至分辨率c_h為0。

9.根據(jù)權(quán)利要求5所述的一種激光雷達(dá)機器人重定位方法，其特征在于：

所述方法具體包括步驟如下：

S100、接收請求訊號；

S200、獲取相對基站的位置和置信度；

S300、根據(jù)機器人相對定位基站的位置作為搜索窗口進(jìn)行搜索；

S400、對柵格地圖模糊化處理；

S500、依據(jù)補償范圍的大小給出多個補償?shù)貓D；

S600、在像素最低的補償?shù)貓D內(nèi)刪選高出給定閾值的候選點；

S700、根據(jù)候選點在高像素的地圖依據(jù)雷達(dá)擊中在柵格地圖的得分進(jìn)行分支定界搜索；

S800、最高得分即為重定位的結(jié)果。