CN114459500A - 激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质 - Google Patents
激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质 Download PDFInfo
- Publication number
- CN114459500A CN114459500A CN202111460529.8A CN202111460529A CN114459500A CN 114459500 A CN114459500 A CN 114459500A CN 202111460529 A CN202111460529 A CN 202111460529A CN 114459500 A CN114459500 A CN 114459500A
- Authority
- CN
- China
- Prior art keywords
- attitude sensor
- laser radar
- rotation matrix
- optimization
- rotation
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 239000011159 matrix material Substances 0.000 claims abstract description 44
- 238000005457 optimization Methods 0.000 claims abstract description 41
- 230000009466 transformation Effects 0.000 claims abstract description 6
- 238000010276 construction Methods 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000004590 computer program Methods 0.000 description 5
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- 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
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Optimization (AREA)
- Remote Sensing (AREA)
- Pure & Applied Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Databases & Information Systems (AREA)
- Manufacturing & Machinery (AREA)
- Algebra (AREA)
- Computing Systems (AREA)
- Computer Networks & Wireless Communication (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质，包括如下步骤：S1，激光雷达与姿态传感器在环境中自由运动，并分别采集点云、输出空间位姿；S2，基于激光雷达与姿态传感器的旋转位姿和旋转矩阵的关系，构建优化方程；S3，使用线性优化求得优化方程的最优解，得到激光雷达与姿态传感器之间的旋转矩阵。本发明利用激光雷达与姿态传感器的相对空间位姿，获得精确的激光雷达与姿态传感器间的实时变换关系，使得三维重建系统在对周围环境进行重建时能够更加准确。
Description
技术领域
本发明涉及三维重建系统图像传感器的标定技术领域，具体地说是一种激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质。
背景技术
激光雷达通过发射激光和接收激光的反射，来确定物体的位置。为了提高激光雷达的探测精度，在单线激光雷达的基础上发明了多线激光雷达。多线激光雷达可以同时发射和接收多束激光，在扫描时，可以生成多个不同角度的同心扫描线。因此，多线激光雷达的单帧点云数据可以包含相当丰富的周边环境信息。姿态传感器是一种高性能三维运动姿态测量系统，它包含三轴陀螺仪、三轴加速度计、三轴电子罗盘等运动传感器，利用基于四元数的三维算法和数据融合技术，可以实时输出以四元数、欧拉角表示的零偏移三维姿态方位数据。在基于激光雷达的三维重建的应用中，激光雷达与姿态传感器融合能够提供更准确的重建场景的信息。但是在现有的系统中，激光雷达与姿态传感器都有自身的局部坐标系，需要标定算法对激光雷达和姿态传感器进行标定，以确定局部坐标系之间的变换关系。目前，尚无相关技术方案可以在线标定激光雷达与姿态传感器之间的位姿关系。
发明内容
本发明为解决现有的问题，旨在提供一种激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质，用于实现激光雷达与姿态传感器间的标定。为了达到上述目的，本发明提供的方法，包括如下步骤：
S1，激光雷达与姿态传感器在环境中自由运动，并分别采集点云、输出空间位姿；
S2，基于激光雷达与姿态传感器的旋转位姿和旋转矩阵的关系，构建优化方程：
其中，
和
分别为姿态传感器与激光雷达的左乘旋转矩阵与右乘旋转矩阵，
是激光雷达相对于姿态传感器的旋转；
S3，使用线性优化求得优化方程的最优解，得到激光雷达与姿态传感器之间的旋转矩阵。
在一些实施例中，S1中，自由运动至少包含旋转。
在一些实施例中，S1中，激光雷达与姿态传感器刚体连接。
在一些实施例中，S1中，通过测量获得或者直接给定单位矩阵获得激光雷达与姿态传感器初始的变换矩阵。
在一些实施例中，S2中，构建优化方程的过程为：
S21，基于旋转矩阵的性质，可得：
其中，设
是姿态传感器从bk时刻到bk+1时刻过程中输出的结果，
是激光雷达在bk+1时刻帧相对于bk时刻帧的位姿变化，
是激光雷达相对于姿态传感器的旋转
S22,由于
可以得到：
将
移到方程的左边，可得：
S23，将四元数之间的乘法转换为旋转矩阵与四元数之间的乘法，可得：
其中，
和
分别为姿态传感器与激光雷达的四元数得到的左乘旋转矩阵与右乘旋转矩阵；再合并同类项，可得：
S24，设有n组数据参与了优化，则优化方程为：
在一些实施例中，S3中，使用列文伯格-马夸尔特法对优化方程进行线性优化。本发明还提供一种标定装置，包括：
采集模块,获取激光雷达与姿态传感器在环境中自由运动所采集点云、输出空间位姿；
构建模块，基于激光雷达与姿态传感器的旋转位姿和旋转矩阵的关系，构建优化方程；
优化模块，使用线性优化求得优化方程的最优解，得到激光雷达与姿态传感器之间的旋转矩阵。
本发明还提供一种标定设备，包括一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现任一所述的激光雷达与姿态传感器的相对位姿动态标定方法。
本发明还提供一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行任一所述的激光雷达与姿态传感器的相对位姿动态标定方法。
和现有技术相比，本发明利用激光雷达与姿态传感器的相对空间位姿，优化激光雷达与姿态传感器间的旋转矩阵，提出了一种基于线性优化的在线标定激光雷达与姿态传感器间旋转矩阵的算法，从而获得精确的激光雷达与姿态传感器间的实时变换关系，使得三维重建系统在对周围环境进行重建时能够更加准确。本发明利用了三维重建的环境信息，不需要专用道具和特定场景就可以实现精确标定。
具体实施方式
下面结合实施例对本发明的具体实施方法作进一步描述。该实施例用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
本发明的实施例包括下述步骤：
S1，数据采集，即激光雷达与姿态传感器在环境中自由运动，并分别采集点云、输出空间位姿。
本实施例采用了一个包含激光雷达与姿态传感器的三维重建系统，且激光雷达与姿态传感器通过刚体连接在一起。激光雷达与姿态传感器的初始变换矩阵可以通过测量获得或者直接给定单位矩阵。在数据采集过程中，三维重建系统在环境中自由运动。
作为优选，自由运动应当包含各种旋转以增加结果的精确度。
激光雷达与姿态传感器经过时间同步，在三维重建系统中激光雷达以10帧每秒的速率采集点云，姿态传感器以200帧每秒的速率输出空间位姿。
S2，构建优化方程，即根据激光雷达与姿态传感器得到的旋转矩阵，构建优化方程。
设
是姿态传感器从bk时刻到bk+1时刻过程中输出的结果，
是激光雷达在bk+1时刻帧相对于bk时刻帧的位姿变化，
是激光雷达相对于姿态传感器的旋转。根据旋转矩阵的性质，可得：
由于
可以得到：
将
移到方程的左边，可得：
上述公式描述了激光雷达与姿态传感器获得旋转位姿和它们之间旋转矩阵的关系。再将四元数之间的乘法转换为旋转矩阵与四元数之间的乘法，可得：
其中，
和
分别为姿态传感器与激光雷达的四元数得到的左乘旋转矩阵与右乘旋转矩阵。之后，合并同类项，可得：
设有n组数据参与了优化，则优化方程为：
至此便完成了优化方程的构建。进一步地，观测的数据越多，得到的旋转矩阵越精确。
S3，对方程进行线性优化，使用线性优化求得优化方程的最优解，得到激光雷达与姿态传感器之间的旋转矩阵。
利用上述优化方程和已知的数据，可以使用优化器对方程进行优化。列文伯格-马夸尔特算法，全称为Levenberg-Marquardt算法，是一种利用梯度求最大(小)值的线性优化算法。优化的起点无论是给定的旋转矩阵，还是单位矩阵，该方程都会在几次迭代之后收敛。方程收敛之后，就得到了激光雷达与姿态传感器之间精确的旋转矩阵。
本发明实施例还提供一种标定装置，包括：
采集模块,获取激光雷达与姿态传感器在环境中自由运动所采集点云、输出空间位姿；
构建模块，基于激光雷达与姿态传感器的旋转位姿和旋转矩阵的关系，并构建上述优化方程；
优化模块，使用线性优化求得优化方程的最优解，得到激光雷达与姿态传感器之间的旋转矩阵。
本发明实施例一种标定设备，包括一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现上述的激光雷达与姿态传感器的相对位姿动态标定方法。
在一些实施方式中，本实施例中涉及的存储装置存储了如下的元素，升级包、可执行单元或者数据结构，或者他们的子集，或者他们的扩展集：操作系统和应用程序。
其中，操作系统，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序，包含各种应用程序，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序中。
在本发明实施例中，处理器通过调用存储器存储的程序或指令，具体的，可以是应用程序中存储的程序或指令，处理器用于执行上述方法步骤。
本发明还提供一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现本发明上述的激光雷达与姿态传感器的相对位姿动态标定方法。
例如，机器可读存储介质可以包括但不限于各种已知和未知类型的非易失性存储器。
本发明实施例还提供一种计算机程序产品，包括计算机程序指令，该计算机程序指令使得计算机执行上述方法。
本领域技术人员可以明白的是，结合本文中所公开的实施例描述的各示例的单元及算法步骤能够以电子硬件、或者软件和电子硬件的结合来实现。这些功能是以硬件还是软件方式来实现，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以针对每个特定的应用，使用不同的方式来实现所描述的功能，但是这种实现并不应认为超出本申请的范围。
在本申请实施例中，所公开的系统、电子设备和方法可以通过其它方式来实现。例如，单元的划分仅仅为一种逻辑功能划分，在实际实现时还可以有另外的划分方式。例如，多个单元或组件可以进行组合或者可以集成到另一个系统中。另外，各个单元之间的耦合可以是直接耦合或间接耦合。另外，在本申请实施例中的各功能单元可以集成在一个处理单元中，也可以是单独的物理存在等等。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在机器可读存储介质中。因此，本申请的技术方案可以以软件产品的形式来体现，该软件产品可以存储在机器可读存储介质中，其可以包括若干指令用以使得电子设备执行本申请实施例所描述的技术方案的全部或部分过程。上述存储介质可以包括ROM、RAM、可移动盘、硬盘、磁盘或者光盘等各种可以存储程序代码的介质。
上述仅为本发明的优选实施方式，应指出的是，对于本行业内的普通技术技术人员而言，在本发明的原理之下可以由一些改进和替换，该改进和替换也应视为本发明的保护范围。
Claims (9)
1.一种激光雷达与姿态传感器的相对位姿动态标定方法，包括如下步骤：
S1，激光雷达与姿态传感器在环境中自由运动，并分别采集点云、输出空间位姿；
S2，基于激光雷达与姿态传感器的旋转位姿和旋转矩阵的关系，构建优化方程：
其中，
和
分别为姿态传感器与激光雷达的左乘旋转矩阵与右乘旋转矩阵，
是激光雷达相对于姿态传感器的旋转；
S3，使用线性优化求得优化方程的最优解，得到激光雷达与姿态传感器之间的旋转矩阵。
2.根据权利要求1所述的激光雷达与姿态传感器的相对位姿动态标定方法，其特征在于：S1中，自由运动至少包含旋转。
3.根据权利要求1所述的激光雷达与姿态传感器的相对位姿动态标定方法，其特征在于：S1中，激光雷达与姿态传感器刚体连接。
4.根据权利要求1所述的激光雷达与姿态传感器的相对位姿动态标定方法，其特征在于：S1中，通过测量获得或者直接给定单位矩阵获得激光雷达与姿态传感器初始的变换矩阵。
5.根据权利要求1所述的激光雷达与姿态传感器的相对位姿动态标定方法，其特征在于：S2中，构建优化方程的过程为：
S21，基于旋转矩阵的性质，可得：
其中，设
是姿态传感器从bk时刻到bk+1时刻过程中输出的结果，
是激光雷达在bk+1时刻帧相对于bk时刻帧的位姿变化，
是激光雷达相对于姿态传感器的旋转
S22,由于
可以得到：
将
移到方程的左边，可得：
S23，将四元数之间的乘法转换为旋转矩阵与四元数之间的乘法，可得：
其中，
和
分别为姿态传感器与激光雷达的四元数得到的左乘旋转矩阵与右乘旋转矩阵；再合并同类项，可得：
S24，设有n组数据参与了优化，则优化方程为：
6.根据权利要求1所述的激光雷达与姿态传感器的相对位姿动态标定方法，其特征在于：S3中，使用列文伯格-马夸尔特法对优化方程进行线性优化。
7.一种标定装置，其特征在于：包括：
采集模块,获取激光雷达与姿态传感器在环境中自由运动所采集点云、输出空间位姿；
构建模块，基于激光雷达与姿态传感器的旋转位姿和旋转矩阵的关系，构建优化方程；
优化模块，使用线性优化求得优化方程的最优解，得到激光雷达与姿态传感器之间的旋转矩阵。
8.一种标定设备，其特征在于：包括一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-6中任一所述的激光雷达与姿态传感器的相对位姿动态标定方法。
9.一种包含计算机可执行指令的存储介质，其特征在于，所述计算机可执行指令在由计算机处理器执行时用于执行如权利要求1-6中任一所述的激光雷达与姿态传感器的相对位姿动态标定方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111460529.8A CN114459500B (zh) | 2021-12-01 | 2021-12-01 | 激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111460529.8A CN114459500B (zh) | 2021-12-01 | 2021-12-01 | 激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114459500A true CN114459500A (zh) | 2022-05-10 |
| CN114459500B CN114459500B (zh) | 2024-05-24 |
Family
ID=81405858
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111460529.8A Active CN114459500B (zh) | 2021-12-01 | 2021-12-01 | 激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114459500B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111207774A (zh) * | 2020-01-17 | 2020-05-29 | 山东大学 | 一种用于激光-imu外参标定的方法及系统 |
| CN111208492A (zh) * | 2018-11-21 | 2020-05-29 | 长沙智能驾驶研究院有限公司 | 车载激光雷达外参标定方法及装置、计算机设备及存储介质 |
| WO2020155616A1 (zh) * | 2019-01-29 | 2020-08-06 | 浙江省北大信息技术高等研究院 | 一种基于数字视网膜的拍摄装置的定位方法 |
| CN111505606A (zh) * | 2020-04-14 | 2020-08-07 | 武汉大学 | 多相机与激光雷达系统相对位姿检校方法及装置 |
| CN112017248A (zh) * | 2020-08-13 | 2020-12-01 | 河海大学常州校区 | 一种基于点线特征的2d激光雷达相机多帧单步标定方法 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
| CN113091771A (zh) * | 2021-04-13 | 2021-07-09 | 清华大学 | 一种激光雷达-相机-惯导联合标定方法及系统 |
-
2021
- 2021-12-01 CN CN202111460529.8A patent/CN114459500B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111208492A (zh) * | 2018-11-21 | 2020-05-29 | 长沙智能驾驶研究院有限公司 | 车载激光雷达外参标定方法及装置、计算机设备及存储介质 |
| WO2020155616A1 (zh) * | 2019-01-29 | 2020-08-06 | 浙江省北大信息技术高等研究院 | 一种基于数字视网膜的拍摄装置的定位方法 |
| CN111207774A (zh) * | 2020-01-17 | 2020-05-29 | 山东大学 | 一种用于激光-imu外参标定的方法及系统 |
| CN111505606A (zh) * | 2020-04-14 | 2020-08-07 | 武汉大学 | 多相机与激光雷达系统相对位姿检校方法及装置 |
| CN112017248A (zh) * | 2020-08-13 | 2020-12-01 | 河海大学常州校区 | 一种基于点线特征的2d激光雷达相机多帧单步标定方法 |
| CN113066105A (zh) * | 2021-04-02 | 2021-07-02 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
| CN113091771A (zh) * | 2021-04-13 | 2021-07-09 | 清华大学 | 一种激光雷达-相机-惯导联合标定方法及系统 |
Non-Patent Citations (1)
| Title |
|---|
| 陈健武;全思博;全燕鸣;郭清达;: "双二维激光雷达相对位姿的标定方法", 中国激光, no. 10, 6 July 2017 (2017-07-06) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114459500B (zh) | 2024-05-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113570721B (zh) | 三维空间模型的重建方法、装置和存储介质 | |
| US10984554B2 (en) | Monocular vision tracking method, apparatus and non-volatile computer-readable storage medium | |
| CN109521403B (zh) | 多线激光雷达的参数标定方法及装置、设备及可读介质 | |
| US10595784B2 (en) | Object pose measurement system based on MEMS IMU and method thereof | |
| CN106679651B (zh) | 声源定位方法、装置和电子设备 | |
| WO2019127445A1 (zh) | 三维建图方法、装置、系统、云端平台、电子设备和计算机程序产品 | |
| CN108629831B (zh) | 基于参数化人体模板和惯性测量的三维人体重建方法及系统 | |
| US20100204974A1 (en) | Lidar-Assisted Stero Imager | |
| CN111415387A (zh) | 相机位姿确定方法、装置、电子设备及存储介质 | |
| JP2016057108A (ja) | 演算装置、演算システム、演算方法およびプログラム | |
| CN108605098A (zh) | 用于卷帘快门校正的系统和方法 | |
| CN111380514A (zh) | 机器人位姿估计方法、装置、终端及计算机存储介质 | |
| CN108170297B (zh) | 实时六自由度vr/ar/mr设备定位方法 | |
| CN103759670A (zh) | 一种基于数字近景摄影的物体三维信息获取方法 | |
| WO2019191288A1 (en) | Direct sparse visual-inertial odometry using dynamic marginalization | |
| US20200029025A1 (en) | Imaging system and method of imaging control | |
| CN113587934A (zh) | 一种机器人、室内定位方法、装置和可读存储介质 | |
| CN114111776A (zh) | 定位方法及相关装置 | |
| El Habchi et al. | CGA: A new approach to estimate the geolocation of a ground target from drone aerial imagery | |
| WO2022267444A1 (zh) | 用于相机标定的方法及装置 | |
| CN114459500B (zh) | 激光雷达与姿态传感器的相对位姿动态标定方法、装置、设备和介质 | |
| JP2007226580A (ja) | 画像出力装置、及び画像出力方法 | |
| CN114049401A (zh) | 双目相机标定方法、装置、设备及介质 | |
| CN116952229A (zh) | 无人机定位方法、装置、系统和存储介质 | |
| CN117367458A (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |