CN113686321A - 一种高效地面绝对标靶提取和全局优化方法 - Google Patents

一种高效地面绝对标靶提取和全局优化方法 Download PDF

Info

Publication number
CN113686321A
CN113686321A CN202111245537.0A CN202111245537A CN113686321A CN 113686321 A CN113686321 A CN 113686321A CN 202111245537 A CN202111245537 A CN 202111245537A CN 113686321 A CN113686321 A CN 113686321A
Authority
CN
China
Prior art keywords
target
backpack
constraint
optimization
coordinates
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.)
Granted
Application number
CN202111245537.0A
Other languages
English (en)
Other versions
CN113686321B (zh
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.)
Leador Spatial Information Technology Co ltd
Original Assignee
Leador Spatial 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 Leador Spatial Information Technology Co ltd filed Critical Leador Spatial Information Technology Co ltd
Priority to CN202111245537.0A priority Critical patent/CN113686321B/zh
Publication of CN113686321A publication Critical patent/CN113686321A/zh
Application granted granted Critical
Publication of CN113686321B publication Critical patent/CN113686321B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T11/00—2D [Two Dimensional] image generation
    • G06T11/20—Drawing from basic elements, e.g. lines or circles
    • G06T11/206—Drawing of charts or graphs

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种高效地面绝对标靶提取和全局优化方法。针对现有技术中存在的问题，本发明提供了一种高效的地面绝对标靶提取方法以及包含绝对坐标约束等四种残差方程的全局优化算法，解决了绝对坐标转换精度不高；无绝对坐标约束SLAM算法有累计误差；特殊场景算法退化；标靶图像提取操作繁琐，且有诸多误差的问题。

Description

