[發明專利]機器人定位方法、裝置、計算機可讀存儲介質及機器人

有效

• 申請號: 201911399798.0
• 申請日: 2019-12-30
• 公開（公告）號: CN111113422B
• 公開（公告）日: 2021-10-29
• 發明（設計）人: 張思民;劉志超;劉洪劍;趙文愷;趙云;龐建新;熊友軍
• 申請（專利權）人: 深圳市優必選科技股份有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 深圳中一聯合知識產權代理有限公司 44414
• 代理人: 劉永康

1.一種機器人定位方法，其特征在于，包括：

使用粒子濾波算法對機器人進行定位，得到第一定位位姿；

使用牛頓迭代優化算法對所述第一定位位姿進行矯正計算，得到矯正的位姿增量；判斷所述位姿增量是否大于預設的閾值；

若所述位姿增量大于預設的閾值，則將所述第一定位位姿確定為所述機器人的定位結果；

若所述位姿增量小于或等于所述閾值，則根據所述第一定位位姿和所述位姿增量計算第二定位位姿，并將所述第二定位位姿確定為所述機器人的定位結果；

所述根據所述第一定位位姿和所述位姿增量計算第二定位位姿包括：

根據下式計算所述第二定位位姿：

ξ′＝ξ+Δξ

其中，ξ為所述第一定位位姿，Δξ為所述位姿增量，ξ′為所述第二定位位姿。

2.根據權利要求1所述的機器人定位方法，其特征在于，所述使用粒子濾波算法對機器人進行定位，得到第一定位位姿包括：

獲取對所述機器人進行定位的粒子集合；

根據預設的運動模型對所述粒子集合中的各個粒子的狀態分別進行更新，得到各個粒子的更新狀態；

獲取激光雷達的觀測數據；

根據所述觀測數據和各個粒子的更新狀態分別計算各個粒子的權重；

根據各個粒子的權重進行重采樣，得到更新后的粒子集合；

根據所述更新后的粒子集合對所述機器人進行定位，得到所述第一定位位姿。

3.根據權利要求2所述的機器人定位方法，其特征在于，所述根據各個粒子的權重進行重采樣，得到更新后的粒子集合包括：

根據目標粒子的權重計算所述目標粒子的采樣概率，所述目標粒子為所述粒子集合中的任意一個粒子，且所述目標粒子的采樣概率與所述目標粒子的權重正相關；

根據所述目標粒子的采樣概率對所述目標粒子進行采樣，并將采樣結果添加入所述更新后的粒子集合中。

4.根據權利要求2所述的機器人定位方法，其特征在于，所述使用牛頓迭代優化算法對所述第一定位位姿進行矯正計算，得到矯正的位姿增量包括：

根據下式計算所述位姿增量：

Δξ＝H^-1dTr

其中，ξ為所述第一定位位姿，i為所述觀測數據中的各個激光點的序號，1≤i≤n，n為激光點的數目，S_i(ξ)為第i個激光點變換到柵格地圖坐標系下的坐標，M(S_i(ξ))為S_i(ξ)被占據的概率，Δξ為所述位姿增量。

5.根據權利要求4所述的機器人定位方法，其特征在于，還包括：

根據下式計算第i個激光點變換到柵格地圖坐標系下的坐標：

其中，(p_x,p_y)為所述第一定位位姿中的位置坐標，為所述第一定位位姿中的姿態角，為第i個激光點的位置坐標。

6.一種機器人定位裝置，其特征在于，包括：

粒子濾波模塊，用于使用粒子濾波算法對機器人進行定位，得到第一定位位姿；

牛頓迭代優化模塊，用于使用牛頓迭代優化算法對所述第一定位位姿進行矯正計算，得到矯正的位姿增量；判斷所述位姿增量是否大于預設的閾值；

第一定位結果確定模塊，用于若所述位姿增量大于預設的閾值，則將所述第一定位位姿確定為所述機器人的定位結果；

第二定位結果確定模塊，用于若所述位姿增量小于或等于所述閾值，則根據所述第一定位位姿和所述位姿增量計算第二定位位姿，并將所述第二定位位姿確定為所述機器人的定位結果；

所述根據所述第一定位位姿和所述位姿增量計算第二定位位姿包括：

根據下式計算所述第二定位位姿：

ξ′＝ξ+Δξ

其中，ξ為所述第一定位位姿，Δξ為所述位姿增量，ξ′為所述第二定位位姿。