Cubature卡尔曼滤波在刚性点集配准中的应用
5 浏览量
更新于2024-08-26
收藏 3.08MB PDF 举报
"基于Cubature卡尔曼滤波的刚性点集配准在智能车辆中的应用"
这篇研究论文深入探讨了刚性点集配准问题，这是智能车辆定位和建图中的关键问题。作者提出了一个利用Cubature卡尔曼滤波（CKF）算法的创新方法。点集注册是将两个点集对齐的过程，通常在没有预先知道对应关系的情况下进行。文章首先将点集注册问题转化为状态空间模型，然后通过非线性滤波器CKF来解决这个问题。
CKF是一种扩展卡尔曼滤波（EKF）的高精度替代方案，它通过高次积分（Cubature）近似来处理非线性问题，从而更准确地估计系统状态。在CKF的每一步迭代中，移动点集中的所有点都将被考虑，模型点集中的对应点会相应地更新。这种方法确保了所有点的信息都被充分利用，以提高配准的准确性。
在时间更新阶段，算法的关键在于能够探索的自由空间的规模。为了优化这一点，论文采用了连续模拟退火（CSA）技术。CSA是一种全局优化方法，它模仿了固体冷却过程中的原子重排，以逐步找到最佳解决方案。这种策略有助于跳出局部最优，从而在全球范围内寻找更好的点集配准。
在智能车辆领域，点集配准对于环境感知至关重要，特别是对于激光雷达数据的处理。通过精确的点集配准，车辆可以构建和更新其周围环境的三维地图，这对于自动驾驶系统的路径规划、障碍物检测和避障功能都具有重要意义。此外，这种方法还有助于车辆的自我定位，与全球定位系统（GPS）结合使用时，可以提供更可靠的定位信息，尤其是在GPS信号受到干扰或遮挡的环境中。
这篇论文为解决刚性点集配准问题提供了一个新的有效工具，并且特别关注了其在智能车辆中的应用。CKF和CSA的结合展示了在复杂环境中实现精确点集配准的潜力，为未来智能交通系统的开发和优化提供了理论支持。
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
LI et al.: RIGID POINT SET REGISTRATION BASED ON CKF AND ITS APPLICATION IN INTELLIGENT VEHICLES 3
set registration. In their work, the prediction (time update)
relied on a proposed density that depends on the alignment
error. In the measurement model, a robust version of ICP
will run for a preset number of iterations, and the weights
of every particle will be updated accordingly. As is well
known, particle-filtering-based methods can hardly meet the
real-time requirement when the number of particles is large,
which means that such methods are not suitable in SLAM
applications or for map matching. Authors in [21] proposed
a UKF-based algorithm for use in medical imaging. This
algorithm outperforms some widely used algorithms (such
as ICP) in terms of outliers or additive Gaussian noise.
However, it assumes that the corresponding points between
the two data sets are known and that the noise in SSM is
constant. Moreover, UKF can only approximate nonlinearity
in quadratic items, which is not sufficiently precise in some
cases.
The method proposed in this paper is based on the basic
filtering scheme. In contrast to the UKF-based registration
method, we treat the point set as a whole rather than adding
one point after one iteration, and the correspondences between
points are not assumed to be known. The algorithm in this
paper is CKF based, and it can approximate the nonlinearity
in SSM in third-order statistics, which is more precise than
the UKF-based method. In contrast to ICP and many other
distance-optimization-based methods, the correspondence here
does not rely on the nearest neighbor algorithm, which will
make the algorithm in this paper more robust to initialization
misalignment. Compared to probability-based methods, this
method considers not only noise in the point sets but also
noise in the prediction model. The exploration of the predictive
space is neither infinite nor stationary but will be optimized
step by step as the iterations proceed. This is achieved by
CSA, which will make the exploration more effective. The
filtering process does not rely on the Monte Carlo concept such
as that in particle-filtering-based methods. This will greatly
decrease the runtime complexity. The precision and robustness
are guaranteed by CKF and CSA.
Work in this paper extends work in a conference paper
version [22] by providing a more detailed analysis of the
relation to previous studies on point set registration, and a
more comprehensive set of numerical experiments. Moreover,
the corresponding point selection method in this paper is based
on the proposed local shape descriptor, whereas that in the
conference paper is based on the simple nearest neighbor
assumption.
III. P
ROBLEM FORMULATION
In this section, the point set registration problem will be
formulated as an SSM with a process model and obser-
vation model. This decomposes the point set registration
problem into a dual-phase problem. To make the state move
toward the minimum every step, the system should be
dynamic and explore the probabilistic domain to the greatest
extent possible. Inspired by [21], the SSM is constructed as
follows.
Since we focus on the point set registration problem in
SLAM, only rigid registration is discussed here. Given two
point sets U ={u
1
, u
2
,...,u
m
} and Y ={y
1
, y
2
,...,y
n
},
the goal is to find the best rotation matrix Q, translation
vector t and a scalar s that minimize the distance between
these two point sets: Y − (sQU +t). In the SLAM problem,
the scalar s is constrained to 1. Thus, the SSM is formulated
as equation (1) and equation (2).
x
k
= F(x
k−1
, v
k−1
) = x
k−1
+ v(k − 1) (1)
y
k
= H(x
k
, u
k
, e
k
) = R(x
θ
k
)u
k
+ x
t
k
+ e(k) (2)
Equation (1) is the process model, and x
k
is the state
at step k.Thestatex
k
=[θ
x
k
,θ
y
k
,θ
z
k
, t
x
k
, t
y
k
, t
z
k
]
T
=
[(x
θ
k
)
T
,(x
t
k
)
T
]
T
is a 6×1 vector with rotation parameter x
θ
k
=
[θ
x
k
,θ
y
k
,θ
z
k
]
T
and translation parameter x
t
k
=[t
x
k
, t
y
k
, t
z
k
]
T
.
v(k) is the white Gaussian noise at step k with v(k − 1) ∼
N (x
k−1
;0, Q
k−1
),whereQ
k−1
is its covariance matrix. Note
that the noise is important in this situation. The noise is what
makes the process model dynamic. With the noise, the state x
k
can move a random step from the previous state x
k−1
. Thus,
the process model can also be called a stochastic dynamic
process. Since there is no accurate model for transform pre-
diction, this dynamic process can explore the probability space
and solve the local minimum problem. As iterations proceed,
the uncertainty in the state should decrease. Thus, a simulated
annealing scheme is adopted here. As the iteration goes on,
CSA will make the noise covariance smaller, which means that
the exploring space will become smaller, then the uncertainty
of the transform becomes smaller. This scheme will improve
convergence, which will be discussed in detail in the next
section.
Equation (2) is the observation model, and y
k
is the obser-
vation at step k. At every step, a new measurement y
k
will
be measured according to the input u
k
and the state x
k
.The
corresponding point u
k
is treated as control input and will
remain the same as u
k
= U. e(k) is the white Gaussian noise
at step k with e(k) ∼ N (y
k
;0, R
k
),whereR
k
is its covariance
matrix. R(x
θ
k
) is the rotation matrix formulated by the rotation
parameter as shown in equations (3) - (6).
R(x
θ
k
) = R
(θ
x
)
× R
(θ
y
)
× R
(θ
z
)
(3)
R
(θ
x
)
=
⎡
⎣
10 0
0cos(θ
x
) sin(θ
x
)
0 −sin(θ
x
) cos(θ
x
)
⎤
⎦
(4)
R
(θ
y
)
=
⎡
⎣
cos(θ
y
) 0 −sin(θ
y
)
01 0
sin(θ
y
) 0cos(θ
y
)
⎤
⎦
(5)
R
(θ
z
)
=
⎡
⎣
cos(θ
z
) sin(θ
z
) 0
−sin(θ
z
) cos(θ
z
) 0
001
⎤
⎦
(6)
Note that the observation model is a highly nonlinear
process with white Gaussian noise. The original Kalman filter
cannot deal with nonlinearity. Two well-known extensions to
deal with nonlinearity are the extended Kalman filter (EKF)
and UKF. EKF approximates the nonlinearity using the
first-order Taylor series expansion evaluated at the mean
estimation of state x. It ignores high-order terms, which may
cause nonnegligible error in some cases and can diverge when
nonlinearities are large. UKF approximates the nonlinearity
剩余11页未读，继续阅读
2022-04-18 上传
2021-07-04 上传
2021-01-14 上传
2021-01-14 上传
点击了解资源详情
2021-01-14 上传
点击了解资源详情
2024-10-09 上传
2022-08-21 上传
weixin_38520437
- 粉丝: 5
- 资源: 920
我的内容管理
展开
最新资源
- Android圆角进度条控件的设计与应用
- mui框架实现带侧边栏的响应式布局
- Android仿知乎横线直线进度条实现教程
- SSM选课系统实现：Spring+SpringMVC+MyBatis源码剖析
- 使用JavaScript开发的流星待办事项应用
- Google Code Jam 2015竞赛回顾与Java编程实践
- Angular 2与NW.js集成：通过Webpack和Gulp构建环境详解
- OneDayTripPlanner：数字化城市旅游活动规划助手
- TinySTM 轻量级原子操作库的详细介绍与安装指南
- 模拟PHP序列化:JavaScript实现序列化与反序列化技术
- ***进销存系统全面功能介绍与开发指南
- 掌握Clojure命名空间的正确重新加载技巧
- 免费获取VMD模态分解Matlab源代码与案例数据
- BuglyEasyToUnity最新更新优化：简化Unity开发者接入流程
- Android学生俱乐部项目任务2解析与实践
- 掌握Elixir语言构建高效分布式网络爬虫
