2021
作者:华为诺亚方舟实验室
看华为的论文我似乎找到了他们写论文的固有格式:
传感器:单目相机(提取语义特征) + IMU + 两个编码器 + GNSS
IV.METHODOLOGY
A. Semantic Features and Detection
B. Semantic Data Association with HD map
step 1 在里程计先验位姿附近进行采样得到候选位姿(预测),并根据采样位姿将地图上的特征投影到图像上
step 2 执行局部一致性的粗关联,找到近似最优的采样姿态,同时消除由于大的先验姿态误差引起的不匹配。 局部结构一致性:感知特征的横向位置分布 + 对应特征重投影最小化。(a)感知与重投影特征按横向位置升序排列; (b)计算每个感知特征
s
t
s_t
st 和每个重投影特征
r
k
r_k
rk 之间的相似度:
p
(
s
t
∣
r
k
)
=
p
(
s
t
l
∣
r
k
l
)
p
(
s
t
c
∣
s
t
l
,
r
k
l
)
p
(
s
t
b
∣
r
k
b
)
(4)
p\left(s_{t} \mid r_{k}\right)=p\left(s_{t}^{l} \mid r_{k}^{l}\right) p\left(s_{t}^{c} \mid s_{t}^{l}, r_{k}^{l}\right) p\left(s_{t}^{b} \mid r_{k}^{b}\right) \tag{4}
p(st∣rk)=p(stl∣rkl)p(stc∣stl,rkl)p(stb∣rkb)(4)
where,
p
(
s
t
l
∣
r
k
l
)
p\left(s_{t}^{l} \mid r_{k}^{l}\right)
p(stl∣rkl) and
p
(
s
t
c
∣
s
t
l
,
r
k
l
)
p\left(s_{t}^{c} \mid s_{t}^{l}, r_{k}^{l}\right)
p(stc∣stl,rkl) can be obtained by offline learning of perception results.点云类别与点云得分相似度好计算
step 3 基于 step2 中近似最优匹配采样位姿,进行考虑匹配数、匹配相似度和局部结构相似度的最优关联方法,实现最优全局一致性匹配。 通过解决以下优化问题,将其表述为多阶图匹配问题:
X
^
=
arg
max
X
ω
1
N
m
+
ω
2
1
N
m
∑
i
=
1
N
∑
i
′
=
1
M
x
i
i
′
s
i
i
′
+
ω
3
1
N
e
∑
i
N
∑
i
′
M
∑
j
N
∑
j
′
M
x
i
i
′
x
j
j
′
s
i
j
,
i
′
j
′
(7)
ˆX=argmax \tag{7}
X^=Xargmaxω1Nm+ω2Nm1i=1∑Ni′=1∑Mxii′sii′+ω3Ne1i∑Ni′∑Mj∑Nj′∑Mxii′xjj′sij,i′j′(7)
S.t.
∑
i
=
1
N
x
i
i
′
≤
1
,
∑
i
′
=
1
M
x
i
i
′
≤
1
,
x
i
i
′
=
0
or
1
\sum_{i=1}^{N} x_{i i^{\prime}} \leq 1, \sum_{i^{\prime}=1}^{M} x_{i i^{\prime}} \leq 1, x_{i i^{\prime}}=0 \text { or } 1
i=1∑Nxii′≤1,i′=1∑Mxii′≤1,xii′=0 or 1
step 4 执行连续帧间的特征跟踪。该过程在连续帧中的特征之间建立关联。 由于感知特征是静态的并保持局部结构,我们将该过程表述为类似于等式(7)的多阶图匹配问题。
step 5 执行时间平滑以得到时间一致性的数据关联。该过程在连续帧中的感知特征和地图特征之间构建最佳一致匹配。 当前帧的匹配正确性可以通过滑动窗口中先前的匹配结果来验证。 此外,如果当前帧中出现不匹配,则可以根据之前的匹配和跟踪来找到并纠正它。 时间平滑通过对滑动窗口中每一帧的匹配 D 1 : T D_{1: T} D1:T 和匹配置信度 c t , i c_{t, i} ct,i 加权来获取地图特征 x l x^{l} xl 的对应感知特征 s i s_{i} si:
s ^ i = arg max s i p ( s i ∣ D 1 : T ) = arg max s i ∑ t I ( s i , D t ) c t , i ∑ t , j I ( s j , D t ) c t , j s^i=siargmaxp(si∣D1:T)=siargmax∑t,jI(sj,Dt)ct,j∑tI(si,Dt)ct,i
I ( s i , D t ) I\left(s_{i}, D_{t}\right) I(si,Dt) denotes if map feature x l x^{l} xl is matched with perceived feature s i s_{i} si.
The matching confidence
c
t
,
i
c_{t, i}
ct,i is given by evaluating feature and local structure similarity:
c
t
,
i
=
ω
exp
(
−
1
2
(
s
i
i
′
σ
p
)
2
)
+
(
1
−
ω
)
exp
(
−
1
2
(
1
N
m
−
1
∑
j
=
1
,
j
≠
i
N
m
s
i
j
,
i
′
j
′
σ
e
)
2
)
ct,i=ωexp(−21(σpsii′)2)+(1−ω)exp⎝⎛−21(σeNm−11∑j=1,j=iNmsij,i′j′)2⎠⎞
如果最佳感知特征的累积置信度远大于次优感知特征的置信度,则认为最佳感知特征为地图特征 x l x^{l} xl的匹配对。否则,认为地图特征 x l x^{l} xl具有不确定匹配, 并且可以给出与每个感知特征的匹配概率。 该过程区分确定匹配和不确定匹配,可以解决奇点引起的不匹配问题。
C. Pose Graph Optimization
等式(2)的位姿估计过程可以定义为先验概率和似然的乘积:
X ^ = arg max X p ( X ∣ Z , L , D ^ ) = arg max X p ( X ) p ( Z ∣ X , L , D ^ ) (11) \tag{11} X^=Xargmaxp(X∣Z,L,D^)=Xargmaxp(X)p(Z∣X,L,D^)(11)
通过高斯分布假设,先验分布 p ( X ) p(\mathcal{X}) p(X) 是通过里程计的相对运动估计得到的。 我们基于里程计测量 z i , i + 1 o z_{i, i+1}^{o} zi,i+1o 和匹配的特征对 z i l z_{i}^{l} zil 制定滑动窗口非线性最小二乘估计器,以估计最近的 T T T 个姿势。 与常用的滤波方法相比,优化方法可以处理异步和延迟测量,并在相同的计算资源下实现更高的精度 [ 40 ] [40] [40]。 优化目标表示为:
X ^ = arg min X ∑ i e o ( x i p , x i + 1 p , z i , i + 1 o ) T Ω i o e o ( x i p , x i + 1 p , z i , i + 1 o ) + ∑ i e l ( x i p , x l , z i l ) T Ω i l e l ( x i p , x l , z i l ) + ∑ i e j m ( x l ) T Ω j m e j m ( x l ) ) (12) \tag{12} X^=Xargmini∑eo(xip,xi+1p,zi,i+1o)TΩioeo(xip,xi+1p,zi,i+1o)+i∑el(xip,xl,zil)TΩilel(xip,xl,zil)+i∑ejm(xl)TΩjmejm(xl))(12)
其中,每个误差项连同对应的信息矩阵可以看作一个因子,每个状态变量可以看作一个节点,因此定位问题可以用因子图表示,如图4所示。误差项由下式组成 里程计误差 e o e^{o} eo、语义测量误差 e l e^{l} el 和地图误差 e j m e_{j}^{m} ejm。 里程计误差定义为:
e
o
(
x
i
p
,
x
i
+
1
p
,
z
i
,
i
+
1
o
)
=
(
x
i
+
1
p
T
x
i
p
)
z
i
,
i
+
1
o
e^{o}\left(x_{i}^{p}, x_{i+1}^{p}, z_{i, i+1}^{o}\right)=\left(x_{i+1}^{p}{ }^{\mathrm{T}} x_{i}^{p}\right) z_{i, i+1}^{o}
eo(xip,xi+1p,zi,i+1o)=(xi+1pTxip)zi,i+1o
Semantic measurement error factor
e
l
e^{l}
el is expressed as:
e
l
(
x
i
p
,
x
l
,
z
i
l
)
=
[
1
Z
i
c
K
T
v
c
x
i
p
T
x
l
]
0
−
[
z
i
l
]
0
e^{l}\left(x_{i}^{p}, x^{l}, z_{i}^{l}\right)=\left[\frac{1}{Z_{i}^{c}} K T_{v}^{c} x_{i}^{p \mathrm{~T}} x^{l}\right]_{0}-\left[z_{i}^{l}\right]_{0}
el(xip,xl,zil)=[Zic1KTvcxip Txl]0−[zil]0
其中, [ ⋅ ] 0 [\cdot]_{0} [⋅]0 表示向量的第一个元素。 测量误差仅采用横向误差,以消除高度误差的影响以及对地图特征准确绝对高度的要求,如图5所示。
地图误差因子 e j m e_{j}^{m} ejm 表示为:
e j m ( x l ) = x l − m j e_{j}^{m}\left(x^{l}\right)=x^{l}-m_{j} ejm(xl)=xl−mj
其中, m j m_{j} mj 是第 j j j 个地图特征的位置。 在本文中,我们采用参考文献[26]中提出的地图特征的方差构造方法。 在地图因子各向同性假设的情况下,根据假设的地图质量,地图因子的方差可以定义为:
σ m 2 = 1 γ ( c ) r 2 \sigma_{m}^{2}=\frac{1}{\gamma(c)} r^{2} σm2=γ(c)1r2
其中, γ \gamma γ 是反卡方累积分布函数, c c c 表示置信度, r r r 表示半径。
非线性优化问题可以直接通过迭代算法求解。 采用滑动窗口代替全批处理方法,在保证定位精度的同时提高计算效率。 旧状态被直接截断和忽略。 边缘化方法也可以处理旧状态,但它会累积线性化误差,使系统矩阵密集,并导致死锁。 边缘化方法基于过去的数据约束姿态,但使用地图特征作为先验足以约束车辆姿态。