CN113268697A - 基于Go-ICP的室外机器人重定位方法 - Google Patents

基于Go-ICP的室外机器人重定位方法 Download PDF

Info

Publication number
CN113268697A
CN113268697A CN202110672612.5A CN202110672612A CN113268697A CN 113268697 A CN113268697 A CN 113268697A CN 202110672612 A CN202110672612 A CN 202110672612A CN 113268697 A CN113268697 A CN 113268697A
Authority
CN
China
Prior art keywords
search
icp
rotation
algorithm
drift
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110672612.5A
Other languages
English (en)
Inventor
王锦山
王宇雨
于泠汰
冼志怀
方锐涌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
CHANGSHA HAIGE BEIDOU INFORMATION TECHNOLOGY CO LTD
Original Assignee
CHANGSHA HAIGE BEIDOU INFORMATION TECHNOLOGY CO LTD
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by CHANGSHA HAIGE BEIDOU INFORMATION TECHNOLOGY CO LTD filed Critical CHANGSHA HAIGE BEIDOU INFORMATION TECHNOLOGY CO LTD
Priority to CN202110672612.5A priority Critical patent/CN113268697A/zh
Publication of CN113268697A publication Critical patent/CN113268697A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种基于Go‑ICP的室外机器人重定位方法，包括获取现有的Go‑ICP算法；对用于剪枝的下界进行修改；在分支过程中规定最低分辨率并降低分支个数；若在定位中丢定位则计算重定位搜索范围并进行重定位；采用修改后的Go‑ICP算法进行室外机器人的实时重定位。本发明将Go‑ICP算法立方形搜索区域转换为扁平长方体搜索区域，降低Z轴的搜索范围；降低了Go‑ICP算法分支个数；基础搜索范围+丢定位时间*相应旋转或平移分量的漂移系数来确定Go‑ICP算法重定位搜索范围，从而大大降低了基于Go‑ICP算法的重定位搜索时间，而且效率较高、搜索时间较短且搜索范围合理可靠。

Description

