[发明专利]基于ROS环境下的多线程分布式SLAM系统的实现方法

摘要

本发明公开了基于ROS环境下的多线程分布式SLAM系统的实现方法，基于ROS环境下的多线程的分布式SLAM系统是利用分布式的结构将整个状态向量分为机器人位姿估计和路标估计共五维状态，本设计采用系统模型的分布化处理，将多个有效路标点建立多个线程，形成了相互平行单独构成子滤波器的模型结构，然后将子滤波器的机器人的位姿估计结果通过主线程在主滤波器中进行融合，通过子滤波器的融合结果最后得到机器人位置估算结果。最后通过真实实验使用本发明算法和离线分布式算法对比，证明了本算法的可行性和有效性，从而完成了对基于ROS环境下的多线程分布式SLAM系统的实现。

主权项

1.基于ROS环境下的多线程分布式SLAM系统的实现方法，其特征在于：该实现方法的具体步骤如下，/n步骤1：搭建信息数据采集系统；/n依据各传感器之间的信息采集和传输方式，构建载有激光，里程计，磁罗盘等传感器的多传感器的信息数据采集系统；上电后，激光传感器以及磁罗盘开始进行数据采集和数据的传送，并将不同的传感器数据进行时钟匹配，将里程计数据、激光扫描数据、罗盘航向数据按照统一的数据格式进行数据保存，以便用于之后多线程的分布式SLAM算法；/n步骤2：模型的建立；/n将整个状态向量分为机器人位姿估计和路标点估计共五维状态，其状态向量为：x_L＝[x_v,y_v,φ_v,x_L,y_L]^T，x_v,y_v,φ_v是小车的状态，x_L,y_L是路标点的状态；建立运动模型和观测模型，机器人的状态描述为：s_t＝(x,y,φ)^T，其中，x，y表示坐标值，φ表示偏航角度其中，φ＝0表示指向x轴正向，φ＝π/2表示指向y轴正向；机器人的状态转移模型如下：/n /n式中X_L(t)＝(x_v(t) y_v(t) φ_v(t))^T为机器人在t时刻的位姿，ΔT为时间变化量，v_c为机器人移动速度，α为机器人车轮转角，L为两轮间距，γ是协方差为Q均值为零的高斯白噪声；/n扩展卡尔曼滤波的量测更新结果是由激光传感器测得的路标点距离信息及磁罗盘测得的航向信息计算得到的，针对激光传感器及磁罗盘给出机器人的观测模型，观测量z分别是某个路标点相对于传感器的距离和角度；将机器人状态信息与观测信息相关联，获得路标点的观测模型：/n /n式中，为观测量包括传感器测得的机器人与路标点间距离和角度，X_L＝(x_Ly_L)^T为路标点坐标，v是协方差为R均值为0的高斯白噪声；m_L为地图上坐标；/n运动模型和观测模型分别利用里程计的输出和激光传感器的输出来估计相应的机器人及路标点位置信息；/n步骤3：地图初始化；/n建立坐标系，以机器人初始时刻的位置为坐标原点，以正东和正北方向为x轴和y轴的正方向；地图初始化，机器人在初始位置，利用传感器扫描到的路标点信息建立环境地图，将机器人位姿和路标点共五维状态一起作为全局地图进行存储；/n步骤4：路标点匹配并建立分布式子滤波器；/n获取机器人t-1时刻测得的路标点状态信息，并与在之前已经存储在地图中的路标点信息相匹配，对于在t-1时刻没有匹配上的路标点，将其添加至地图；对于在t-1时刻能够匹配上的多个路标点分别对其建立子滤波器，用于并行多线程分布式滤波器对机器人下一时刻位姿估计和路标点估计；/n步骤5：通过分布式多线程扩展卡尔曼滤波算法进行状态更新；/n每个路标点的信息及t时刻的机器人航向信息送入同一个子滤波器，通过分布式多线程扩展卡尔曼滤波算法对上述步骤中建立的各个子滤波器的状态向量和协方差进行处理，分别进行状态转移矩阵和相应协方差矩阵的迭代更新，包括预测过程和量测更新两个方面；/n预测过程：/n首先将t-1时刻的每个子滤波器的估计状态和协方差矩阵，结合t时刻里程计的输出数据，代入状态转移模型中，再经过一步预测方程进行一步递推，得到机器人位置；/n每个子滤波器位姿估计过程为：/n /n /n量测更新：/n通过预测过程中递推的方式，得到机器人位置的预测值后，然后再收集激光传感器和磁罗盘的观测信息；结合预测值和测量值，将得到的结果作为以下量测更新的输入；子滤波器和主滤波器中的量测更新为：/n /n /n /n得到现在状态的修正后的估算值并同时输出相应的协方差矩阵供后续迭代；/n通过上述预测过程和量测更新，对机器人位姿和路标点进行更新；/n步骤6：计算权值归一化并对子滤波器数据融合输出结果；/n根据步骤4中将各个子滤波器的估计结果输入到主滤波器中，根据各子滤波器协方差确定各子滤波器在主滤波器中状态和协方差的权重，然后根据式得到子滤波器更新的结果后，根据协方差矩阵进行融合过程，主滤波器中的融合采用加权平均法，从而输出最终的位置：/n /n /n其中η_i为加权系数，加权平均法将来自不同子滤波器的机器人位姿估计信息计算权值，进行加权平均后的结果作为融合值；/n利用加权平均法归一化权值；通过主滤波器对子滤波器中所有数据进行融合计算，最终输出t时刻机器人的状态估计和方差并把估计存入地图；/n步骤7：将算法融入系统之中，搭建基于多传感器采集系统的ROS环境下的多线程分布式SLAM系统。/n