Camera-IMU联合标定原理
在VIO系统中，camera-imu间内外参精确与否对整个定位精度起着重要的作用。所以良好的标定结果是定位系统的前提工作。
目前标定算法主要分为离线和在线标定，离线标定以kalibr为代表，能够标定camera内参、camera-imu之间位移旋转、时间延时以及imu自身的刻度系数、非正交性等。标定过程中，kalibr 会使用 apriltag 作为视觉基准，利用非线性优化的方法优化视觉的重投影误差、角速度误差、加速度误差和 bias 误差，最后得到所需要的 IMU 和相机的外参数以及重力向量大小。除此之外，kalibr 还考虑了相机和 IMU不同步的情况，因此，还可以使用 kalibr 获得相机和 IMU 之间的时间延迟。
一.相机投影模型
这里以常用的针孔相机投影为例，而畸变模型包括最常见rantan、equidistant两种。对于空间中3D点怎样投影得到像素平面2D坐标：
由上图，可以看到空间三维点P由三角形相似，可以简单计算出来像素平面坐标(u,v)。实际投影成像过程不想上图那么简单，主要包含三步：
[
X
C
Y
C
Z
C
]
⟹
归一化平面
[
u
=
X
C
/
Z
C
v
=
Y
C
/
Z
C
]
⟹
加畸变
[
r
=
u
2
+
v
2
d
r
=
1
+
k
1
r
+
k
2
r
2
+
k
3
r
2
u
d
=
u
d
r
+
2
u
v
p
1
+
(
r
+
2
u
2
)
p
2
v
d
=
v
d
r
+
2
u
v
p
2
+
(
r
+
2
v
2
)
p
1
]
⟹
像素
[
p
x
=
f
x
u
d
+
c
x
p
y
=
f
y
v
d
+
c
y
]
\begin{bmatrix} X_{C}\\Y_{C}\\ Z_{C}\end{bmatrix}\overset{归一化平面}{\Longrightarrow } \begin{bmatrix} u=X_{C}/Z_{C}\\v=Y_{C}/Z_{C}\end{bmatrix}\overset{加畸变}{\Longrightarrow } \begin{bmatrix} r=u^{2}+v^{2}\\d_{r}=1+k_{1}r+k_{2}r^{2}+k_{3}r^{2}\\u_{d}=ud_{r}+2uvp_{1}+(r+2u^{2})p_{2}\\ v_{d}=vd_{r}+2uvp_{2}+(r+2v^{2})p_{1}\end{bmatrix}\overset{像素}{\Longrightarrow }\begin{bmatrix} p_{x}=f_{x}u_{d}+c_{x}\\p_{y}=f_{y}v_{d}+c_{y} \end{bmatrix}
XCYCZC⟹归一化平面[u=XC/ZCv=YC/ZC]⟹加畸变r=u2+v2dr=1+k1r+k2r2+k3r2ud=udr+2uvp1+(r+2u2)p2vd=vdr+2uvp2+(r+2v2)p1⟹像素[px=fxud+cxpy=fyvd+cy]
上图第二步以rantan畸变为例，对于equidistant畸变模型则是另外一种形式：
{
r
=
u
2
+
v
2
θ
d
=
θ
(
1
+
k
1
θ
2
+
k
2
θ
4
+
k
3
θ
6
+
k
4
θ
8
)
u
d
=
u
⋅
θ
d
r
,
v
d
=
v
⋅
θ
d
r
\left\{\begin{matrix} r=\sqrt{u^2+v^2} \\ \theta_{d}=\theta(1+k_{1}\theta^{2}+k_{2}\theta^{4}+k_{3}\theta^{6}+k_{4}\theta^{8}) \\ u_{d}=u\cdot \frac{\theta_{d}}{r},v_{d}=v\cdot \frac{\theta_{d}}{r} \end{matrix}\right.
⎩⎨⎧r=u2+v2θd=θ(1+k1θ2+k2θ4+k3θ6+k4θ8)ud=u⋅rθd,vd=v⋅rθd
投影过程首先是算出归一化平面上点，然后再对归一化平面上点加rantan、equidistant畸变，最后再作用于内参焦距、主点，得到成像像素点坐标。
二.IMU 模型
惯性测量单元(Inertial Measurement Unit，IMU)，主要用来测量机器人运动过程中的角速度以及加速度。由于相机在运动过快的过程中拍摄的图片会产生模糊，从而导致在两帧图片中存在的共同区域将变少，算法无法确定机器人的运动方向。因此，使用 IMU 可以持续提供可靠的(R,t)估算。对于视觉算法，IMU 起着互补的作用。
对于一个理想的 IMU，其加速度的三个轴与陀螺仪的三个轴定义了一个共享、正交的三维坐标系。加速度计用来感应物体在运动过程中沿着不同轴产生的加速度，陀螺仪用来测量物体围绕不同轴的角速度。但在实际情况下，由于组装的原因，加速度计和陀螺仪的三维坐标系是两个非正交，因此导致轴偏转角误差，如下图所示。对于两个坐标系之间的变换，可以通过下式获得：
非正交传感器坐标轴
(
x
S
、
y
S
、
z
S
)
(x^{S}、y^{S}、z^{S})
(xS、yS、zS)和 IMU 主体坐标轴
(
x
B
、
y
B
、
z
B
)
(x^{B}、y^{B}、z^{B})
(xB、yB、zB)
假设 IMU 主体坐标系与加速度计正交坐标系相重合，那么在这种情况下，角度
β
x
z
、
β
x
y
、
β
y
x
β_{xz}、β_{xy}、β_{yx}
βxz、βxy、βyx的值都为零，上式可以简化为:
式中，字母 a 表示加速度计情况，而
a
O
a^{O}
aO和
a
S
a^{S}
aS分别表示理想坐标系和实际坐标系。
陀螺仪和加速度计测量应该参考同样的坐标系，因此可以使用下式来表示如何从实际陀螺仪坐标系变换到理想陀螺仪坐标系：
加速度计和陀螺仪都会受数字信号转换为物理量带来误差的影响，因此引入加速度计与陀螺仪尺度因子矩阵：
加速度计和陀螺仪精确加工的条件下，当 IMU 静止时，加速度计和陀螺仪三轴均输出0。但是由于加工误差的存在，加速度计与陀螺仪产生零偏向量：
在相机和 IMU 联合使用的情况下，主要考虑数据怎么从一个坐标系转换到另一个坐标系. 相机坐标系{C}、IMU 坐标系{B}和世界坐标系{W}之间的关系如下图所示，各个坐标系之间可以相互变换：
不同坐标系之间相互转换的关系
用 T 来表示不同坐标系之间的变换，在相机和 IMU 使用前，需要对相机和 IMU 之间的变换矩阵
T
c
b
T_{cb}
Tcb进行标定。因此相机坐标系和 IMU 坐标系之间满足如下变换关系：
T
w
b
=
T
w
c
⋅
T
c
b
T_{wb}=T_{wc}\cdot T_{cb}
Twb=Twc⋅Tcb
将 T 表示成旋转矩阵 R 和平移向量 t：
将上式进行矩阵之间的运算，可以分别得到相机坐标系和 IMU 坐标系之间旋转和平移的变换关系：
由于相机和 IMU 是两个不同的传感器，两者的触发不能完全同步进行，因此在使用中相机和 IMU 之间触发存在时间差
t
d
t_{d}
td。如下图 所示，传感器采样实例和时间戳之间存在偏差。
传感器采样时间戳
IMU 的采样时间等于相机的采样时间加上时间差
t
d
t_{d}
td:
t
I
M
U
=
t
c
a
m
+
t
d
t_{IMU}=t_{cam}+t_{d}
tIMU=tcam+td
三.Camera-IMU标定模型
对于低纹理、运动模糊和遮挡的环境，视觉和惯性传感器的结合具有较强的鲁棒性。在纯视觉 SLAM 中通常缺少尺度信息，IMU 通过提供可靠的 R 和 t 以恢复绝对尺度。具有代表性的算法有 EKF-SLAM、MSCKF、OKVIS、VINS-Mono和ORB-SLAM3。以上经典的算法都依赖于系统准确的初始化和相机-IMU精准的标定。相机-IMU 的外参标定主要来标定从相机坐标系{C}到 IMU 坐标系{B}的变换矩阵(即旋转和平移)，我们首先求取从相机到 IMU 的旋转，然后再求取平移.
(一) 相机-IMU旋转
相机-IMU 之间的旋转对于视觉惯性 SLAM 系统的鲁棒性非常重要，过大的偏差会导致系统的初始化崩溃。因为单目相机可以跟踪系统的位姿，而两幅图像之间的相对旋转可以通过经典的五点算法来解决两帧之间图片的相对旋转
R
c
k
,
c
k
+
1
R_{c_{k},c_{k+1}}
Rck,ck+1。此外，角速度可以通过对陀螺仪积分来获得相关的旋转
R
b
k
,
b
k
+
1
R_{b_{k},b_{k+1}}
Rbk,bk+1。对于任意的 k 帧的图片，遵循以下等式:
R
b
k
,
b
k
+
1
⋅
R
b
c
=
R
b
c
⋅
R
c
k
,
c
k
+
1
R_{b_{k},b_{k+1}}\cdot R_{bc}=R_{bc}\cdot R_{c_{k},c_{k+1}}
Rbk,bk+1⋅Rbc=Rbc⋅Rck,ck+1
将上式中的旋转矩阵用四元数来表示:
对于给定的多对连续图像之间的旋转，我们可以构建一个超定方程:
式中 N 表示旋转矩阵收敛时所使用帧的数量，
w
k
,
k
+
1
w_{k,k+1}
wk,k+1是异常处理的权重。当旋转校准与传入的测量一起运行时，先前估计的结果
R
^
b
c
\hat{R}_{bc}
R^bc可以作为初始值对残差进行加权:
残差函数的权重为:
如果没有足够的特征来估计相机旋转，则将
w
k
,
k
+
1
w_{k,k+1}
wk,k+1设置为零。以上的超定方程的解可以找到对应于
Q
N
Q_{N}
QN的最小奇异值的右单位奇异向量。即可以解算出对应的旋转矩阵
R
b
c
R_{bc}
Rbc。
(二) 相机-IMU平移
相机-IMU 的旋转可以从上部分得到，获得旋转矩阵
R
b
c
R_{bc}
Rbc后，我们可以估计相机-IMU 的平移，使用紧耦合的滑动窗口来初始化这些参数。初始化在 IMU 坐标系下完成，我们定义的状态向量为:
其中
x
k
x_{k}
xk是第 k 个 IMU 的状态，
g
b
k
g^{bk}
gbk是重力向量，N 是 IMU 的状态在滑动窗口中 s 的数量，M是在滑动窗口中具有足够视差的特征的数量。n 和 m 是在滑动窗口中的起始索引。
λ
l
λ_{l}
λl 是第
l
l
l个特征从第一次观察开始的深度，
P
b
0
b
k
P_{b_{0}b_{k}}
Pb0bk=[0,0,0]事前设定好。
初始化通过最大似然估计来完成，即最小化滑动窗口内来自 IMU 和单目相机的所有测量误差的马氏范数之和:
其中 B 是所有 IMU 测量值的集合，C 是任何特征和任何相机姿态之间的所有观测值的集合。由于增量和相对旋转是已知的，上式可以使用非迭代线性方式求解。其中 IMU 的测量可以表示为:
{
r
p
,
H
p
r_{p},H_{p}
rp,Hp}是线性估计器的解，{
P
b
k
,
b
k
+
1
,
H
b
k
,
b
k
+
1
P_{b_{k},b_{k+1}},H_{b_{k},b_{k+1}}
Pbk,bk+1,Hbk,bk+1}是线性 IMU 测量模型，
H
c
j
l
H_{cjl}
Hcjl是线性相机测量模型。上式最优化方程描述的过于复杂，为了便于参数的求解，将线性代价函数转换为以下形式：
其中{$ \Lambda_{B},b_{B}KaTeX parse error: Expected 'EOF', got '}' at position 1: }̲和{\Lambda_{C},b_{C}$}分别是 IMU 和视觉测量的信息矩阵和向量。由于已知的增量和对应的旋转，代价函数的状态是线性的，并且上式具有唯一的解。通过对该方程线性的求解，就能获得相机-IMU 之间的平移向量
t
b
c
t_{bc}
tbc。
(三) 视觉惯性代价函数
单目视觉惯性问题可以被公式化为一个联合优化的代价函数
J
(
x
)
J(x)
J(x)，代价函数包括视觉测量的残差权重
e
v
e_{v}
ev和惯性测量的残差权重
e
i
e_{i}
ei:
其中 i 是图像的特征索引，k 代表相机数量索引，j 表示 3D 目标的位置，W 表示位置测量的信息矩阵。
四. camera-imu联合标定
imu可以获取每一时刻加速度和角速度，对加速度、角速度进行积分可以得到速度、位置、旋转。不同于SLAM中对离散imu数据进行积分得到状态可能带来较大的误差，采用对时间连续的状态求导来反推imu数据。把离散的状态描述成连续的就需要B-spline。
Kalibr 离线标定方法步骤：
- 粗略估计camera与imu之间时间延时。
- 获取imu-camera之间初始旋转，还有一些必要的初始值：重力加速度、陀螺仪偏置。
- 大优化，非线性优化代价函数，包括所有的角点重投影误差、imu加速度计与陀螺仪测量误差、偏置随机游走噪声。
(一) 粗略估计camera与imu之间时间延时
通过相机内参标定,可以先标定出camera的内参。现在已知每一帧图像的3D-2D对应，可以算出每一帧camera的pose。用这些离散的pose构造连续的B-spline，就可以获取任意时刻pose。
这里对pose参数化采用六维的列向量，分别三维的位移和旋转矢量 。对位移和旋转矢量分别求一阶导、二阶导可以得到速度与加速度：
v
=
t
˙
v = \dot{t}
v=t˙
a
=
t
˙
a = \dot{t}
a=t˙
w
i
=
J
r
(
θ
)
θ
˙
w_{i} = J_{r}(\theta)\dot{\theta}
wi=Jr(θ)θ˙
利用camera的样条曲线获取任意时刻camera旋转角速度，而陀螺仪又测量imu的角速度。忽略偏置和噪声影响，两者相差一个旋转，且模长相等：
w
i
=
R
i
c
w
c
w_{i}=R_{ic}w_{c}
wi=Ricwc
(二) 获取imu-camera之间初始旋转，还有一些必要的初始值：重力加速度、陀螺仪偏置
同样利用角速度测量关系，这次构造一个优化问题
这样就可以获得camera-imu之间的旋转，以及陀螺仪偏置初始值。
忽略加速度偏置与噪声，假设整个标定过程中平均加速度为零，所以也可以获得重力加速度在参考坐标系下的表示：
g
^
=
−
1
n
∑
i
=
1
n
R
w
c
∗
R
c
i
∗
a
m
\hat{g} =-\frac{1}{n} \sum_{i=1}^{n} R_{wc}*R_{ci}*a_{m}
g^=−n1∑i=1nRwc∗Rci∗am
(三) 大优化，包括所有的角点重投影误差、imu加速度计与陀螺仪测量误差、偏置随机游走噪声
前面两步为最后大优化提供一个不错的初始值，接着大优化就是调整所有要优化的变量来让所有的观测误差最小。
误差项包括所有标定板角点重投影误差、imu加速度计与陀螺仪测量误差、偏置的随机游走噪声(相对特殊点)。
为了简化imu测量误差的构建，这里利用camera pose乘上上面计算出来外参，得到imu的pose 曲线。当然这个曲线可能误差比较大，会在后续优化过程中进行调整。
参考:
版权归原作者 会上树的机器人 所有， 如有侵权，请联系我们删除。