同步定位与地图构建方法
【技术领域】
[0001] 本发明涉及一种移动机器人导航领域，特别是涉及一种同步定位与地图构建方 法。
【背景技术】
[0002] 自主移动机器人的同步定位与地图构建（simultaneouslocalizationand mapping，SLAM)问题可描述为：在未知环境中，移动机器人通过机载传感器（如里程计、视 觉传感器、超声波及激光等）来感知环境信息，逐步构建周围环境地图，同时运用此地图对 其位置和姿态进行估计。该问题一直是移动机器人研究领域的热点和难点，被认为是能否 真正实现机器人自主导航的关键问题，具有广阔的应用前景。
[0003] 现有解决SLAM问题采用的方法有粒子滤波器和扩展卡尔曼滤波器等方法，但不 能很好地表示提议分布，随着重采样的反复运用，使得粒子退化严重，降低粒子多样性，所 得到的2-D栅格地图的精度不高，同时存在复杂度高、执行效率低等问题。
【发明内容】
[0004] 本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种同步定 位与地图构建方法。
[0005] 为了实现本发明的上述目的，本发明提供了一种同步定位与地图构建方法，包括 以下步骤：
[0006]S1，初始化系统状态：
[0007] 当t=0时，根据机器人运动模型先验概率p (X。）选取N个粒子，记为,i= 1，2,…，N，所述N为正整数，每个粒子对应的权重为4,
[0008]S2,计算优化的混合提议分布；
[0009] S3,在该提议分布中采样粒子；
[0010] S4,计算并更新权重；
[0011]S5,计算有效粒子数Nrff，当有效粒子数Nrff小于预先设定的阈值Nth时，进行重采 样；否则直接执行步骤S6;
[0012] S6,更新地图，返回步骤S2。
[0013] 以退火参数调控运动模型和观测模型在混合提议分布中的比重，使得改进后的提 议分布更加接近真实状态；融入当前最新激光传感器观测信息，使得重要性权重的方差较 小，减少所需粒子数，并保持粒子多样性，在不同环境下能在线创建高精度的2-D栅格地 图。
[0014] 在本发明的一种优选实施方式中，混合提议分布的计算方法为：
[0015] 利用退火参数α来优化控制里程计运动模型J和观测模型 扒之间的比例，此时，混合提议分布的表示形式为：
[0016]
[0017] 其中α为退火参数，取值范围[0, 1] 表示第i个粒子在t时刻的 状态由该粒子在t-Ι时刻的状态和t-Ι时刻的里程计的控制输入u决定；jm丨表 示在t时刻的观测信息由第i个粒子在t-Ι时刻的携带的地图信息和该粒子在t时刻的状 态决定。
[0018] 融入当前最新激光测距仪的观测信息，以退火参数调控结合里程计运动模型和观 测模型在混合提议分布中的比重，使提议分布更加接近真实分布，有效地降低粒子数量，并 保持粒子多样性，大大减小计算复杂度。
[0019] 在本发明的一种更加优选实施方式中，当提议分布由里程计运动模型起主导作用 时，设置退火参数α为0.6 ;当融入激光测距仪的观测模型更接近真实状态分布时，则增加 观测模型的比率，设置α为0.02。
[0020] 通过退火参数优化控制两者在提议分布中的比重，当激光传感器的观测信息的精 度明显高于里程计运动模型时，使得尽可能多的粒子分布在观测信息高似然的可行区域， 使粒子权重不会出现明显差别，保持粒子多样性。
[0021] 在本发明的一种优选实施方式中，权重的计算方法为：
[0022]
[0023] 其中ρ(Λ·;(?) | )表示第i个粒子在t时刻的状态由该粒子在t_l时刻的状态 和t-Ι时刻的里程计的控制输入U决定；1?? 表示在t时刻的观测信息由第i个 粒子在t-ι时刻的携带的地图信息和该粒子在t时刻的状态决定；ufil表示第i个粒子在t-i时刻的权重。
[0024] 在本发明的一种优选实施方式中，有效粒子数Nrff的计算方法为：
[0025]
[0026] 其中N为粒子数目个数，为归一化重要性权重，_为第i 个粒子在t时刻的权重。
[0027] 有效粒子数Nrff保证了足够数量的粒子数来近似目标分布，呈现真实的后验概率， 防止粒子退化，保持粒子多样性。
[0028] 在本发明的一种优选实施方式中，重采样的计算方法包括以下步骤：
[0029] a.设置高阈值权重％ =-^χΗ和低阈值权重％ 所述η为大于l的正数， Ν % 根据粒子权重与门限值的大小关系将粒子分成两部分：
[0030] I部分为高权重粒子域和低权重粒子域：
[0031]
[0032]II部分为中等权重粒子域：
[0033] ' 1=1
5
[0034] 其中PV=NH+队表示高权重粒子域与低权重粒子域的粒子数量之和，N-PV表示中 等权重粒子域的粒子数量，xP表示第i个粒子在t时刻的状态，wf表示第i个粒子在t时 刻的权重；
[0035]b.决定第i个粒子被复制的次数n1，对于II部分中的粒子，直接复制；对I部分 的粒子，根据权重由高到低给粒子排序，依次设定粒子的序号为1、2、3、……PV，设置每个 粒子被选择的概率：
[0036]
[0037] 其中rank(i)为某一粒子排序后的序号，arank和βrank为系数，当粒子数确定时， arank= 2-βrank，1 彡βrank< 2,取值为：
[0038] ^
厶：，：
[0039] 如果第i个粒子的概率小于其被选择的概率，即p(ir= (1/NJ<p(i)，则 该粒子被舍弃；如果第i个粒子的概率大于或者等于其被选择的概率，即P(ir= (1/ PV)多p(i)，则该粒子被复制，被复制的次数η;由以下决定：
[0040]
[0041] L:」表示舍余取整，
[0042] 如果粒子被复制的总次数小于采样前的高权重和低权重的粒子总数，则依次增加 由最高到低权重粒子被复制的次数，使得粒子被复制的总次数等于采样前的高权重和低权 重的粒子总数；如果粒子被复制的总次数大于采样前的高权重和低权重的粒子总数，则依 次减少由最低到高权重粒子被复制的次数，使得粒子被复制的总次数等于采样前的高权重 和低权重的粒子总数；
[0043] 复制后高权重的各粒子的权重为W;( *为高权重粒子的权重，叫*为对 应各粒子增加或者减少后实际被复制的次数；其余粒子权重保持不变。
[0044] 改进的方法能减少运行时间，降低计算复杂度，增加算法的执行效率；同时通过控 制权值的门限值增加粒子多样性，缓解粒子退化，使得所建地图更精确。
[0045] 在本发明的一种更加优选实施方式中，Η为2,L为0. 5。
[0046] 在本发明的一种优选实施方式中，更新地图的计算方法为：
[0047] 根据描述机器人轨迹的粒子的位姿xf和历史观测信息来更新计算对应的地 图的后验概率ρΟΥ?Ο1，~)，其中为第i粒子在[1，t]时间段的状态，Zl:t为[1，t]时 间段的观测信息，/为第i粒子的地图信息由第i粒子在[1，t]时间段的状 态和[i，t]时间段的观测信息决定。
[0048] 综上所述，由于采用了上述技术方案，本发明的有益效果是：在相同环境下使用相 同粒子数，改进方法提高机器人建立2-D栅格地图的精度，增加移动机器人SLAM的鲁棒性， 保持粒子多样性，防