运动方程
做scanmatch前,需要预测lidar当前姿态,用线性插值方法估计。
问题:
已知n时刻姿态为
(
q
n
,
p
n
)
(q_n, p_n)
(qn,pn), 和n-1时刻与n时刻的相对运动关系
(
q
n
n
−
1
,
p
n
n
−
1
)
(q^{n-1}_{n}, p^{n-1}_n)
(qnn−1,pnn−1),预测n+1时刻姿态。
解答:
world <- n <- n+1
X w = q n w X n + p n w X w = q n w ( q n + 1 n X n + 1 + p n + 1 n ) + p n w X w = q n w q n + 1 n X n + 1 + q n w p n + 1 n + p n w X w ≈ q n w q n n − 1 X n + 1 + q n w p n n − 1 + p n w = > q n + 1 w = q n w q n n − 1 , p n + 1 w = q n w p n n − 1 + p n w X_w=q^w_nX_n + p^w_n \\ X_w = q^w_n(q^n_{n+1}X_{n+1} + p^n_{n+1}) + p^w_n \\ X_w = q^w_n q^n_{n+1}X_{n+1} + q^w_n p^n_{n+1} + p^w_n \\ X_w ≈ q^w_n q^{n-1}_{n}X_{n+1} + q^w_n p^{n-1}_{n} + p^w_n \\ => \\ q^w_{n+1}=q^w_n q^{n-1}_n , p^w_{n+1} = q^w_n p^{n-1}_n + p^w_n Xw=qnwXn+pnwXw=qnw(qn+1nXn+1+pn+1n)+pnwXw=qnwqn+1nXn+1+qnwpn+1n+pnwXw≈qnwqnn−1Xn+1+qnwpnn−1+pnw=>qn+1w=qnwqnn−1,pn+1w=qnwpnn−1+pnw
点云补偿
问题:
已知n时刻姿态为
(
q
n
,
p
n
)
(q_n, p_n)
(qn,pn), 和n-1时刻与n时刻的相对运动关系
(
q
n
n
−
1
,
p
n
n
−
1
)
(q^{n-1}_{n}, p^{n-1}_n)
(qnn−1,pnn−1),以及某点在
n
∗
n^*
n∗时刻
X
n
∗
X_{n^*}
Xn∗,其中
(
n
<
=
n
∗
<
n
+
1
)
(n<=n^*<n+1)
(n<=n∗<n+1) ,预测该点在n+1时刻,在姿态
(
q
n
+
1
,
p
n
+
1
)
(q_{n+1}, p_{n+1})
(qn+1,pn+1)下的坐标。
解答:
先线性插值得到
n
∗
n^*
n∗时刻姿态
q
n
∗
n
=
q
n
+
1
n
.
s
l
e
r
p
(
s
)
p
n
∗
n
=
s
p
n
+
1
n
=
>
q
n
∗
n
=
q
n
n
−
1
.
s
l
e
r
p
(
s
)
p
n
∗
n
=
s
p
n
n
−
1
q^n_{n^*} = q^{n}_{n+1}.slerp(s) \\ p^n_{n^*} = s p^n_{n+1} \\ => \\ q^n_{n^*} = q^{n-1}_{n}.slerp(s) \\ p^n_{n^*} = s p^{n-1}_{n} \\
qn∗n=qn+1n.slerp(s)pn∗n=spn+1n=>qn∗n=qnn−1.slerp(s)pn∗n=spnn−1
其中s为时间占比。
n
+
1
←
n
←
n
∗
n+1 \leftarrow n \leftarrow n ^*
n+1←n←n∗
X
n
=
q
n
+
1
n
X
n
+
1
+
p
n
+
1
n
=
>
X
n
+
1
=
q
n
+
1
n
−
1
(
X
n
−
p
n
+
1
n
)
X
n
+
1
=
q
n
+
1
n
−
1
(
q
n
∗
n
X
n
∗
+
p
n
∗
n
−
p
n
+
1
n
)
=
>
X
n
+
1
≈
q
n
n
−
1
−
1
(
q
n
∗
n
X
n
∗
+
p
n
∗
n
−
p
n
n
−
1
)
X_n = q^{n}_{n+1} X_{n+1} + p^n_{n+1} \\ => \\ X_{n+1} = {q^n_{n+1}}^{-1} (X_n - p^n_{n+1}) \\ X_{n+1} = {q^n_{n+1}}^{-1} (q^n_{n^*}X_{n^*} + p^n_{n^*} - p^n_{n+1} ) \\ => \\ X_{n+1} ≈ {q^{n-1}_{n}}^{-1} (q^n_{n^*}X_{n^*} + p^n_{n^*} - p^{n-1}_{n} )
Xn=qn+1nXn+1+pn+1n=>Xn+1=qn+1n−1(Xn−pn+1n)Xn+1=qn+1n−1(qn∗nXn∗+pn∗n−pn+1n)=>Xn+1≈qnn−1−1(qn∗nXn∗+pn∗n−pnn−1)