机器人在使用激光雷达时必然会遇到的一个问题就是激光雷达数据去畸变。为什么会产生畸变呢,这是因为激光雷达产生激光数据时如果机器人在运动,那么就会产生运动畸变。
在ros下使用激光雷达时,激光数据是相对于激光本体坐标系的。画个图看下会比较直观一点。
如图所示,假设三角形表示激光雷达,在激光雷达上有2个激光发射孔,左右对称分布。这里设定激光雷达从
t
0
t_0
t0到
t
1
t_1
t1时间内激光雷达顺时针旋转角度为
θ
\theta
θ。在
t
0
t_0
t0时刻激光位置为
P
1
P_1
P1时发射了一束激光,在
t
1
t_1
t1时刻激光位置为
P
2
P_2
P2时又发射了一束激光。激光数据是在激光雷达自身坐标系下表示的,那么静止时刻下2束激光的坐标为[
r
c
o
s
θ
1
rcos\theta_1
rcosθ1,
r
s
i
n
θ
1
rsin\theta_1
rsinθ1],[
r
c
o
s
θ
2
rcos\theta_2
rcosθ2,
r
s
i
n
θ
2
rsin\theta_2
rsinθ2]。
在
t
0
t_0
t0时刻到
t
1
t_1
t1时刻激光数据在激光雷达坐标系下的坐标与静止条件下一致的,但是两束激光在惯性坐标系下的在
t
1
t_1
t1时刻坐标明显与惯性系下是不一致的。所以这里就存在一个问题,一帧激光雷达数据包含成百上千个激光束,每个激光束发射时激光雷达本身在惯性系下的坐标是实时变化的。在实际使用中我们要把所有的激光束转换到最后一个时刻时激光雷达在惯性系的位姿
T
l
a
s
t
T^{last}
Tlast下。
将
l
i
li
li时刻的激光束数据转换到最后一束激光位姿的公式表示如下:
T
l
a
s
t
W
−
1
T
l
i
W
p
l
i
=
T
l
i
l
a
s
t
p
l
i
=
p
l
a
s
t
{T_{last}^W}^{-1} T_{li}^{W} p^{li}= T_{li}^{last} p^{li} = p^{last}\\
TlastW−1TliWpli=Tlilastpli=plast