一种高效地面绝对标靶提取和全局优化方法
技术领域
本发明属于移动测绘领域，具体涉及一种标靶坐标获取方法以及一种融合绝对坐标约束的全局轨迹优化算方法。
背景技术
便携式移动测量背包，是一种新型的移动测绘技术，和移动测绘车一样能够实现移动测量，地图采集等功能。背包上通常配有激光雷达，惯导等器件，采用背负式的方式进行数据采集，然后通过3D-SLAM算法进行数据处理，最终生成3D点云。便携式移动测量背包特别适用于园区、室内、楼宇、地下等环境，是移动测绘车在测绘场景上的补充。在使用背包进行测图的过程中，通常会遇到以下问题。
1. 绝对坐标转换问题
背包的应用环境通常没有卫星信号，在这种情况下，数据处理之后得到的点云的坐标是在局部坐标系下的，该坐标系以数据采集开始时刻的背包位姿为坐标系原点。有一种方法，求解从该局部坐标系到绝对坐标系的坐标变换，然后把整个点云变换到绝对坐标下。方法步骤：1）在场景中布置一些控制点，或者选取一些明显的点；2）使用全站仪测得这些点的绝对坐标；3）通过最小二乘法求出这个坐标变换矩阵；4）整个点云坐标变换。该方法可以得到据对坐标系下的点云，但是操作很繁琐，精度也不高。
2. 累计误差问题
背包测图采用的3D-SLAM算法，在没有卫星信号情况下，采用点云配准融合惯导解算，这是一个马尔科夫过程，每一帧的位姿估计都是基于上一帧位姿的相对增量，虽然得到的是最优的估计，但是还是存在微小误差。这种方式短时间内精度还可以保证，但是随着时间推移误差会逐渐累计，在没有引入绝对坐标约束时对测图的精度影响很大。
3. 特殊场景问题
3D-SLAM算法中点云配准基于场景的几何特征，在几何特征丰富的场景中有很好的效果，但是在长廊、隧道等缺乏特征的场景中配准精度会下降，会出现退化现象，也会影像测图精度。
4. 图像识别问题
通过十字标靶引入控制点是移动测绘领域中常用的方法。采用基于图像识别的方式，在镜头对准标靶时操作不方便，很耗时且效率不高；在识别标靶时会有识别误差，尺度误差等，会影响测图精度。
为了解决以上问题，需要一种高效地面绝对标靶提取和全局优化算法。
发明内容
为了解决上述的技术问题，本发明提供一种高效地面绝对标靶提取和全局优化方法。
为了达到上述目的，本发明提供的技术方案是：一种高效地面绝对标靶提取和全局优化方法，包括如下步骤：
步骤1，布置控制点；首先根据要采集的环境，规划好合适的路线，然后在采集路线上，布置控制点标靶；
步骤2，测量控制点坐标；使用全站仪测量设备，测量所有的标靶的绝对坐标，并记录标靶的编号；
步骤3，数据采集；启动背包的数据采集功能，沿着预先规划好的路线行进；
步骤4，采集标靶；提取标靶坐标，同时使背包控制系统记录下此时刻的时间戳，并把标靶编号录入到背包控制系统中；
步骤5，优化处理；把测好的标靶坐标录入到背包处理软件中，进行全局优化处理，得到优化后的轨迹；
步骤6，利用优化后得到的轨迹重新生成地图。
进一步的，步骤4中采用物理接触的方式来提取标靶坐标，采集标靶的设备包括一个结构件和一个触发装置，所述结构件的一端连接在背包设备上，一端有一个尖端的形状，称之为标靶指针，标靶指针接触到标靶中心时，按下触发开关，背包控制系统会记录下此刻的时间，以及对应的标靶编号。
进一步的，使用采集标靶的设备时，事先在标定场中，通过外参标定算法，标定出标靶指针尖端到背包坐标系中心准确的外参T LM ，即标靶指针到背包中心的坐标系的变换关系；
p lidai =T LM p mark
p lidai 表示激光坐标系下的点的坐标，p mark 表示标靶坐标系下的点的坐标。
进一步的，步骤5中所述全局优化，是指采用非线性优化的方式，对轨迹进行优化，全局优化的框架是一个无向图结构，无向图中的节点是背包某一时刻的状态（X0，X1，X2，X3 ... Xi-1，Xi），无向图的边是节点之间的状态约束；
以状态变量作为优化变量，其中状态变量为X=[q T p T v T b T ] T ， q表示背包姿态，p表示背包位置，v表示背包速度，b表示惯导漂移；状态约束包括（1）惯导预积分约束，（2）激光里程计约束，（3）标靶绝对坐标约束，（4）闭环检测约束；各项的约束方程如下所示，
Figure 842282DEST_PATH_IMAGE001
Figure 697105DEST_PATH_IMAGE002
Figure 929372DEST_PATH_IMAGE003
Figure 262264DEST_PATH_IMAGE004
其中r imu ,r lidar ,r mark ,r loop 分别表示惯导预积分约束、激光里程计约束、标靶绝对 坐标约束、闭环检测约束的残差，ξ i , ξ i+1 ,ξ j 表示i、i+1、j时刻的优化变量对应的李代数；ξ imu , ξ lidar ,ξ loop 分别表示惯导测量值、激光测量值、闭环测量值的李代数，李代数由 (q, p)组合得到，
Figure 499473DEST_PATH_IMAGE005
表示将李代数转换为相应的变换矩阵；p mark 表示标靶测量值的坐标； 激光和闭环的李代数是通过点云匹配直接得到，惯导的李代数需要通过加速度计和陀螺仪 的测量值预积分得到，预积分由以下公式计算得到：
Figure 259619DEST_PATH_IMAGE006
Figure DEST_PATH_IMAGE007
Figure 916865DEST_PATH_IMAGE008
其中，g表示重力加速度，a表示加速度测量值，w表示角速度测量值，i、i+1、j分别表示i时刻、i+1时刻、j时刻， △t表示时间间隔；
将四种约束输入到全局优化框架中，得到总的残差项的表达式如下：
r=r imu +r lidar +r mark +r loop
最后使用非线性优化库进行优化，最终求得优化变量的最优值。
进一步的，非线性优化库包括ceres，gtsam，g2o。
进一步的，采用LM算法进行优化，其流程为：在每一次迭代中，求取一个优化变量的增量，使得总的残差项的值下降最快，之后不断迭代，直至总的残差项不再改变，优化迭代过程趋于收敛，最终求得优化变量的最优值。
与现有技术相比，本发明的优点和有益效果：
（1）该方法采用位姿图非线性优化的方式，来调整采集轨迹中每个点的位姿，以达到整体轨迹误差最小，最后以最优解作为基准轨迹，重新绘制出完整点云地图，以替代整体点云地图的坐标变换的方式。最终得到拥有准确的绝对坐标的地图。
（2）该方法能提供绝对坐标约束，可以解决点云匹配和惯导解算提供的相对约束，随时间推移误差逐渐增大的问题。
（3）该方法能提供绝对坐标约束，可以解决在几何特征较少，甚至几何特征缺失场景中点云匹配不稳定的问题。
（4）该方法采用物理接触的方式引入绝对坐标约束，规避了图像识别控制点的问题。该方法不仅避免了误差，而且操作灵活简便，同时不受地形约束。
附图说明
图1为本发明中触发开关示意图。
图2为本发明中结构件示意图。
图3为本发明中标靶提取示意图。
图4为本发明中标靶提取流程图。
图5为本发明中全局优化示意图。
图6为本发明中全局优化流程图。
具体实施方式
下面结合附图和实施例对本发明的技术方案作进一步说明。
本实施例提供一种高效地面绝对标靶提取和全局优化方法，其具体的工作过程包括六个阶段，具体为：
1）布置控制点
首先根据要采集的环境，规划好合适的路线；然后在采集路线上，布置控制点标靶，可以布置在地面，墙体等方便标靶指针接触到，同时便于全站仪等测量设备测量的地方。
2）测量控制点坐标
使用全站仪等测量设备，测量所有的标靶的绝对坐标，并记录标靶的编号。
3）开始数据采集
启动背包的数据采集功能，沿着预先规划好的路线行进。
4）采集标靶
每走到一个标靶位置，用标靶指针接触到标靶中心，同时激活触发装置，使背包控制系统记录下此时刻的时间戳，并把标靶编号录入到背包控制系统中。
本方案采用物理接触的方式来提取标靶坐标，需要一个结构件以及触发装置。其中，结构件如图中所示，一端连接在背包设备上，一端有一个尖端的形状，称之为标靶指针，这个标靶指针可以方便准确的指到标靶的中心。
使用时，事先在标定厂中，通过外参标定算法，标定出标靶指针尖端到背包坐标系中心准确的外参T LM ，即标靶指针到背包中心的坐标系的变换关系。
p lidai =T LM p mark
p lidai 表示激光坐标系下的点的坐标，p mark 表示标靶坐标系下的点的坐标。
在标靶指针接触到标靶中心时，按下触发开关，系统会记录下此刻的时间，以及对应的标靶编号，最终会得到一个加了时间戳的标靶坐标列表。通过列表就可以检索到每个标靶坐标在全局优化中对应的优化变量节点。
5）优化处理
把测好的标靶坐标录入到背包处理软件中，进行全局优化处理，得到优化后的轨迹。
全局优化，是指采用非线性优化的方式，对轨迹进行优化。全局优化的框架如图5所示，整体是一个无向图结构，无向图中的节点是背包某一时刻的状态（X0，X1，X2，X3 ...Xi-1，Xi）；无向图的边是节点之间的状态约束。以状态变量作为优化变量，X=[q T p T v T b T ] T ，q表示背包姿态，p表示背包位置，v表示背包速度，b表示惯导漂移。约束项包括（1）惯导预积分约束，（2）激光里程计约束，（3）标靶绝对坐标约束，（4）闭环检测约束。各项的约束方程如下所示。
Figure DEST_PATH_IMAGE009
Figure 473355DEST_PATH_IMAGE002
Figure 63605DEST_PATH_IMAGE010
Figure 260231DEST_PATH_IMAGE004
其中r imu ,r lidar ,r mark ,r loop 分别表示惯导预积分约束、激光里程计约束、标靶绝对 坐标约束、闭环检测约束的残差，
Figure DEST_PATH_IMAGE011
表示i、i+1、j时刻的优化变量对应的李代数；ξ i , ξ i+1 ,ξ j 表示i、i+1、j时刻的优化变量对应的李代数；ξ imu , ξ lidar ,ξ loop 分别表示惯导 测量值、激光测量值、闭环测量值的李代数，李代数由 (q,p)组合得到，
Figure 375080DEST_PATH_IMAGE012
表示将李 代数转换为相应的变换矩阵；p mark 表示标靶测量值的坐标；激光和闭环的李代数是通过点 云匹配直接得到，惯导的李代数需要通过加速度计和陀螺仪的测量值预积分得到，预积分 由以下公式计算得到：
Figure 193650DEST_PATH_IMAGE006
Figure DEST_PATH_IMAGE013
Figure 576090DEST_PATH_IMAGE008
其中，g表示重力加速度，a表示加速度测量值，w表示角速度测量值，i、i+1、j分别表示i时刻、i+1时刻、j时刻， △t表示时间间隔；
优化变量与其之间的约束在整个图结构中的关系如5图所示，将四种约束输入到全局优化框架中，得到总的残差项的表达式如下：
r=r imu +r lidar +r mark +r loop
将四种约束输入到优化框架中，使用ceres，gtsam，g2o等非线性优化库进行优化。优化采用LM算法，其主要流程为：在每一次迭代中，求取一个优化变量的增量，使得总的残差项的值下降最快。之后不断迭代，直至总的残差项不再改变，优化迭代过程趋于收敛。最终求得优化变量的最优值。
6）重新生成地图
用得到的轨迹重新生成地图。
应当理解的是，本说明书未详细阐述的部分均属于现有技术。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (6)