基于Go-ICP的室外机器人重定位方法
技术领域
本发明属于定位技术领域，具体涉及一种基于Go-ICP的室外机器人重定位方法。
背景技术
重定位和3D点云配准是室外移动机器人的关键定位技术。点云配准的目的是求解出同一坐标下不同姿态点云的变换矩阵，利用该矩阵实现多视扫描点云的精确配准，最终获取定位信息。
本质上，关于六自由度(旋转和平移)的3D点云配准问题是典型的非凸优化问题，其目标函数在六维可行域空间中具有多个波峰波谷，从而使得优化求解过程中容易受初始变换矩阵的影响，容易陷入局部最优解。室外移动机器人重定位需要全局优化的3D点云配准算法；Go-ICP算法为其中出色的全局优化的3D点云配准算法。Go-ICP算法作者Yang等基于范数配准模型，首次推导出了关于六维变换域的上下界函数，并利用分支定界算法提出了全局优化的Go-ICP算法，且该算法有效地证明了所求解变换矩阵的全局最优性。
现有的Go-ICP算法，对于旋转和平移的搜索，采用外层先搜索旋转，内层搜索平移的方式进行：
外层旋转空间搜索界：
Figure BDA0003119271960000011
Figure BDA0003119271960000012
式中
Figure BDA0003119271960000021
为ICP目标函数的上界；θt为平移搜索立方；M为点云数据点总数；ei为第i个点的残差；
Figure BDA0003119271960000022
为搜索中心的旋转向量；
Figure BDA0003119271960000023
为旋转向量
Figure BDA0003119271960000024
所对应的旋转矩阵；t为平移向量；
Figure BDA0003119271960000025
为ICP目标函数的下界；
Figure BDA0003119271960000026
为第i个点的旋转不确定半径；
内层平移搜索空间界：
Figure BDA0003119271960000027
Figure BDA0003119271960000028
式中
Figure BDA0003119271960000029
为目标函数在平移变化时的上界；t0为搜索中心的平移向量；Et 为目标函数在平移变化时的下界；γt为平移不确定半径；
旋转搜索空间可以被半径为π的球空间表示；为了处理方便，选用包围球体的最小立方体[-π,π]3作为旋转区域；平移搜索区域立方体由[-ξ,ξ]3表示；初始搜索立方可以由octree(Branch)数据结构划分为子立方。
现有的Go-ICP算法的伪代码如下：
算法1：Go-ICP主算法(外层旋转搜索算法)
输入：数据与地图点云{pi}{mi}，收敛阈值ε，初始位姿[φ0,t0]，初始旋转与平移搜索立方[θφθt]；
输出：最优代价函数值E*，全局最优位姿搜索结果
Figure BDA00031192719600000210
1.将旋转搜索立方区域θφ加入优先队列中
Figure BDA00031192719600000211
(队列以旋转搜索区域下界值作为优先级参考值，队列的front中存储着下界值最低的旋转搜索区域)
2.E*＝+∞
3.Loop:
4.
Figure BDA0003119271960000031
5.if
Figure BDA0003119271960000032
return
Figure BDA0003119271960000033
6.Branch
Figure BDA0003119271960000034
7.for
Figure BDA0003119271960000035
in
Figure BDA0003119271960000036
do
8.
Figure BDA0003119271960000037
9.if
Figure BDA0003119271960000038
then
10.
Figure BDA0003119271960000039
11.end if
12.
Figure BDA00031192719600000310
13.if
Figure BDA00031192719600000311
then
14.continue
15.end if
16.
Figure BDA00031192719600000312
17.end for
18.end loop
算法结束。
在上述的算法中，E*为ICP目标函数的最优值；
Figure BDA00031192719600000313
为旋转搜索空间；
Figure BDA00031192719600000314
为ICP目标函数的下界；pop_front为出队列；Branch为分支，既对搜索空间进行划分；
Figure BDA00031192719600000315
为划分后的第i个旋转搜索空间；N为整数集；
Figure BDA00031192719600000316
为ICP目标函数的上界；TransSearchAlgorithm2为下述“算法2：Go-ICP内层平移搜索算法”；
Figure BDA00031192719600000317
为第i次搜索；
Figure BDA00031192719600000318
为旋转不确定半径；ICPAlign为利用普通ICP算法求解点云配准；push_back为进队列；
Figure BDA0003119271960000041
为将第i个旋转搜索空间和目标函数下界加入队列；
算法2：Go-ICP内层平移搜索算法
输入：数据与地图点云{pi}{mi}，收敛阈值ε，初始位姿
Figure BDA0003119271960000042
初始平移搜索立方θt，当前最佳代价函数值E*，旋转不确定性半径γφ
输出：最优代价函数值
Figure BDA0003119271960000043
全局最优位姿搜索结果t*
Figure BDA0003119271960000044
Figure BDA0003119271960000051
算法结束。
在上述的算法中，Ct为平移搜索空间；Qt.pop_front为出队列；
Figure BDA0003119271960000052
为第i次平移搜索空间的划分；ei为第i个点的残差；
Figure BDA0003119271960000053
为旋转矩阵；
Figure BDA0003119271960000054
为第i个点的旋转不确定半径；Et 为平移变化下的目标函数下界；
Figure BDA0003119271960000055
为平移变化下的目标函数最优值；t*为平移最优解；
Figure BDA0003119271960000056
为平移变化下的目标函数上界；t0为平移搜索中心；
Figure BDA0003119271960000057
为将第i次的平移搜索空间与平移变化下的目标函数下界加入队列。
但是，上述现有的Go-ICP算法，其在运行时，全局重定位搜索范围较大，搜索时间较长，效率较低。
发明内容
本发明的目的在于提供一种效率较高、搜索时间较短且搜索范围合理可靠的基于Go-ICP的室外机器人重定位方法。
本发明提供的这种基于Go-ICP的室外机器人重定位方法，包括如下步骤：
S1.获取现有的Go-ICP算法；
S2.针对步骤S1获取的Go-ICP算法，对用于剪枝的下界进行修改，从而提升算法收敛速度；
S3.针对步骤S2修改后的Go-ICP算法，在分支过程中规定最低分辨率，并降低分支个数，从而提升算法收敛速度；
S4.针对步骤S3修改后的Go-ICP算法，若在定位中丢定位，则计算重定位搜索范围，并进行重定位；
S5.采用步骤S4得到的修改后的Go-ICP算法，进行室外机器人的实时重定位。
步骤S2所述的对用于剪枝的下界进行修改，从而提升算法收敛速度，具体为采用如下算式进行用于剪枝的下界的修改：
Figure BDA0003119271960000061
Figure BDA0003119271960000062
式中
Figure BDA0003119271960000063
为旋转矩阵；x为点数据；
Figure BDA0003119271960000064
为旋转矩阵；σr为旋转搜索立方的半边长；|| ||为2范数；
Figure BDA0003119271960000065
为定义等于；
Figure BDA0003119271960000066
为旋转不确定半径；t平移；t0为平移搜索中心；σt为X搜索立方区域的半边长；γt为平移不确定半径。
步骤S3所述的在分支过程中规定最低分辨率，并降低分支个数，从而提升算法收敛速度，具体为在分支过程中要设定叶节点立方体的最低分辨率，同时在达到最低分辨率时降低分支个数。
步骤S4所述的若在定位中丢定位，则计算重定位搜索范围，具体为采用如下算式确定重定位搜索范围：
Figure BDA0003119271960000067
Figure BDA0003119271960000068
Figure BDA0003119271960000069
tx＝txb+trans_x_drift_ratio*Δt
ty＝tyb+trans_y_drift_ratio*Δt
tz＝tzb+trans_z_drift_ratio*Δt
式中
Figure BDA0003119271960000071
t＝[tx ty tz]；
Figure BDA0003119271960000072
为旋转向量x方向上的搜索基础量；
Figure BDA0003119271960000073
为旋转向量y方向上的搜索基础范围；
Figure BDA0003119271960000074
为旋转向量z方向上的搜索基础范围；rot_x_drift_ratio为旋转搜索范围，在x方向上随时间变化的比例系数；rot_y_drift_ratio为旋转搜索范围，在y方向上随时间变化的比例系数；rot_z_drift_ratio为旋转搜索范围，在z方向上随时间变化的比例系数；txb为位移向量x方向上的搜索基础范围；tyb为位移向量y方向上的搜索基础范围；tzb为位移向量z方向上的搜索基础范围；trans_x_drift_ratio为平移搜索范围，在x方向上随时间变化的比例系数；trans_y_drift_ratio为平移搜索范围，在y方向上随时间变化的比例系数；trans_z_drift_ratio为平移搜索范围，在z方向上随时间变化的比例系数；Δt为时间的变化量。
本发明提供的这种基于Go-ICP的室外机器人重定位方法，将Go-ICP算法立方形搜索区域转换为扁平长方体搜索区域，降低Z轴的搜索范围；降低了Go-ICP算法分支个数；基础搜索范围+丢定位时间*相应旋转或平移分量的漂移系数来确定Go-ICP算法重定位搜索范围，从而大大降低了基于Go-ICP算法的重定位搜索时间，而且效率较高、搜索时间较短且搜索范围合理可靠。
附图说明
图1为本发明方法的方法流程示意图。
图2为本发明方法中搜索范围修改变化示意图。
具体实施方式
如图1所示为本发明方法的方法流程示意图：本发明提供的这种基于Go-ICP的室外机器人重定位方法，包括如下步骤：
S1.获取现有的Go-ICP算法；
S2.针对步骤S1获取的Go-ICP算法，对用于剪枝的下界进行修改，从而提升算法收敛速度；具体为采用如下算式进行用于剪枝的下界的修改：
Figure BDA0003119271960000081
Figure BDA0003119271960000082
式中
Figure BDA0003119271960000083
为旋转矩阵；x为点数据；
Figure BDA0003119271960000084
为旋转矩阵；σr为旋转搜索立方的半边长；||||为2范数；
Figure BDA0003119271960000085
为定义等于；
Figure BDA0003119271960000086
为旋转不确定半径；t平移；t0为平移搜索中心；σt为X搜索立方区域的半边长；γt为平移不确定半径。
具体实施时，其中旋转范围变化如图2(a)所示，将球形旋转向量范围变为斜线圆柱所示以绕Z轴为主要旋转轴的旋转角度搜索范围，由旋转向量的向量方向为旋转轴方向，模值为旋转大小的物理意义来讲，圆柱的地面直径代表了roll和pitch的搜索范围，而高度代表了yaw角度的搜索范围；而平移的范围变化如上图2(b)所示，将立方形搜索区域转换为斜线扁平长方体搜索区域，降低Z轴的搜索范围；
S3.针对步骤S2修改后的Go-ICP算法，在分支过程中规定最低分辨率，并降低分支个数，从而提升算法收敛速度；具体为在分支过程中要设定叶节点立方体的最低分辨率，同时在达到最低分辨率时降低分支个数；
S4.针对步骤S3修改后的Go-ICP算法，若在定位中丢定位，则计算重定位搜索范围，并进行重定位；具体为采用如下算式确定重定位搜索范围：
Figure BDA0003119271960000087
Figure BDA0003119271960000088
Figure BDA0003119271960000091
tx＝txb+trans_x_drift_ratio*Δt
ty＝tyb+trans_y_drift_ratio*Δt
tz＝tzb+trans_z_drift_ratio*Δt
式中
Figure BDA0003119271960000092
t＝[tx ty tz]；
Figure BDA0003119271960000093
为旋转向量x方向上的搜索基础量；
Figure BDA0003119271960000094
为旋转向量y方向上的搜索基础范围；
Figure BDA0003119271960000095
为旋转向量z方向上的搜索基础范围；rot_x_drift_ratio为旋转搜索范围，在x方向上随时间变化的比例系数；rot_y_drift_ratio为旋转搜索范围，在y方向上随时间变化的比例系数；rot_z_drift_ratio为旋转搜索范围，在z方向上随时间变化的比例系数；txb为位移向量x方向上的搜索基础范围；tyb为位移向量y方向上的搜索基础范围；tzb为位移向量z方向上的搜索基础范围；trans_x_drift_ratio为平移搜索范围，在x方向上随时间变化的比例系数；trans_y_drift_ratio为平移搜索范围，在y方向上随时间变化的比例系数；trans_z_drift_ratio为平移搜索范围，在z方向上随时间变化的比例系数；Δt为时间的变化量。
上述计算公式中，以基础搜索范围+丢定位时间*相应旋转或平移分量的漂移系数来确定搜索范围大小；
S5.采用步骤S4得到的修改后的Go-ICP算法，进行室外机器人的实时重定位。

