[發(fā)明專利]一種定位方法和裝置

有效

• 申請?zhí)枺?/td>: 201710147835.3
• 申請日: 2017-03-13
• 公開（公告）號: CN108571967B
• 公開（公告）日: 2020-06-26
• 發(fā)明（設(shè)計）人: 彭志遠(yuǎn);史思總;劉波;吳本全;查盛
• 申請（專利權(quán)）人: 深圳市朗馳欣創(chuàng)科技股份有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 深圳中一聯(lián)合知識產(chǎn)權(quán)代理有限公司 44414
• 代理人: 張全文

1.一種定位方法，其特征在于，所述定位方法包括：

獲取初始點云數(shù)據(jù)，其中，所述初始點云數(shù)據(jù)包括預(yù)設(shè)個數(shù)的采樣點的采樣信息，所述采樣信息包括所述采樣點的位置坐標(biāo)和方向角；

計算所述初始點云數(shù)據(jù)與預(yù)設(shè)時間段內(nèi)的歷史點云數(shù)據(jù)之間的點云匹配度；

按照預(yù)設(shè)數(shù)量的步長變化量修改所述初始點云數(shù)據(jù)，得到所述預(yù)設(shè)數(shù)量的初始參考點云數(shù)據(jù)，并計算每個所述初始參考點云數(shù)據(jù)與所述歷史點云數(shù)據(jù)之間的點云匹配度，其中，所述步長變化量包括位置坐標(biāo)變化量和方向角變化量；

根據(jù)所述初始點云數(shù)據(jù)的點云匹配度和每個所述初始參考點云數(shù)據(jù)的點云匹配度，確定最大點云匹配度對應(yīng)的點云數(shù)據(jù)，作為第一點云數(shù)據(jù)；

按照所述預(yù)設(shè)數(shù)量的步長變化量修改所述第一點云數(shù)據(jù)，得到所述預(yù)設(shè)數(shù)量的第一參考點云數(shù)據(jù)，并計算每個所述第一參考點云數(shù)據(jù)與所述歷史點云數(shù)據(jù)之間的點云匹配度；

根據(jù)所述第一點云數(shù)據(jù)的點云匹配度和每個所述第一參考點云數(shù)據(jù)的點云匹配度，確定最大點云匹配度對應(yīng)的點云數(shù)據(jù)，作為第二點云數(shù)據(jù)；

若所述第一點云數(shù)據(jù)的點云匹配度大于或者等于所述第二點云數(shù)據(jù)的點云匹配度，則根據(jù)所述第一點云數(shù)據(jù)輸出定位信息，其中，所述定位信息包括機器人的位置和姿態(tài)。

2.根據(jù)權(quán)利要求1所述的定位方法，其特征在于，所述獲取初始點云數(shù)據(jù)包括：

采集每個所述采樣點的里程計信息和激光傳感器信息；

根據(jù)所述里程計信息計算每個所述采樣點的所述位置坐標(biāo)和所述方向角；

將所述位置坐標(biāo)和所述方向角與所述激光傳感器信息關(guān)聯(lián)，生成所述初始點云數(shù)據(jù)。

3.根據(jù)權(quán)利要求1所述的定位方法，其特征在于，所述計算所述初始點云數(shù)據(jù)與預(yù)設(shè)時間段內(nèi)的歷史點云數(shù)據(jù)之間的點云匹配度包括：

按照點云匹配條件，將所述初始點云數(shù)據(jù)與所述預(yù)設(shè)時間段內(nèi)的歷史點云數(shù)據(jù)進(jìn)行匹配，得到滿足所述點云匹配條件的歷史點云數(shù)據(jù)，其中，所述點云匹配條件包括所述初始點云數(shù)據(jù)中的每個所述采樣點與所述歷史點云數(shù)據(jù)中的對應(yīng)采樣點之間的距離的平方和最小；

計算所述初始點云數(shù)據(jù)與所述滿足所述點云匹配條件的歷史點云數(shù)據(jù)之間的角度偏差和距離偏差，其中，所述距離偏差包括所述初始點云數(shù)據(jù)中的每個所述采樣點與所述滿足所述點云匹配條件的歷史點云數(shù)據(jù)中的對應(yīng)采樣點之間的距離誤差；所述角度偏差是初始點云數(shù)據(jù)和歷史點云數(shù)據(jù)之間的中心角度偏差，或者是每個對應(yīng)采樣點之間的角度偏差的綜合；

按照如下公式計算所述初始點云數(shù)據(jù)與所述滿足所述點云匹配條件的歷史點云數(shù)據(jù)之間的點云匹配度：

其中，S為所述點云匹配度，α為預(yù)設(shè)的可信度參數(shù)，θ為所述角度偏差，β為預(yù)設(shè)的匹配度參數(shù)，M_i為所述距離誤差小于預(yù)設(shè)的誤差閾值的所述采樣點中第i個采樣點對應(yīng)的距離誤差，N為所述距離誤差小于預(yù)設(shè)的誤差閾值的所述采樣點的個數(shù)。

4.根據(jù)權(quán)利要求1至3任一項所述的定位方法，其特征在于，所述若所述第一點云數(shù)據(jù)的點云匹配度大于或者等于所述第二點云數(shù)據(jù)的點云匹配度，則根據(jù)所述第一點云數(shù)據(jù)輸出定位信息之后，所述定位方法還包括：

若所述第一點云數(shù)據(jù)的點云匹配度小于所述第二點云數(shù)據(jù)的點云匹配度，則獲取當(dāng)前的優(yōu)化次數(shù)；

若所述優(yōu)化次數(shù)未達(dá)到預(yù)設(shè)的次數(shù)閾值，則將所述第二點云數(shù)據(jù)識別為所述第一點云數(shù)據(jù)，執(zhí)行所述按照所述預(yù)設(shè)數(shù)量的步長變化量修改所述第一點云數(shù)據(jù)，得到所述預(yù)設(shè)數(shù)量第一參考點云數(shù)據(jù)，并計算每個所述第一參考點云數(shù)據(jù)與所述歷史點云數(shù)據(jù)之間的點云匹配度。

5.根據(jù)權(quán)利要求4所述的定位方法，其特征在于，所述若所述第一點云數(shù)據(jù)的點云匹配度小于所述第二點云數(shù)據(jù)的點云匹配度，則獲取當(dāng)前的優(yōu)化次數(shù)之后，所述定位方法還包括：

若所述優(yōu)化次數(shù)達(dá)到所述次數(shù)閾值，則根據(jù)所述第二點云數(shù)據(jù)輸出所述定位信息。