1.一种高效地面绝对标靶提取和全局优化方法，其特征在于，包括如下步骤：
步骤1，布置控制点；首先根据要采集的环境，规划好合适的路线，然后在采集路线上，布置控制点标靶；
步骤2，测量控制点坐标；使用全站仪测量设备，测量所有的标靶的绝对坐标，并记录标靶的编号；
步骤3，数据采集；启动背包的数据采集功能，沿着预先规划好的路线行进；
步骤4，采集标靶；提取标靶坐标，同时使背包控制系统记录下此时刻的时间戳，并把标靶编号录入到背包控制系统中；
步骤5，优化处理；把测好的标靶坐标录入到背包处理软件中，进行全局优化处理，得到优化后的轨迹；
步骤6，利用优化后得到的轨迹重新生成地图。
2.如权利要求1所述的一种高效地面绝对标靶提取和全局优化方法，其特征在于：步骤4中采用物理接触的方式来提取标靶坐标，采集标靶的设备包括一个结构件和一个触发装置，所述结构件的一端连接在背包设备上，一端有一个尖端的形状，称之为标靶指针，标靶指针接触到标靶中心时，按下触发开关，背包控制系统会记录下此刻的时间，以及对应的标靶编号。
3.如权利要求1所述的一种高效地面绝对标靶提取和全局优化方法，其特征在于：
使用采集标靶的设备时，事先在标定场中，通过外参标定算法，标定出标靶指针尖端到背包坐标系中心准确的外参T LM ，即标靶指针到背包中心的坐标系的变换关系；
p lidai =T LM p mark
p lidai 表示激光坐标系下的点的坐标，p mark 表示标靶坐标系下的点的坐标。
4.如权利要求1所述的一种高效地面绝对标靶提取和全局优化方法，其特征在于：
步骤5中所述全局优化，是指采用非线性优化的方式，对轨迹进行优化，全局优化的框架是一个无向图结构，无向图中的节点是背包某一时刻的状态（X0，X1，X2，X3 ... Xi-1，Xi），无向图的边是节点之间的状态约束；
以状态变量作为优化变量，其中状态变量为X=[q T p T v T b T ] T ， q表示背包姿态，p表示背包位置，v表示背包速度，b表示惯导漂移；状态约束包括（1）惯导预积分约束，（2）激光里程计约束，（3）标靶绝对坐标约束，（4）闭环检测约束；各项的约束方程如下所示，
Figure 385619DEST_PATH_IMAGE001
Figure 312730DEST_PATH_IMAGE002
Figure 525537DEST_PATH_IMAGE003
Figure 90379DEST_PATH_IMAGE004
其中r imu ,r lidar ,r mark ,r loop 分别表示惯导预积分约束、激光里程计约束、标靶绝对坐标约 束、闭环检测约束的残差，ξ i , ξ i+1 ,ξ j 表示i、i+1、j时刻的优化变量对应的李代数；ξ imu , ξ lidar ,ξ loop 分别表示惯导测量值、激光测量值、闭环测量值的李代数，李代数由 (q,p)组 合得到，
Figure 363228DEST_PATH_IMAGE005
表示将李代数转换为相应的变换矩阵；p mark 表示标靶测量值的坐标；激 光和闭环的李代数是通过点云匹配直接得到，惯导的李代数需要通过加速度计和陀螺仪的 测量值预积分得到，预积分由以下公式计算得到：
Figure 514986DEST_PATH_IMAGE006
Figure 531484DEST_PATH_IMAGE007
Figure 950833DEST_PATH_IMAGE008
其中，g表示重力加速度，a表示加速度测量值，w表示角速度测量值，i、i+1、j分别表示i时刻、i+1时刻、j时刻，△t表示时间间隔；
将四种约束输入到全局优化框架中，得到总的残差项的表达式如下：
r=r imu +r lidar +r mark +r loop
最后使用非线性优化库进行优化，最终求得优化变量的最优值。
5.如权利要求4所述的一种高效地面绝对标靶提取和全局优化方法，其特征在于：非线性优化库包括ceres，gtsam，g2o。
6.如权利要求4所述的一种高效地面绝对标靶提取和全局优化方法，其特征在于：采用LM算法进行优化，其流程为：在每一次迭代中，求取一个优化变量的增量，使得总的残差项的值下降最快，之后不断迭代，直至总的残差项不再改变，优化迭代过程趋于收敛，最终求得优化变量的最优值。
CN202111245537.0A 2021-10-26 2021-10-26 一种高效地面绝对标靶提取和全局优化方法 Active CN113686321B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111245537.0A CN113686321B (zh) 2021-10-26 2021-10-26 一种高效地面绝对标靶提取和全局优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111245537.0A CN113686321B (zh) 2021-10-26 2021-10-26 一种高效地面绝对标靶提取和全局优化方法

