基于UKF的建图规划方法

REF：基于 RBPF-SLAM 算法的学前儿童机器人路径规划模型研究

1. 基础模型

• 高斯函数：将激光雷达数据、运动模型相结合，在似然函数的峰值区域采样，获取概率大的区域，扫描里程传感器中的初始值，选择 K个在该区域内的粒子

6f6fb1a31913a66acd9d9a0a206bc070.png

其中：

• η(i)：表示归一化因子
• μ(i)：表示均值
• ∑(i)：表示协方差矩阵

• 时间弹性带约束：TEB算法需要使用多目标加权，对位姿和时间两方面进行调整，约束包括路径约束和速度、加速度约束，公式如下：

047e833c97abf0cd3892717a3af23f16.png

其中：

• x_r：表示边界值
• ε：表示逼近的位移
• n：表示多项式阶数
• S：表示缩放比例

2. 模型优化

• 引入退火参数：分布的计算过程较为复杂，高斯函数存在峰值的采样效率变低，通过退火参数α调节模型的比重，当运动模型的数据占比大于观测模型时，将 α 数值调小

429c891326c99e2634ce42284a1d868d.png

• UKF的权重排序：通过高斯分布重采样方法根据粒子权值进行排序，保证 RBPF-SLAM 算法中采样粒子的多样性，新粒子的权值和粒子的大小成正比关系，排序后的高权值粒子采样示意图和粒子分布图如图所示：

e568d8a10c32503f0b6800ecc4ec4bdf.png

• A-star启发函数优化：启发函数采用曼哈顿和欧式距离相结合的方式构建，公式如下

6056eb0853d34086b213010f25e54bb6.png

• A-star路径曲率和目标函数优化：传统A∗算法规划的路径拐点多，采用梯度下降法对路径曲率和规划目标函数进行改进，有助于获取平滑的全局路径，目标函数如式

1b5f0084ca431327c2f3dec98b53b035.png