Claims (4)

1.一种基于Go-ICP的室外机器人重定位方法，包括如下步骤：
S1.获取现有的Go-ICP算法；
S2.针对步骤S1获取的Go-ICP算法，对用于剪枝的下界进行修改，从而提升算法收敛速度；
S3.针对步骤S2修改后的Go-ICP算法，在分支过程中规定最低分辨率，并降低分支个数，从而提升算法收敛速度；
S4.针对步骤S3修改后的Go-ICP算法，若在定位中丢定位，则计算重定位搜索范围，并进行重定位；
S5.采用步骤S4得到的修改后的Go-ICP算法，进行室外机器人的实时重定位。
2.根据权利要求1所述的基于Go-ICP的室外机器人重定位方法，其特征在于步骤S2所述的对用于剪枝的下界进行修改，从而提升算法收敛速度，具体为采用如下算式进行用于剪枝的下界的修改：
Figure FDA0003119271950000011
Figure FDA0003119271950000012
式中
Figure FDA0003119271950000013
为旋转矩阵；x为点数据；
Figure FDA0003119271950000014
为旋转矩阵；σr为旋转搜索立方的半边长；|| ||为2范数；
Figure FDA0003119271950000015
为定义等于；
Figure FDA0003119271950000016
为旋转不确定半径；t平移；t0为平移搜索中心；σt为X搜索立方区域的半边长；γt为平移不确定半径。
3.根据权利要求2所述的基于Go-ICP的室外机器人重定位方法，其特征在于步骤S3所述的在分支过程中规定最低分辨率，并降低分支个数，从而提升算法收敛速度，具体为在分支过程中要设定叶节点立方体的最低分辨率，同时在达到最低分辨率时降低分支个数。
4.根据权利要求3所述的基于Go-ICP的室外机器人重定位方法，其特征在于步骤S4所述的若在定位中丢定位，则计算重定位搜索范围，具体为采用如下算式确定重定位搜索范围：
Figure FDA0003119271950000021
Figure FDA0003119271950000022
Figure FDA0003119271950000023
tx＝txb+trans_x_drift_ratio*Δt
ty＝tyb+trans_y_drift_ratio*Δt
tz＝tzb+trans_z_drift_ratio*Δt
式中
Figure FDA0003119271950000024
t＝[tx ty tz]；
Figure FDA0003119271950000025
为旋转向量x方向上的搜索基础量；
Figure FDA0003119271950000026
为旋转向量y方向上的搜索基础范围；
Figure FDA0003119271950000027
为旋转向量z方向上的搜索基础范围；rot_x_drift_ratio为旋转搜索范围，在x方向上随时间变化的比例系数；rot_y_drift_ratio为旋转搜索范围，在y方向上随时间变化的比例系数；rot_z_drift_ratio为旋转搜索范围，在z方向上随时间变化的比例系数；txb为位移向量x方向上的搜索基础范围；tyb为位移向量y方向上的搜索基础范围；tzb为位移向量z方向上的搜索基础范围；trans_x_drift_ratio为平移搜索范围，在x方向上随时间变化的比例系数；trans_y_drift_ratio为平移搜索范围，在y方向上随时间变化的比例系数；trans_z_drift_ratio为平移搜索范围，在z方向上随时间变化的比例系数；Δt为时间的变化量。
CN202110672612.5A 2021-06-17 2021-06-17 基于Go-ICP的室外机器人重定位方法 Pending CN113268697A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110672612.5A CN113268697A (zh) 2021-06-17 2021-06-17 基于Go-ICP的室外机器人重定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110672612.5A CN113268697A (zh) 2021-06-17 2021-06-17 基于Go-ICP的室外机器人重定位方法

