CN114199281A - 基于速度优化的多传感器联合标定方法及*** - Google Patents
基于速度优化的多传感器联合标定方法及*** Download PDFInfo
- Publication number
- CN114199281A CN114199281A CN202111521844.7A CN202111521844A CN114199281A CN 114199281 A CN114199281 A CN 114199281A CN 202111521844 A CN202111521844 A CN 202111521844A CN 114199281 A CN114199281 A CN 114199281A
- Authority
- CN
- China
- Prior art keywords
- sensor
- sensors
- speed
- data set
- time
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000005457 optimization Methods 0.000 title claims abstract description 18
- 230000009466 transformation Effects 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims abstract description 14
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 7
- 238000004891 communication Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 3
- 239000000126 substance Substances 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Manipulator (AREA)
Abstract
本发明公开的一种基于速度优化的多传感器联合标定方法，包括如下步骤：S1、m个传感器采集移动机器人行驶过程中的数据及时间戳，计算出传感器在采集时刻的速度；S2、通过插值法获取m个传感器在相同时间序列下的速度值，放入数据集Sensor_list中；S3、将不同传感器在基坐标系下的速度值相等转化成最小二乘问题，非线性求解最小二乘即可获取各个传感器相对于基坐标系的变换矩阵。本发明对待标定的传感器类型、性能和数量无要求，可扩展性强；对传感器自身的累计误差不敏感，标定精度高。
Description
技术领域
本发明属于机器人技术领域，更具体地，本发明涉及一种基于速度优化的多传感器联合标定方法及***。
背景技术
随着科技的发展，移动机器人在仓储物流和智能巡检等领域有着越来越广泛的应用，而环境感知技术是移动机器人技术的重要组成部分，是移动机器人执行任务的基础。依赖单一的传感器去感知环境往往无法取得令人满意的结果，因此会在移动机器人的车身安装多个传感器，将各种传感器的信息进行互补和优化组合处理，最终产生对环境的最优观测结果，如IMU、GPS、摄像机和激光雷达等。综合应用多个传感器之前需要知道各个传感器的相对位姿变换，即传感器的标定，需要标定出各个传感器到基坐标系的变换矩阵。
现有的标定方法对不同传感器的定位结果进行计算，得到传感器之间的相对关系，如激光雷达和里程计或IMU之间的标定。但是上述标定方案的扩展性较差，无法适应传感器类型和数量的变动，且标定结果会受传感器自身测量的累计误差而影响。
发明内容
本发明提供一种基于速度优化的多传感器联合标定方法，旨在提供一种扩展性强、标定精度高的多传感器标定方法。
本发明是这样实现的，一种基于速度优化的多传感器联合标定方法，所述方法具体包括如下步骤：
S1、m个传感器采集移动机器人行驶过程中的数据及时间戳，计算出传感器在采集时刻的速度；
S2、通过插值法获取m个传感器在相同时间序列下的速度值，放入数据集Sensor_list中；
S3、将不同传感器在基坐标系下的速度值相等转化成最小二乘问题，非线性求解最小二乘即可获取各个传感器相对于基坐标系的变换矩阵。
进一步的，所述m个传感器在相同时间序列下的速度值获取方法具体如下：
S21、将每个传感器的时间戳放入时间戳容器Time_all，按顺序进行排列，并删除冗余时间戳；
S22、顺序选择数据集Sensor_list中的每一个传感器的数据集S，对数据集S进行插值，获取所有传感器在相同时间序列下的速度值，其中，传感器i的数据集Si的插值过程如下：
遍历时间戳容器Time_all中的所有时间戳t，检测数据集S中是否存在与时间戳t的时间戳，若检测结果为否，则基于插值法计算传感器i在时间戳t下的速度值vt，***数据集Si中。
进一步的，在数据集Si中找到与时间t相邻的两个时间戳ti、ti+1，其中ti＜t＜ti+1，传感器i在时间戳t下的速度值vt计算公式具体如下：
其中，
分别表示传感器i在时间戳ti、ti+1下的速度值。
进一步的，最小二乘问题的公式如下：
通过非线性求解该最小二乘问题，计算得到各个传感相对于基坐标系的精准变换矩阵T_list＝{T1,T2,…,Tm}。
本发明是这样实现的，一种基于速度优化的多传感器联合标定***，所述***包括：
设置同一移动机器人上的m个传感器，与m个传感器通讯连接的处理单元，处理单元基于上述基于速度优化的多传感器联合标定方法来标定各个传感相对于基坐标系的变换矩阵T_list＝{T1,T2,…,Tm}。
本发明对待标定的传感器类型、性能和数量无要求，可扩展性强；对传感器自身的累计误差不敏感，标定精度高。
附图说明
图1为本发明实施例提供的基于速度优化的多传感器联合标定方法流程图；
图2为本发明实施例提供的基于速度优化的多传感器联合标定***结构示意图。
具体实施方式
下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
本发明要解决的是移动机器人在进行定位导航等操作之前，需要计算不同传感器的坐标系与基坐标系之间的转换关系。本发明中的各传感器必须安装在同一移动机器人上，基坐标系为所有传感器所需要转换到的目标坐标系，基坐标系可以与某个传感器的坐标系重合，需要提前标定各传感器到基坐标系的变换的初值，初值的精度相对较低，该初值可以在图上或实物上测量或估计得到。假定m个传感器及其到基坐标系的初始变换矩阵的集合T_list＝{T1,T2,…,Tm}，基于速度优化的多传感器联合标定方法流程如图1所示，该方法具体包括如下步骤：
S1、m个传感器采集移动机器人行驶过程中的数据及时间戳，计算出传感器在采集时刻的速度，将时间戳和对应速度成对存储于数据集Sensor_list中；
IMU、里程计可以通过原始数据简单的积分计算获得传感器速度，视觉(相机)和激光传感器需要通过前端里程计算法计算里程计位姿，再通过连续时间的位姿计算传感器的速度，传感器速度v计算公式如下：
其中posei+1、posei代表连续时刻ti+1、ti下的位姿；
完成后得到所有传感器的数据集合Sensor_list＝{S1,S2,…,Sm}，其中Si代表传感器i的数据集合，表示为
表示传感器i在时刻tn的速度。
S2、通过插值法获取m个传感器在相同时间序列下的速度值，放入数据集Sensor_list中；
由于多传感器的数据采样频率可能不同，所以采集到的数据可能不是同一个时刻的，因此需要对采集到的数据进行插值计算，得到同一时刻下的数据，m个传感器在相同时间序列下的速度值获取方法具体如下：
S21、将每个传感器的时间戳放入时间戳容器Time_all，按顺序进行排列，并删除冗余时间戳；
S22、顺序选择数据集Sensor_list中的每一个传感器的数据集S，针对传感器i的数据集Si执行如下操作：
遍历时间戳容器Time_all中的所有时间戳t，检测数据集S中是否存在与时间戳t的时间戳，若检测结果为否，则基于插值法计算传感器i在时间戳t下的速度值vt，***数据集Si中；
在本发明实施例中，在数据集Si中找到与时间t相邻的两个时间戳ti、ti+1，时间戳满足ti＜t＜ti+1，传感器i在时间戳t下的速度值vt计算公式具体如下：
其中，
分别表示传感器i在时间戳ti、ti+1下的速度值。
在完成所有传感器数据集的插值后，得到所有传感器插值后的数据集
其中，
表示tk时刻传感器1、传感器2……、传感器m在自身坐标系下的速度值。
S3、基于不同传感器在基坐标系下的速度值相等原理构建最小二乘问题，非线性求解最小二乘即可获取各个传感器相对于基坐标系的变换矩阵。
S31、计算得到所有传感器插值后的数据集合，可构建如下的最小二乘问题：
Vt-i表示传感器i在t时刻在自身坐标下的速度值，Ti为传感器i相对于基坐标下的变换矩阵，Vt-j表示传感器j在t时刻在自身坐标下的速度值，Tj为传感器j相对于基坐标下的变换矩阵，
以其中一个传感器的坐标系作为基坐标系，即同一时刻各传感器的速度值转换到基坐标系后应相等，利用步骤S32求解该最小二乘问题，计算得到各个传感相对于基坐标系的精准变换矩阵T_list＝{T1,T2,…,Tm}。
S32、非线性最小二乘求解：
1)对于需要求解的非线性最小二乘问题：
对F(x)进行泰勒展开，取一阶线性项近似带入上式中：
其中，J(x)为F(x)的雅可比矩阵；
2)对上式求导，并令倒数为0：
J(x)TJ(x)Δx＝-J(x)TF(x)
令A＝J(x)TJ(x),B＝-J(x)TF(x)，则上式可简化为AΔx＝B，求解该线性方程组，即可获得位姿增量Δx；
3)令当前位姿x加上当位姿增量Δx，即可得到优化后的位姿，判断Δx的各个分量是否满足约束条件，若满足，则结束迭代，得到最终的优化位姿，若不满足，则将优化后的位姿带入步骤3)其中继续迭代求解。
图2为本发明实施例提供的基于速度优化的多传感器联合标定***的结构示意图，为了便于说明，仅示出于本发明实施例相关的部分，该***包括：
设置同一移动机器人上的m个传感器，与m个传感器通讯连接的处理单元，处理单元基于m个传感器采集的数据来标定各个传感相对于基坐标系的变换矩阵T_list＝{T1,T2,…,Tm}。
上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。
Claims (5)
1.一种基于速度优化的多传感器联合标定方法，其特征在于，所述方法具体包括如下步骤：
S1、m个传感器采集移动机器人行驶过程中的数据及时间戳，计算出传感器在采集时刻的速度；
S2、通过插值法获取m个传感器在相同时间序列下的速度值，放入数据集Sensor_list中；
S3、将不同传感器在基坐标系下的速度值相等转化成最小二乘问题，非线性求解最小二乘即可获取各个传感器相对于基坐标系的变换矩阵。
2.如权利要求1所述基于速度优化的多传感器联合标定方法，其特征在于，所述m个传感器在相同时间序列下的速度值获取方法具体如下：
S21、将每个传感器的时间戳放入时间戳容器Time_all，按顺序进行排列，并删除冗余时间戳；
S22、顺序选择数据集Sensor_list中的每一个传感器的数据集S，对数据集S进行插值，获取所有传感器在相同时间序列下的速度值，其中，传感器i的数据集Si的插值值过程如下：
遍历时间戳容器Time_all中的所有时间戳t，检测数据集S中是否存在与时间戳t的时间戳，若检测结果为否，则基于插值法计算传感器i在时间戳t下的速度值vt，***数据集Si中。
3.如权利要求1所述基于速度优化的多传感器联合标定方法，其特征在于，在数据集Si中找到与时间t相邻的两个时间戳ti、ti+1，其中ti＜t＜ti+1，传感器i在时间戳t下的速度值vt计算公式具体如下：
其中，vti、vti+1分别表示传感器i在时间戳ti、ti+1下的速度值。
4.如权利要求1所述基于速度优化的多传感器联合标定方法，其特征在于，最小二乘问题的公式如下：
通过非线性求解该最小二乘问题，计算得到各个传感相对于基坐标系的精准变换矩阵T_list＝{T1,T2,…,Tm}。
5.一种基于速度优化的多传感器联合标定***，其特征在于，所述***包括：
设置同一移动机器人上的m个传感器，与m个传感器通讯连接的处理单元，处理单元基于权利要求1至4任一权利要求所述基于速度优化的多传感器联合标定方法来标定各个传感相对于基坐标系的变换矩阵T_list＝{T1,T2,…,Tm}。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111521844.7A CN114199281A (zh) | 2021-12-13 | 2021-12-13 | 基于速度优化的多传感器联合标定方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111521844.7A CN114199281A (zh) | 2021-12-13 | 2021-12-13 | 基于速度优化的多传感器联合标定方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114199281A true CN114199281A (zh) | 2022-03-18 |
Family
ID=80653256
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111521844.7A Pending CN114199281A (zh) | 2021-12-13 | 2021-12-13 | 基于速度优化的多传感器联合标定方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114199281A (zh) |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106251305A (zh) * | 2016-07-29 | 2016-12-21 | 长春理工大学 | 一种基于惯性测量单元imu的实时电子稳像方法 |
| WO2017161608A1 (zh) * | 2016-03-21 | 2017-09-28 | 完美幻境(北京)科技有限公司 | 一种相机几何标定处理方法及装置 |
| CN107443377A (zh) * | 2017-08-10 | 2017-12-08 | 埃夫特智能装备股份有限公司 | 传感器‑机器人坐标系转换方法及机器人手眼标定方法 |
| WO2018006521A1 (zh) * | 2016-07-06 | 2018-01-11 | 深圳市元征科技股份有限公司 | 加速度传感器校准方法及装置 |
| CN108225327A (zh) * | 2017-12-31 | 2018-06-29 | 芜湖哈特机器人产业技术研究院有限公司 | 一种顶标地图的构建与定位方法 |
| CN109544638A (zh) * | 2018-10-29 | 2019-03-29 | 浙江工业大学 | 一种用于多传感器融合的异步在线标定方法 |
| CN109975792A (zh) * | 2019-04-24 | 2019-07-05 | 福州大学 | 基于多传感器融合矫正多线激光雷达点云运动畸变的方法 |
| CN112254729A (zh) * | 2020-10-09 | 2021-01-22 | 北京理工大学 | 一种基于多传感器融合的移动机器人定位方法 |
| CN112762937A (zh) * | 2020-12-24 | 2021-05-07 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 一种基于占据栅格的2d激光序列点云配准方法 |
| CN113091771A (zh) * | 2021-04-13 | 2021-07-09 | 清华大学 | 一种激光雷达-相机-惯导联合标定方法及*** |
| WO2021174507A1 (zh) * | 2020-03-05 | 2021-09-10 | 深圳市大疆创新科技有限公司 | 参数标定方法、装置、***和存储介质 |
| CN113483755A (zh) * | 2021-07-09 | 2021-10-08 | 北京易航远智科技有限公司 | 一种基于非全局一致地图的多传感器组合定位方法及*** |
-
2021
- 2021-12-13 CN CN202111521844.7A patent/CN114199281A/zh active Pending
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017161608A1 (zh) * | 2016-03-21 | 2017-09-28 | 完美幻境(北京)科技有限公司 | 一种相机几何标定处理方法及装置 |
| WO2018006521A1 (zh) * | 2016-07-06 | 2018-01-11 | 深圳市元征科技股份有限公司 | 加速度传感器校准方法及装置 |
| CN106251305A (zh) * | 2016-07-29 | 2016-12-21 | 长春理工大学 | 一种基于惯性测量单元imu的实时电子稳像方法 |
| CN107443377A (zh) * | 2017-08-10 | 2017-12-08 | 埃夫特智能装备股份有限公司 | 传感器‑机器人坐标系转换方法及机器人手眼标定方法 |
| CN108225327A (zh) * | 2017-12-31 | 2018-06-29 | 芜湖哈特机器人产业技术研究院有限公司 | 一种顶标地图的构建与定位方法 |
| CN109544638A (zh) * | 2018-10-29 | 2019-03-29 | 浙江工业大学 | 一种用于多传感器融合的异步在线标定方法 |
| CN109975792A (zh) * | 2019-04-24 | 2019-07-05 | 福州大学 | 基于多传感器融合矫正多线激光雷达点云运动畸变的方法 |
| WO2021174507A1 (zh) * | 2020-03-05 | 2021-09-10 | 深圳市大疆创新科技有限公司 | 参数标定方法、装置、***和存储介质 |
| CN112254729A (zh) * | 2020-10-09 | 2021-01-22 | 北京理工大学 | 一种基于多传感器融合的移动机器人定位方法 |
| CN112762937A (zh) * | 2020-12-24 | 2021-05-07 | 哈尔滨工业大学芜湖机器人产业技术研究院 | 一种基于占据栅格的2d激光序列点云配准方法 |
| CN113091771A (zh) * | 2021-04-13 | 2021-07-09 | 清华大学 | 一种激光雷达-相机-惯导联合标定方法及*** |
| CN113483755A (zh) * | 2021-07-09 | 2021-10-08 | 北京易航远智科技有限公司 | 一种基于非全局一致地图的多传感器组合定位方法及*** |
Non-Patent Citations (2)
| Title |
|---|
| 杨杰;史震;岳鹏;程子健;: "三轴加速度计GFSINS安装误差标定与补偿方法", ***工程与电子技术, no. 04, 15 April 2011 (2011-04-15) * |
| 林家春;朱敏杰;李睿;李梦玮;: "基于激光跟踪仪点位测量的工业机器人位置稳定时间测量与分析", 机电工程, no. 06, 20 June 2018 (2018-06-20) * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113311411B (zh) | 一种用于移动机器人的激光雷达点云运动畸变校正方法 | |
| CN111238535B (zh) | 一种基于因子图的imu误差在线标定方法 | |
| CN103017771B (zh) | 一种静止传感器平台的多目标联合分配与跟踪方法 | |
| CN110987068B (zh) | 一种多传感器综合控制***数据融合方法 | |
| CN110442831B (zh) | 基于非线性偏差演化的空间非合作目标天基搜索方法 | |
| CN113960622A (zh) | 融合激光雷达及imu传感器信息的实时定位方法及装置 | |
| CN110738275A (zh) | 基于ut-phd的多传感器序贯融合跟踪方法 | |
| CN115290071A (zh) | 相对定位融合方法、装置、设备及存储介质 | |
| CN113791240B (zh) | 基于高阶滑膜跟踪微分器的加速度估计方法、***、设备和介质 | |
| CN109916417B (zh) | 一种地图建立方法、装置、计算机设备及其存储介质 | |
| CA2675310C (en) | Method for evaluation of sensor measured values | |
| CN110677140A (zh) | 一种含未知输入和非高斯量测噪声的随机***滤波器 | |
| CN112965076B (zh) | 一种用于机器人的多雷达定位***及方法 | |
| CN114199281A (zh) | 基于速度优化的多传感器联合标定方法及*** | |
| CN117554976A (zh) | 激光雷达去除畸变方法 | |
| CN111121818B (zh) | 一种无人车中相机与二维码的标定方法 | |
| CN109655057B (zh) | 一种六推无人机加速器测量值的滤波优化方法及其*** | |
| CN111578939A (zh) | 考虑采样周期随机变化的机器人紧组合导航方法及*** | |
| CN116481543A (zh) | 一种移动机器人多传感器融合双层滤波定位方法 | |
| CN112873280B (zh) | 一种用于机器人的传感器的标定方法及设备 | |
| CN112987010B (zh) | 一种用于机器人的多雷达建图的***及方法 | |
| CN111664882B (zh) | 一种基于时间戳的编码器位置信号估计方法 | |
| CN103870714B (zh) | 一种基于高阶可观阵的导航***可观测分析方法 | |
| CN113608442A (zh) | 基于特征函数的非线性状态模型***的状态估计方法 | |
| CN112489075A (zh) | 基于特征函数的序惯式多传感器融合滤波方法 |
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 |