CN116543057B - 一种水下多相机与imu一体化标定方法 - Google Patents

一种水下多相机与imu一体化标定方法 Download PDF

Info

Publication number
CN116543057B
CN116543057B CN202310759915.XA CN202310759915A CN116543057B CN 116543057 B CN116543057 B CN 116543057B CN 202310759915 A CN202310759915 A CN 202310759915A CN 116543057 B CN116543057 B CN 116543057B
Authority
CN
China
Prior art keywords
imu
camera
image
checkerboard
matrix
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
Application number
CN202310759915.XA
Other languages
English (en)
Other versions
CN116543057A (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.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
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 South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202310759915.XA priority Critical patent/CN116543057B/zh
Publication of CN116543057A publication Critical patent/CN116543057A/zh
Application granted granted Critical
Publication of CN116543057B publication Critical patent/CN116543057B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30204—Marker
    • G06T2207/30208—Marker matrix

Abstract

本发明涉及传感器标定技术领域，具体提供了一种水下多相机与IMU一体化标定方法：分别标定各个IMU误差；将所有相机和IMU一起运动；分别录制各个相机图像数据和各个IMU数据；分别对各个相机进行内参粗标定；获取每帧粗定位图像相对于棋盘格的外参，组成粗定位图像的位姿序列；将粗定位图像的位姿序列输入到扩展卡尔曼滤波器系统中，来预测后续时刻图像的位姿向量；求取棋盘格角点像素坐标精确位置和图像位姿序列；标定相机与IMU外参关系，及标定各相机外参关系。该方法可避免棋盘格角点的漏检和误检，提高多相机与IMU标定效率，大幅节约时间成本，为复杂水下环境相机标定提供更稳定的方法。

Description

一种水下多相机与IMU一体化标定方法
技术领域
本发明涉及传感器标定技术领域，更具体地说，涉及一种水下多相机与IMU一体化标定方法。
背景技术
深潜器、水下探测器等高端海洋装备作为勘探海洋环境的关键装备之一，在其工作过程中，不可避免的需要进行周边环境信息的感知与交互，传统基于声学的声呐探测技术设备昂贵且不具备色彩信息，基于光学的视觉探测技术因其直观的传感器信息而被广泛关注。然而，在水下环境光照的非稳态、非平衡衰减干扰下，水下单一视觉定位稳定性差、匹配精度低、误匹配率高，难以满足水下快速定位、高精度三维重建的要求。
多传感器融合技术可将多种传感器优劣互补，具备高精度、高效率、强鲁棒性的优势，近年来在环境感知与三维重建领域得到了广泛应用，其中，惯性测量单元(InertialMeasurement Unit，IMU)可有效解决相机尺度估计问题，降低相机误匹配概率，提升定位精度。然而，应用多传感器融合技术不可避免地需要进行传感器自身内参与传感器间的外参标定。在水下相机-IMU标定过程中，现有研究将传感器内参与外参标定分割为两个步骤，传感器间的外参标定精度严重依赖于传感器内参标定精度，且在多相机-IMU时标定流程复杂，效率较低，在水下光照不足的条件下，现有相机内参标定方法极易在标定板角点检测过程产生漏检、误检现象，严重影响内参标定的稳定性与准确性。
发明内容
为克服现有技术中的缺点与不足，本发明的目的在于提供一种水下多相机与IMU一体化标定方法，可避免棋盘格角点的漏检和误检，提高多相机与IMU标定效率，大幅节约时间成本，为复杂水下环境相机标定提供更稳定的方法。
为了达到上述目的，本发明通过下述技术方案予以实现：一种水下多相机与IMU一体化标定方法，用于水下多相机与IMU一体化数据采集装置；所述水下多相机与IMU一体化数据采集装置包括多个相机、多个IMU和机械臂；多个相机和多个IMU分别固定于机械臂执行末端；
所述水下多相机与IMU一体化标定方法包括如下步骤：
S1、将各个IMU静置，分别标定各个IMU的确定性误差与随机误差；
S2、机械臂将所有相机和IMU一起在带有棋盘格标定板的水槽中运动；分别录制各个相机的图像数据，以及各个IMU的陀螺仪和加速度计数据；
S3、利用每组图像数据最初数帧有明显旋转或平移的图像作为粗定位图像，分别对各个相机进行内参粗标定，得到粗定位图像中棋盘格角点二维像素点坐标(u,v)和相机内参；相机内参包括内参矩阵K、畸变矩阵D和相对于棋盘格的外参；相对于棋盘格的外参包括旋转矩阵R和平移矩阵T；
S4、获取每帧粗定位图像相对于棋盘格的外参，组成粗定位图像的位姿序列[ρ 1 ,ρ 2 ,ρ 3 ,…,ρ N ]，N为粗定位图像帧数；将粗定位图像的位姿序列输入到扩展卡尔曼滤波器系统中，来预测后续时刻图像的位姿向量；估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；利用区域生长算法优化棋盘格角点像素坐标精确位置；根据棋盘格角点像素坐标精确位置优化相机的内参标定结果，并更新精细定位的图像位姿序列；
S5、基于IMU预积分方法获取IMU的位姿序列，基于S4得到的内参标定结果，标定相机与IMU外参关系，利用IMU为中间传感器标定各相机外参关系。
优选地，所述S3，相机内参中，内参矩阵K、畸变矩阵D、旋转矩阵R和平移矩阵T的形式分别为：
，
D=[k 1 k 2 k 3 p 1 p 2 ]，
，/>；
其中，f x 、f y 分别代表像素在X轴和Y轴方向上的焦距的长度；(u 0 ,v 0 )代表图像坐标系原点在像素坐标系中的坐标；k 1 、k 2 、k 3 分别代表径向畸变；p 1 、p 2 分别代表切向畸变；r 11 、r 12 、r 13 、r 21 、r 22 、r 23 、r 31 、r 32 、r 33 分别代表旋转矩阵R中的旋转量；t x 、t y 、t z 分别代表平移矩阵T中的X、Y、Z方向的平移量；
设定二维图像与三维空间的映射公式为：
；
其中，Z代表尺度因子；(U,V)代表三维坐标系下角点二维像素点坐标(u,v)对应的坐标值；Λ 11 、Λ 12 、Λ 13 、Λ 21 、Λ 22 、Λ 23 、Λ 31 、Λ 32 、Λ 33 分别代表相机内参矩阵K与旋转矩阵R的积；
利用SUSAN角点检测方法进行棋盘格角点检测，得到各个角点二维像素点坐标(u, v)；设定角点二维像素点坐标分解公式为：
；
；
设定：
，/>，/>，/>，/>，/>；
对于每帧图像，设定约束方程为：
，/>；
联立得到最终的约束方程：
；
根据二维图像与三维空间的映射公式、角点二维像素点坐标分解公式以及约束方程，通过最小二乘法拟合求得相机内参矩阵K、旋转矩阵R和平移矩阵T。
优选地，所述S4包括如下分步骤：
S41、根据相对于棋盘格的外参，获取每帧粗定位图像的位姿向量ρ i ：ρ i =[t x t y t z ø x ø y ø z ]；其中，t x 、t y 、t z 分别代表第i帧粗定位图像对应的平移矩阵T的平移量；ø x 、ø y 、ø z 分别代表由第i帧粗定位图像对应的旋转矩阵R转换得到的X、Y、Z方向旋转量；
所述旋转量ø x 、ø y 、ø z 与旋转矩阵R的转换关系如下：
R=R z ·R y ·R x ；
，/>，/>；
组成粗定位图像的位姿序列[ρ 1 ,ρ 2 ,ρ 3 ,…,ρ N ]；
S42、将粗定位图像的位姿序列输入到扩展卡尔曼滤波器中，来预测后续各个时刻图像对于棋盘格的位姿向量；
利用S3得到粗定位图像中棋盘格角点二维像素点坐标(u,v)和相机内参，估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；
利用相机内参粗定位的每帧图像中棋盘格角点的像素坐标、内参及外参，估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；利用区域生长算法优化棋盘格角点像素坐标精确位置；
S43、根据棋盘格角点像素坐标精确位置优化相机的内参标定结果，并更新精细定位的图像位姿序列；图像位姿序列表示为：[ρ N+1 ,ρ N+2 ,ρ N+3 ,…,ρ N+M ]；其中，M为相机精细定位的图像帧数。
优选地，所述S42中，扩展卡尔曼滤波器系统为：
；
其中，ρ t 代表时刻t位姿向量，t≥N；ϛ t 代表时刻t测量向量；f t ( )、h t ( )分别代表时刻t非线性状态函数和测量函数； 、/>分别代表均值为0、协方差分别为Q t ekf 、R t ekf 的不相关高斯白噪声；
设定扩展卡尔曼滤波器系统的状态估计值和协方差估计值为、P t|t ，则状态量预测值/>、/>为：
；
扩展卡尔曼滤波器系统的预测协方差矩阵P t+1|t ，测量误差协方差矩阵，状态与测量间的互协方差矩阵/>为：
；
其中，F t 和H t+1 为：
；
卡尔曼滤波增益为：
；
时刻t+1的状态估计值和状态估计误差协方差矩阵、P t+1|t+1 为：
；
其中，I代表单位矩阵。
优选地，所述S5包括如下步骤：
S51、利用IMU陀螺仪及加速计数据，积分得IMU的位姿序列；
S52、利用b样条曲线对高频率的IMU位姿序列进行时间插值，弥补相机与IMU之间的时间误差；利用PnP算法计算相机坐标系与IMU坐标系外参关系；利用非线性最小二乘算法对内参和外参进行联合优化；
S53、利用IMU作为中间变量，标定各个相机与各个IMU之间的外参关系。
优选地，所述S1中，IMU的确定性误差包括加速度计与陀螺仪固定偏差；所述加速度计固定偏差按照以下公式校正：
其中，、/>、/>分别代表X、Y、Z坐标轴校正后的加速度；/>、/>、/>分别代表X、Y、Z坐标轴初始加速度；/>、/>、/>分别代表X、Y、Z坐标轴尺度变换；/>、/>、/>、/>、/>、/>分别代表X、Y、Z坐标轴错位；/>、/>、/>代表分别X、Y、Z坐标轴加速度固定偏差；
所述陀螺仪固定偏差按照以下公式校正：
；
其中，、/>、/>分别代表X、Y、Z坐标轴校正后的角速度；/>、/>、/>分别代表X、Y、Z坐标轴初始角速度；/>、/>、/>分别代表X、Y、Z坐标轴尺度变换；/>、/>、/>、/>、/>、/>分别代表X、Y、Z坐标轴错位；/>、/>、/>代表分别X、Y、Z坐标轴角速度固定偏差；
所述IMU的随机误差包括高斯白噪声和误差随机游走；
所述高斯白噪声为：
；
其中，t为时间，n( )为高斯过程，E[ ]为均值，σ2为方差，δ( )为狄拉克函数；
所述误差随机游走b()为：
b(t)=n b (t)=σ b ω(t)；
其中，n b ( )为维纳过程，σ b 为随机游走标准差，ω( )是方差为1的白噪声。
优选地，所述S2是指：将各个相机和IMU刚性固定至机械臂执行末端，并进行防水处理；将棋盘格标定板置于水槽底部，机械臂将所有相机和IMU置于水面下方；机械臂绕水槽边缘运动并回到水槽中心位置；分别录制各个相机的图像数据，以及各个IMU的陀螺仪和加速度计数据。
优选地，所述水下多相机与IMU一体化数据采集装置，多个相机包括双目相机和单目相机；多个IMU包括外置IMU和内置于单目相机的内置IMU。
与现有技术相比，本发明具有如下优点与有益效果：
1、本发明方法能够进行水下一体化多相机与IMU标定，可提高多相机与IMU标定效率20%以上，大幅节约时间成本及人力成本；
2、本发明方法通过粗标定-角点预测-精细标定的方式，可避免棋盘格角点的漏检和误检，提高水下棋盘格角点的准确率27%以上并减少了计算时间32%以上，为复杂水下环境相机内参标定提供更稳定的方法；
3、本发明方法在优化过程中同时考虑了相机内参与相机与IMU间外参，相较于传统解耦优化的方式可提升内参及外参的标定精度15%以上，从而为水下视觉SLAM算法提供更精确的传感器信息。
附图说明
图1是本发明水下多相机与IMU一体化标定方法的流程图；
图2是本发明水下多相机与IMU一体化数据采集装置的结构示意图；
图3是本发明水下多相机与IMU一体化标定方法的坐标系转换示意图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细的描述。
实施例
本实施例一种水下多相机与IMU一体化标定方法，用于水下多相机与IMU一体化数据采集装置；水下多相机与IMU一体化数据采集装置包括多个相机、多个IMU和机械臂。本实施例中，以多个相机包括双目相机2和单目相机3、多个IMU包括外置IMU4和内置于单目相机的内置IMU为例进行说明，水下多相机与IMU一体化数据采集装置的结构如图2所示。多个相机和多个IMU分别固定于机械臂5执行末端；机械臂5将所有相机和IMU一起在带有棋盘格标定板6的水槽1中运动。
水下多相机与IMU一体化标定方法包括如下步骤：
S1、将各个IMU静置，分别标定各个IMU的确定性误差与随机误差。
S1中，IMU的确定性误差包括加速度计与陀螺仪固定偏差；所述加速度计固定偏差按照以下公式校正：
；
其中，、/>、/>分别代表X、Y、Z坐标轴校正后的加速度；/>、/>、/>分别代表X、Y、Z坐标轴初始加速度；/>、/>、/>分别代表X、Y、Z坐标轴尺度变换；/>、/>、/>、/>、/>、/>分别代表X、Y、Z坐标轴错位；/>、/>、/>代表分别X、Y、Z坐标轴加速度固定偏差；
所述陀螺仪固定偏差按照以下公式校正：
；
其中，、/>、/>分别代表X、Y、Z坐标轴校正后的角速度；/>、/>、/>分别代表X、Y、Z坐标轴初始角速度；/>、/>、/>分别代表X、Y、Z坐标轴尺度变换；/>、/>、/>、/>、/>、/>分别代表X、Y、Z坐标轴错位；/>、/>、/>代表分别X、Y、Z坐标轴角速度固定偏差；
所述IMU的随机误差包括高斯白噪声和误差随机游走；
所述高斯白噪声为：
；
其中，t为时间，n( )为高斯过程，E[ ]为均值，σ2为方差，δ( )为狄拉克函数；
所述误差随机游走b()为：
b(t)=n b (t)=σ b ω(t)；
其中，n b ( )为维纳过程，σ b 为随机游走标准差，ω( )是方差为1的白噪声。
S2、机械臂将所有相机和IMU一起在带有棋盘格标定板的水槽中运动；分别录制各个相机的图像数据，以及各个IMU的陀螺仪和加速度计数据。
S2是指：将各个相机和IMU刚性固定至机械臂执行末端，并进行防水处理；将棋盘格标定板置于水槽底部，机械臂将所有相机和IMU置于水面下方；机械臂绕水槽边缘运动并回到水槽中心位置；分别录制各个相机的图像数据，以及各个IMU的陀螺仪和加速度计数据。
S3、利用每组图像数据最初数帧（五帧以上）有明显旋转或平移的图像作为粗定位图像，分别对各个相机进行内参粗标定，得到粗定位图像中棋盘格角点二维像素点坐标(u, v)和相机内参；相机内参包括内参矩阵K、畸变矩阵D和相对于棋盘格的外参；相对于棋盘格的外参包括旋转矩阵R和平移矩阵T。
具体地说，相机内参中，内参矩阵K、畸变矩阵D、旋转矩阵R和平移矩阵T的形式分别为：
，
D=[k 1 k 2 k 3 p 1 p 2 ]，
，/>；
其中，f x 、f y 分别代表像素在X轴和Y轴方向上的焦距的长度；(u 0 ,v 0 )代表图像坐标系原点在像素坐标系中的坐标；k 1 、k 2 、k 3 分别代表径向畸变；p 1 、p 2 分别代表切向畸变；r 11 、r 12 、r 13 、r 21 、r 22 、r 23 、r 31 、r 32 、r 33 分别代表旋转矩阵R中的旋转量；t x 、t y 、t z 分别代表平移矩阵T中的X、Y、Z方向的平移量；
设定二维图像与三维空间的映射公式为：
；
其中，Z代表尺度因子；(U,V)代表三维坐标系下角点二维像素点坐标(u,v)对应的坐标值；Λ 11 、Λ 12 、Λ 13 、Λ 21 、Λ 22 、Λ 23 、Λ 31 、Λ 32 、Λ 33 分别代表相机内参矩阵K与旋转矩阵R的积；
利用SUSAN角点检测方法进行棋盘格角点检测，得到各个角点二维像素点坐标(u, v)；设定角点二维像素点坐标分解公式为：
；
；
设定：
，/>，/>，/>，/>，/>；
对于每帧图像，设定约束方程为：
，/>；
联立得到最终的约束方程：
；
根据二维图像与三维空间的映射公式、角点二维像素点坐标分解公式以及约束方程，通过最小二乘法拟合求得相机内参矩阵K、旋转矩阵R和平移矩阵T。
S4、获取每帧粗定位图像相对于棋盘格的外参，组成粗定位图像的位姿序列[ρ 1 ,ρ 2 ,ρ 3 ,…,ρ N ]，N为粗定位图像帧数；将粗定位图像的位姿序列输入到扩展卡尔曼滤波器系统中，来预测后续时刻图像的位姿向量；估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；利用区域生长算法优化棋盘格角点像素坐标精确位置；根据棋盘格角点像素坐标精确位置优化相机的内参标定结果，并更新精细定位的图像位姿序列。
S4包括如下分步骤：
S41、根据相对于棋盘格的外参，获取每帧粗定位图像的位姿向量ρ i ：ρ i =[t x t y t z ø x ø y ø z ]；其中，t x 、t y 、t z 分别代表第i帧粗定位图像对应的平移矩阵T的平移量；ø x 、ø y 、ø z 分别代表由第i帧粗定位图像对应的旋转矩阵R转换得到的X、Y、Z方向旋转量；
所述旋转量ø x 、ø y 、ø z 与旋转矩阵R的转换关系如下：
R=R z ·R y ·R x ；
，/>，/>；
组成粗定位图像的位姿序列[ρ 1 ,ρ 2 ,ρ 3 ,…,ρ N ]；
S42、将粗定位图像的位姿序列输入到扩展卡尔曼滤波器中，来预测后续各个时刻图像对于棋盘格的位姿向量；
扩展卡尔曼滤波器系统为：
；
其中，ρ t 代表时刻t位姿向量，t≥N；ϛ t 代表时刻t测量向量；f t ( )、h t ( )分别代表时刻t非线性状态函数和测量函数； 、/>分别代表均值为0、协方差分别为Q t ekf 、R t ekf 的不相关高斯白噪声；
设定扩展卡尔曼滤波器系统的状态估计值和协方差估计值为、P t|t ，则状态量预测值/>、/>为：
；
扩展卡尔曼滤波器系统的预测协方差矩阵P t+1|t ，测量误差协方差矩阵，状态与测量间的互协方差矩阵/>为：
；
其中，F t 和H t+1 为：
；
卡尔曼滤波增益为：
；
时刻t+1的状态估计值和状态估计误差协方差矩阵、P t+1|t+1 为：
；
其中，I代表单位矩阵。
利用S3得到粗定位图像中棋盘格角点二维像素点坐标(u,v)和相机内参，估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；
利用相机内参粗定位的每帧图像中棋盘格角点的像素坐标、内参及外参，估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；利用区域生长算法优化棋盘格角点像素坐标精确位置；
S43、根据棋盘格角点像素坐标精确位置优化相机的内参标定结果，并更新精细定位的图像位姿序列；图像位姿序列表示为：[ρ N+1 ,ρ N+2 ,ρ N+3 ,…,ρ N+M ]；其中，M为相机精细定位的图像帧数。
S5、基于IMU预积分方法获取IMU的位姿序列，基于S4得到的内参标定结果，标定相机与IMU外参关系，利用IMU为中间传感器标定各相机外参关系。
S5包括如下步骤：
S51、利用IMU陀螺仪及加速计数据，积分得IMU的位姿序列；
S52、利用b样条曲线对高频率的IMU位姿序列进行时间插值，弥补相机与IMU之间的时间误差；利用PnP算法计算相机坐标系与IMU坐标系外参关系；利用非线性最小二乘算法（Levenberg-Marquardt）对内参和外参进行联合优化；
S53、利用IMU作为中间变量，标定各个相机与各个IMU之间的外参关系。
具体地说，如图3所示，定义H cl ,H cr 分别代表双目相机左右相机坐标系，H i 代表外置IMU坐标系，H r , H ri 分别代表单目相机坐标系和内置IMU坐标系；
在所述S1中标定外置IMU确定性误差,/>和单目相机内置IMU确定性误差/>,/>；
在所述S4中标定双目相机左右相机内参K cl ,K cr ,D cl ,D cr 和单目相机内参K r ,D r ；
利用所述S52中的标定方法、所述S43中的图像位姿序列和S51中的IMU位姿序列，分别标定单目相机内置IMU与单目相机坐标系外参矩阵,/>，单目相机与外置IMU坐标系外参矩阵/>,/>，双目相机右相机与外置IMU坐标系外参矩阵/>,/>，外置IMU与双目相机左相机坐标系外参矩阵/>,/>，实现所有传感器坐标系间的外参标定。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (8)

1.一种水下多相机与IMU一体化标定方法，其特征在于：用于水下多相机与IMU一体化数据采集装置；所述水下多相机与IMU一体化数据采集装置包括多个相机、多个IMU和机械臂；多个相机和多个IMU分别固定于机械臂执行末端；
所述水下多相机与IMU一体化标定方法包括如下步骤：
S1、将各个IMU静置，分别标定各个IMU的确定性误差与随机误差；
S2、机械臂将所有相机和IMU一起在带有棋盘格标定板的水槽中运动；分别录制各个相机的图像数据，以及各个IMU的陀螺仪和加速度计数据；
S3、利用每组图像数据最初数帧有明显旋转或平移的图像作为粗定位图像，分别对各个相机进行内参粗标定，得到粗定位图像中棋盘格角点二维像素点坐标(u,v)和相机内参；相机内参包括内参矩阵K、畸变矩阵D和相对于棋盘格的外参；相对于棋盘格的外参包括旋转矩阵R和平移矩阵T；
S4、获取每帧粗定位图像相对于棋盘格的外参，组成粗定位图像的位姿序列[ρ 1 ,ρ 2 ,ρ 3 ,…,ρ N ]，N为粗定位图像帧数；将粗定位图像的位姿序列输入到扩展卡尔曼滤波器系统中，来预测后续时刻图像的位姿向量；估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；利用区域生长算法优化棋盘格角点像素坐标精确位置；根据棋盘格角点像素坐标精确位置优化相机的内参标定结果，并更新精细定位的图像位姿序列；
S5、基于IMU预积分方法获取IMU的位姿序列，基于S4得到的内参标定结果，标定相机与IMU外参关系，利用IMU为中间传感器标定各相机外参关系。
2.根据权利要求1所述的水下多相机与IMU一体化标定方法，其特征在于：所述S3，相机内参中，内参矩阵K、畸变矩阵D、旋转矩阵R和平移矩阵T的形式分别为：
，
D=[k 1 k 2 k 3 p 1 p 2 ]，
，/>；
其中，f x 、f y 分别代表像素在X轴和Y轴方向上的焦距的长度；(u 0 ,v 0 )代表图像坐标系原点在像素坐标系中的坐标；k 1 、k 2 、k 3 分别代表径向畸变；p 1 、p 2 分别代表切向畸变；r 11 、r 12 、r 13 、r 21 、r 22 、r 23 、r 31 、r 32 、r 33 分别代表旋转矩阵R中的旋转量；t x 、t y 、t z 分别代表平移矩阵T中的X、Y、Z方向的平移量；
设定二维图像与三维空间的映射公式为：
；
其中，Z代表尺度因子；(U,V)代表三维坐标系下角点二维像素点坐标(u,v)对应的坐标值；Λ 11 、Λ 12 、Λ 13 、Λ 21 、Λ 22 、Λ 23 、Λ 31 、Λ 32 、Λ 33 分别代表相机内参矩阵K与旋转矩阵R的积；
利用SUSAN角点检测方法进行棋盘格角点检测，得到各个角点二维像素点坐标(u,v)；设定角点二维像素点坐标分解公式为：
；
；
设定：
，/>，/>，/>，/>，/>；
对于每帧图像，设定约束方程为：
，/>；
联立得到最终的约束方程：
；
根据二维图像与三维空间的映射公式、角点二维像素点坐标分解公式以及约束方程，通过最小二乘法拟合求得相机内参矩阵K、旋转矩阵R和平移矩阵T。
3.根据权利要求1所述的水下多相机与IMU一体化标定方法，其特征在于：所述S4包括如下分步骤：
S41、根据相对于棋盘格的外参，获取每帧粗定位图像的位姿向量ρ i ：ρ i =[t x t y t z ø x ø y ø z ]；其中，t x 、t y 、t z 分别代表第i帧粗定位图像对应的平移矩阵T的平移量；ø x 、ø y 、ø z 分别代表由第i帧粗定位图像对应的旋转矩阵R转换得到的X、Y、Z方向旋转量；
所述旋转量ø x 、ø y 、ø z 与旋转矩阵R的转换关系如下：
R=R z ·R y ·R x ；
，/>，/>；
组成粗定位图像的位姿序列[ρ 1 ,ρ 2 ,ρ 3 ,…,ρ N ]；
S42、将粗定位图像的位姿序列输入到扩展卡尔曼滤波器中，来预测后续各个时刻图像对于棋盘格的位姿向量；
利用S3得到粗定位图像中棋盘格角点二维像素点坐标(u,v)和相机内参，估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；
利用相机内参粗定位的每帧图像中棋盘格角点的像素坐标、内参及外参，估计棋盘格角点在三维坐标系下的坐标；再利用预测得到的后续时刻图像的位姿向量，求取棋盘格角点在后续时刻图像中的像素坐标粗略位置；利用区域生长算法优化棋盘格角点像素坐标精确位置；
S43、根据棋盘格角点像素坐标精确位置优化相机的内参标定结果，并更新精细定位的图像位姿序列；图像位姿序列表示为：[ρ N+1 ,ρ N+2 ,ρ N+3 ,…,ρ N+M ]；其中，M为相机精细定位的图像帧数。
4.根据权利要求3所述的水下多相机与IMU一体化标定方法，其特征在于：所述S42中，扩展卡尔曼滤波器系统为：
；
其中，ρ t 代表时刻t位姿向量，t≥N；ϛ t 代表时刻t测量向量；f t ( )、h t ( )分别代表时刻t非线性状态函数和测量函数； 、/>分别代表均值为0、协方差分别为Q t ekf 、R t ekf 的不相关高斯白噪声；
设定扩展卡尔曼滤波器系统的状态估计值和协方差估计值为、P t|t ，则状态量预测值/>、/>为：
；
扩展卡尔曼滤波器系统的预测协方差矩阵P t+1|t ，测量误差协方差矩阵，状态与测量间的互协方差矩阵/>为：
；
其中，F t 和H t+1 为：
；
卡尔曼滤波增益为：
；
时刻t+1的状态估计值和状态估计误差协方差矩阵、P t+1|t+1 为：
；
其中，I代表单位矩阵。
5.根据权利要求1所述的水下多相机与IMU一体化标定方法，其特征在于：所述S5包括如下步骤：
S51、利用IMU陀螺仪及加速计数据，积分得IMU的位姿序列；
S52、利用b样条曲线对高频率的IMU位姿序列进行时间插值，弥补相机与IMU之间的时间误差；利用PnP算法计算相机坐标系与IMU坐标系外参关系；利用非线性最小二乘算法对内参和外参进行联合优化；
S53、利用IMU作为中间变量，标定各个相机与各个IMU之间的外参关系。
6.根据权利要求1所述的水下多相机与IMU一体化标定方法，其特征在于：所述S1中，IMU的确定性误差包括加速度计与陀螺仪固定偏差；所述加速度计固定偏差按照以下公式校正：
；
其中，、/>、/>分别代表X、Y、Z坐标轴校正后的加速度；/>、/>、/>分别代表X、Y、Z坐标轴初始加速度；/>、/>、/>分别代表X、Y、Z坐标轴尺度变换；/>、/>、/>、/>、/>、/>分别代表X、Y、Z坐标轴错位；/>、/>、/>代表分别X、Y、Z坐标轴加速度固定偏差；
所述陀螺仪固定偏差按照以下公式校正：
；
其中，、/>、/>分别代表X、Y、Z坐标轴校正后的角速度；/>、/>、/>分别代表X、Y、Z坐标轴初始角速度；/>、/>、/>分别代表X、Y、Z坐标轴尺度变换；/>、/>、/>、/>、/>、/>分别代表X、Y、Z坐标轴错位；/>、/>、/>代表分别X、Y、Z坐标轴角速度固定偏差；
所述IMU的随机误差包括高斯白噪声和误差随机游走；
所述高斯白噪声为：
；
其中，t为时间，n( )为高斯过程，E[ ]为均值，σ2为方差，δ( )为狄拉克函数；
所述误差随机游走b()为：
b(t)=n b (t)=σ b ω(t)；
其中，n b ( )为维纳过程，σ b 为随机游走标准差，ω( )是方差为1的白噪声。
7.根据权利要求1所述的水下多相机与IMU一体化标定方法，其特征在于：所述S2是指：将各个相机和IMU刚性固定至机械臂执行末端，并进行防水处理；将棋盘格标定板置于水槽底部，机械臂将所有相机和IMU置于水面下方；机械臂绕水槽边缘运动并回到水槽中心位置；分别录制各个相机的图像数据，以及各个IMU的陀螺仪和加速度计数据。
8.根据权利要求1所述的水下多相机与IMU一体化标定方法，其特征在于：所述水下多相机与IMU一体化数据采集装置，多个相机包括双目相机和单目相机；多个IMU包括外置IMU和内置于单目相机的内置IMU。
CN202310759915.XA 2023-06-27 2023-06-27 一种水下多相机与imu一体化标定方法 Active CN116543057B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310759915.XA CN116543057B (zh) 2023-06-27 2023-06-27 一种水下多相机与imu一体化标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310759915.XA CN116543057B (zh) 2023-06-27 2023-06-27 一种水下多相机与imu一体化标定方法

Publications (2)

Publication Number Publication Date
CN116543057A CN116543057A (zh) 2023-08-04
CN116543057B true CN116543057B (zh) 2023-10-10

Family

ID=87449061

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310759915.XA Active CN116543057B (zh) 2023-06-27 2023-06-27 一种水下多相机与imu一体化标定方法

Country Status (1)

Country Link
CN (1) CN116543057B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10410373B1 (en) * 2017-12-21 2019-09-10 Facebook Technologies, Llc Calibration of a phase interferometry depth camera assembly
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
WO2020237574A1 (zh) * 2019-05-30 2020-12-03 深圳市大疆创新科技有限公司 相机的内参及相对姿态的标定方法、装置、无人机和存储装置
CN114494456A (zh) * 2022-01-11 2022-05-13 上海交通大学 基于移动标定板多相机外参标定方法、系统、介质及终端
CN114998448A (zh) * 2022-05-19 2022-09-02 燕山大学 一种多约束双目鱼眼相机标定与空间点定位的方法
CN115457147A (zh) * 2022-09-16 2022-12-09 北京的卢深视科技有限公司 相机标定方法、电子设备及存储介质
CN115471534A (zh) * 2022-08-31 2022-12-13 华南理工大学 基于双目视觉和imu的水下场景三维重建方法及设备

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10410373B1 (en) * 2017-12-21 2019-09-10 Facebook Technologies, Llc Calibration of a phase interferometry depth camera assembly
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
WO2020237574A1 (zh) * 2019-05-30 2020-12-03 深圳市大疆创新科技有限公司 相机的内参及相对姿态的标定方法、装置、无人机和存储装置
CN114494456A (zh) * 2022-01-11 2022-05-13 上海交通大学 基于移动标定板多相机外参标定方法、系统、介质及终端
CN114998448A (zh) * 2022-05-19 2022-09-02 燕山大学 一种多约束双目鱼眼相机标定与空间点定位的方法
CN115471534A (zh) * 2022-08-31 2022-12-13 华南理工大学 基于双目视觉和imu的水下场景三维重建方法及设备
CN115457147A (zh) * 2022-09-16 2022-12-09 北京的卢深视科技有限公司 相机标定方法、电子设备及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Environment Driven Underwater Camera-IMU Calibration for Monocular Visual-Inertial SLAM;Changjun Gu et al.;《2019 International Conference on Robotics and Automation (ICRA)》;第2405-2411页 *
水下相机标定算法研究;高新浩 等;《燕山大学学报》;第38卷(第3期);第252-258页 *

Also Published As

Publication number Publication date
CN116543057A (zh) 2023-08-04

Similar Documents

Publication Publication Date Title
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN107590827A (zh) 一种基于Kinect的室内移动机器人视觉SLAM方法
CN105352509B (zh) 地理信息时空约束下的无人机运动目标跟踪与定位方法
CN111161337B (zh) 一种动态环境下的陪护机器人同步定位与构图方法
CN107358632B (zh) 应用于水下双目立体视觉的水下摄像机标定方法
CN110807809A (zh) 基于点线特征和深度滤波器的轻量级单目视觉定位方法
CN111890373A (zh) 车载机械臂的感知定位方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN114419109B (zh) 一种基于视觉和气压信息融合的飞行器定位方法
CN115639547A (zh) 多线激光雷达与gnss_ins联合标定方法、系统及介质
CN116543057B (zh) 一种水下多相机与imu一体化标定方法
CN116500595A (zh) 激光雷达、相机和惯性传感器相互约束的外参标定方法
CN109459046B (zh) 悬浮式水下自主航行器的定位和导航方法
CN115930948A (zh) 一种果园机器人融合定位方法
CN111145267A (zh) 基于imu辅助的360度全景视图多相机标定方法
CN116182855A (zh) 一种弱光强环境下仿复眼偏振视觉无人机组合导航方法
CN114485574B (zh) 基于卡尔曼滤波模型的三线阵影像pos辅助对地定位方法
CN112767482B (zh) 一种多传感器融合的室内外定位方法及系统
CN114690226A (zh) 基于载波相位差分技术辅助的单目视觉测距方法及系统
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
CN114842224A (zh) 一种基于地理底图的单目无人机绝对视觉匹配定位方案
CN114543786A (zh) 一种基于视觉惯性里程计的爬壁机器人定位方法
JP2021140262A (ja) カメラキャリブレーション装置、カメラキャリブレーション方法、およびプログラム
CN114018284B (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