Publications (1)

Publication Number Publication Date
CN113268697A true CN113268697A (zh) 2021-08-17

Family

ID=77235243

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110672612.5A Pending CN113268697A (zh) 2021-06-17 2021-06-17 基于Go-ICP的室外机器人重定位方法

Country Status (1)

Country Link
CN (1) CN113268697A (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106092104A (zh) * 2016-08-26 2016-11-09 深圳微服机器人科技有限公司 一种室内机器人的重定位方法及装置
WO2020101424A1 (ko) * 2018-11-16 2020-05-22 한국과학기술원 지상 로봇의 움직임을 고려한 강인한 레이저 스캐너 매칭 방법 및 그 장치
KR20200057586A (ko) * 2018-11-16 2020-05-26 한국과학기술원 지상 로봇의 움직임을 고려한 강인한 레이저 스캐너 매칭 방법 및 그 장치
CN112102377A (zh) * 2020-08-04 2020-12-18 广东工业大学 基于切比雪夫的icp点云全局最优配准方法及装置
CN112651944A (zh) * 2020-12-28 2021-04-13 哈尔滨工业大学（深圳） 基于cad模型的3c部件高精度六维位姿估计方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106092104A (zh) * 2016-08-26 2016-11-09 深圳微服机器人科技有限公司 一种室内机器人的重定位方法及装置
WO2020101424A1 (ko) * 2018-11-16 2020-05-22 한국과학기술원 지상 로봇의 움직임을 고려한 강인한 레이저 스캐너 매칭 방법 및 그 장치
KR20200057586A (ko) * 2018-11-16 2020-05-26 한국과학기술원 지상 로봇의 움직임을 고려한 강인한 레이저 스캐너 매칭 방법 및 그 장치
CN112102377A (zh) * 2020-08-04 2020-12-18 广东工业大学 基于切比雪夫的icp点云全局最优配准方法及装置
CN112651944A (zh) * 2020-12-28 2021-04-13 哈尔滨工业大学（深圳） 基于cad模型的3c部件高精度六维位姿估计方法及系统

Similar Documents

Publication Publication Date Title
US7876967B2 (en) Icosahedral modified generalized balanced ternary and aperture 3 hexagon tree
CN108594816B (zh) 一种通过改进orb-slam算法实现定位与构图的方法和系统
CN108917759A (zh) 基于多层次地图匹配的移动机器人位姿纠正算法
CN108230247B (zh) 基于云端的三维地图的生成方法、装置、设备及计算机可读的存储介质
CN113706710B (zh) 基于fpfh特征差异的虚拟点多源点云融合方法及系统
CN106767833B (zh) 一种融合rgbd深度传感器与编码器的机器人定位方法
CN112418129B (zh) 一种点云数据处理的方法、装置、电子设备及存储介质
CN114004869A (zh) 一种基于3d点云配准的定位方法
CN108537844A (zh) 一种融合几何信息的视觉slam回环检测方法
CN115774265A (zh) 用于工业机器人的二维码和激光雷达融合定位方法与装置
CN116734856A (zh) 一种考虑洋流影响的水下机器人路径规划方法
CN109785221A (zh) 一种数字图像隐写方法及秘密信息提取方法
CN105427177A (zh) 一种基于gis的农地四至自动计算方法
Gao et al. MGG: Monocular global geolocation for outdoor long-range targets
CN113268697A (zh) 基于Go-ICP的室外机器人重定位方法
CN111964665A (zh) 基于车载环视图像的智能车定位方法、系统及存储介质
Zhang et al. Accurate real-time SLAM based on two-step registration and multimodal loop detection
CN102736112B (zh) 可变旋转平面的电磁跟踪搜索方法及其系统
CN116399354A (zh) 高精度低漂移大范围的三维点云地图构建及重定位方法
CN114777775A (zh) 一种多传感器融合的定位方法及系统
CN115408382A (zh) 面向大规模卫星数据的矢量空间信息网格化存储管理方法
CN112461223B (zh) 一种磁力计零偏无关的磁场指纹库生成方法
CN107945277A (zh) 基于增材再制造点云模型的等距曲面分层方法
CN1796939A (zh) 一种基于lpt变换的星图识别方法
CN114527501A (zh) 一种vrs最优格网点快速查找方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination