CN113686321B - 一种高效地面绝对标靶提取和全局优化方法 - Google Patents
一种高效地面绝对标靶提取和全局优化方法 Download PDFInfo
- Publication number
- CN113686321B CN113686321B CN202111245537.0A CN202111245537A CN113686321B CN 113686321 B CN113686321 B CN 113686321B CN 202111245537 A CN202111245537 A CN 202111245537A CN 113686321 B CN113686321 B CN 113686321B
- 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.)
- Active
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 57
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000000605 extraction Methods 0.000 title claims abstract description 15
- 238000005259 measurement Methods 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 10
- 230000009466 transformation Effects 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 6
- 230000010354 integration Effects 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 4
- 230000001172 regenerating effect Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 abstract description 3
- 230000000593 degrading effect Effects 0.000 abstract 1
- 238000013507 mapping Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 3
- FAPWRFPIFSIZLT-UHFFFAOYSA-M Sodium chloride Chemical compound [Na+].[Cl-] FAPWRFPIFSIZLT-UHFFFAOYSA-M 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000000977 initiatory effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000011780 sodium chloride Substances 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
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）闭环检测约束；各项的约束方程如下所示，
其中r imu ,r lidar ,r mark ,r loop 分别表示惯导预积分约束、激光里程计约束、标靶绝对
坐标约束、闭环检测约束的残差，ξ i , ξ i+1 ,ξ j 表示i、i+1、j时刻的优化变量对应的李代数；ξ imu , ξ lidar ,ξ loop 分别表示惯导测量值、激光测量值、闭环测量值的李代数，李代数由 (q, p)组合得到， 
表示将李代数转换为相应的变换矩阵；p mark 表示标靶测量值的坐标；
激光和闭环的李代数是通过点云匹配直接得到，惯导的李代数需要通过加速度计和陀螺仪
的测量值预积分得到，预积分由以下公式计算得到：
其中，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）闭环检测约束。各项的约束方程如下所示。
其中r imu ,r lidar ,r mark ,r loop 分别表示惯导预积分约束、激光里程计约束、标靶绝对
坐标约束、闭环检测约束的残差， 
表示i、i+1、j时刻的优化变量对应的李代数；ξ i , ξ i+1 ,ξ j 表示i、i+1、j时刻的优化变量对应的李代数；ξ imu , ξ lidar ,ξ loop 分别表示惯导
测量值、激光测量值、闭环测量值的李代数，李代数由 (q,p)组合得到， 
表示将李
代数转换为相应的变换矩阵；p mark 表示标靶测量值的坐标；激光和闭环的李代数是通过点
云匹配直接得到，惯导的李代数需要通过加速度计和陀螺仪的测量值预积分得到，预积分
由以下公式计算得到：
其中，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 (4)
1.一种高效地面绝对标靶提取和全局优化方法，其特征在于，包括如下步骤：
步骤1，布置控制点；首先根据要采集的环境，规划好合适的路线，然后在采集路线上，布置控制点标靶；
步骤2，测量控制点坐标；使用全站仪测量设备，测量所有的标靶的绝对坐标，并记录标靶的编号；
步骤3，数据采集；启动背包的数据采集功能，沿着预先规划好的路线行进；
步骤4，采集标靶；提取标靶坐标，同时使背包控制***记录下此时刻的时间戳，并把标靶编号录入到背包控制***中；
步骤4中采用物理接触的方式来提取标靶坐标，采集标靶的设备包括一个结构件和一个触发装置，所述结构件的一端连接在背包设备上，一端有一个尖端的形状，称之为标靶指针，标靶指针接触到标靶中心时，按下触发开关，背包控制***会记录下此刻的时间，以及对应的标靶编号；
步骤5，优化处理；把测好的标靶坐标录入到背包处理软件中，进行全局优化处理，得到优化后的轨迹；
步骤5中所述全局优化，是指采用非线性优化的方式，对轨迹进行优化，全局优化的框架是一个无向图结构，无向图中的节点是背包某一时刻的状态(X0，X1，X2，X3...Xi-1，Xi)，无向图的边是节点之间的状态约束；
以状态变量作为优化变量，其中状态变量为X＝[qT pT vT bT]T，q表示背包姿态，p表示背包位置，v表示背包速度，b表示惯导漂移；状态约束包括(1)惯导预积分约束，(2)激光里程计约束，(3)标靶绝对坐标约束，(4)闭环检测约束；各项的约束方程如下所示，
rmark＝pi-pmark (3)
其中rimu，rlidar，rmark，rloop分别表示惯导预积分约束、激光里程计约束、标靶绝对坐标约束、闭环检测约束的残差，ξi，ξi+1，ξj表示i、i+1、j时刻的优化变量对应的李代数；ξimu，ξlidar，ξloop分别表示惯导测量值、激光测量值、闭环测量值的李代数，李代数由(q，p)组合得到，exp(ξ^)表示将李代数转换为相应的变换矩阵；pmark表示标靶坐标系下的点的坐标；激光和闭环的李代数是通过点云匹配直接得到，惯导的李代数需要通过加速度计和陀螺仪的测量值预积分得到，预积分由以下公式计算得到：
vi+1＝vi-gΔt+qi∫t∈[i,i+1](qiai)δt (6)
其中，g表示重力加速度，a表示加速度测量值，w表示角速度测量值，i、i+1、j分别表示i时刻、i+1时刻、j时刻，Δt表示时间间隔；
将四种约束输入到全局优化框架中，得到总的残差项的表达式如下：
r＝rimu+rlidar+rmark+rloop
最后使用非线性优化库进行优化，最终求得优化变量的最优值；
步骤6，利用优化后得到的轨迹重新生成地图。
2.如权利要求1所述的一种高效地面绝对标靶提取和全局优化方法，其特征在于：
使用采集标靶的设备时，事先在标定场中，通过外参标定算法，标定出标靶指针尖端到背包坐标系中心准确的外参TLM，即标靶指针到背包中心的坐标系的变换关系；
plidai＝TLMpmark
plidai表示激光坐标系下的点的坐标，pmark表示标靶坐标系下的点的坐标。
3.如权利要求1所述的一种高效地面绝对标靶提取和全局优化方法，其特征在于：非线性优化库包括ceres，gtsam，g2o。
4.如权利要求1所述的一种高效地面绝对标靶提取和全局优化方法，其特征在于：采用LM算法进行优化，其流程为：在每一次迭代中，求取一个优化变量的增量，使得总的残差项的值下降最快，之后不断迭代，直至总的残差项不再改变，优化迭代过程趋于收敛，最终求得优化变量的最优值。
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 CN113686321A (zh) | 2021-11-23 |
| CN113686321B true 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)
| 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 | 苏州挚途科技有限公司 | 相机标定方法、装置和电子设备 |
-
2021
- 2021-10-26 CN CN202111245537.0A patent/CN113686321B/zh active Active
Patent Citations (4)
| 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 |
|---|---|
| CN113686321A (zh) | 2021-11-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN110160542B (zh) | 车道线的定位方法和装置、存储介质、电子装置 | |
| CN106017463B (zh) | 一种基于定位传感装置的飞行器定位方法 | |
| CN102445200B (zh) | 微小型个人组合导航***及其导航定位方法 | |
| CN109341706A (zh) | 一种面向无人驾驶汽车的多特征融合地图的制作方法 | |
| CN103968827A (zh) | 一种可穿戴式人体步态检测的自主定位方法 | |
| CN105547288A (zh) | 一种煤矿井下移动设备自主定位的方法及*** | |
| CN104713554A (zh) | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 | |
| CN103196445B (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
| CN105241445A (zh) | 一种基于智能移动终端的室内导航数据获取方法及*** | |
| CN107490378B (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
| CN107014375B (zh) | 超低部署的室内定位***及方法 | |
| CN107830858B (zh) | 一种基于重力辅助的手机航向估计方法 | |
| CN104613965B (zh) | 一种基于双向滤波平滑技术的步进式行人导航方法 | |
| CN112797985B (zh) | 基于加权扩展卡尔曼滤波的室内定位方法及室内定位*** | |
| CN113175933B (zh) | 一种基于高精度惯性预积分的因子图组合导航方法 | |
| CN108007477A (zh) | 一种基于正反向滤波的惯性行人定位***误差抑制方法 | |
| CN108592907A (zh) | 一种基于双向滤波平滑技术的准实时步进式行人导航方法 | |
| CN115183762A (zh) | 一种机场仓库内外建图方法、***、电子设备及介质 | |
| CN110260860A (zh) | 基于足部惯性传感器的室内移动测量定位定姿方法及*** | |
| CN103256932B (zh) | 一种替换结合外推的着陆导航方法 | |
| CN113686321B (zh) | 一种高效地面绝对标靶提取和全局优化方法 | |
| CN103017773B (zh) | 一种基于天体表面特征和天然卫星路标的环绕段导航方法 | |
| Tang et al. | IC-GVINS: A robust, real-time, INS-centric GNSS-visual-inertial navigation system for wheeled robot | |
| CN115290090A (zh) | 基于多传感器信息融合的slam地图构建方法 |
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 |