Publications (2)

Publication Number Publication Date
CN113686321A true CN113686321A (zh) 2021-11-23
CN113686321B CN113686321B (zh) 2022-01-14

Family

ID=78588018

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111245537.0A Active CN113686321B (zh) 2021-10-26 2021-10-26 一种高效地面绝对标靶提取和全局优化方法

Country Status (1)

Country Link
CN (1) CN113686321B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08122077A (ja) * 1994-10-20 1996-05-17 Fuji Photo Optical Co Ltd 位置計測作図装置のターゲット
CN108332759A (zh) * 2018-01-12 2018-07-27 浙江国自机器人技术有限公司 一种基于3d激光的地图构建方法及系统
CN109357635A (zh) * 2018-11-21 2019-02-19 滨州职业学院 一种基于图像的特色建筑立面图测绘方法
CN112767498A (zh) * 2021-02-03 2021-05-07 苏州挚途科技有限公司 相机标定方法、装置和电子设备

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08122077A (ja) * 1994-10-20 1996-05-17 Fuji Photo Optical Co Ltd 位置計測作図装置のターゲット
CN108332759A (zh) * 2018-01-12 2018-07-27 浙江国自机器人技术有限公司 一种基于3d激光的地图构建方法及系统
CN109357635A (zh) * 2018-11-21 2019-02-19 滨州职业学院 一种基于图像的特色建筑立面图测绘方法
CN112767498A (zh) * 2021-02-03 2021-05-07 苏州挚途科技有限公司 相机标定方法、装置和电子设备

Also Published As

Publication number Publication date
CN113686321B (zh) 2022-01-14

Similar Documents

Publication Publication Date Title
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN103968827B (zh) 一种可穿戴式人体步态检测的自主定位方法
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
Zhou et al. Activity sequence-based indoor pedestrian localization using smartphones
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN110553644B (zh) 一种矿用电铲精准定位系统和方法
CN105547288A (zh) 一种煤矿井下移动设备自主定位的方法及系统
CN104713554A (zh) 一种基于mems惯性器件与安卓智能手机融合的室内定位方法
CN105241454A (zh) 一种基于多传感器的行人导航系统及导航方法
CN102445200A (zh) 微小型个人组合导航系统及其导航定位方法
CN107830858B (zh) 一种基于重力辅助的手机航向估计方法
CN105241445A (zh) 一种基于智能移动终端的室内导航数据获取方法及系统
CN106168485B (zh) 步行航迹数据推算方法及装置
CN107490378B (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
CN107014375B (zh) 超低部署的室内定位系统及方法
CN104613965B (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
CN103196445A (zh) 基于匹配技术的地磁辅助惯性的载体姿态测量方法
CN113175933B (zh) 一种基于高精度惯性预积分的因子图组合导航方法
CN112797985B (zh) 基于加权扩展卡尔曼滤波的室内定位方法及室内定位系统
Yu et al. AZUPT: Adaptive zero velocity update based on neural networks for pedestrian tracking
CN108592907A (zh) 一种基于双向滤波平滑技术的准实时步进式行人导航方法
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
CN104897155B (zh) 一种个人携行式多源定位信息辅助修正方法
CN110260860B (zh) 基于足部惯性传感器的室内移动测量定位定姿方法及系统
Zhang et al. Mag-ODO: Motion speed estimation for indoor robots based on dual magnetometers

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
GR01 Patent grant
GR01 Patent grant