【EKF实现2维平面上的SLAM】【EKF-SLAM】NWPU 最优估计课程设计（Matlab代码实现）

欢迎来到本博客❤️❤️博主优势博客内容尽量做到思维缜密逻辑清晰为了方便读者。⛳️座右铭行百里者半于九十。⛳️赠与读者‍做科研涉及到一个深在的思想系统需要科研者逻辑缜密踏实认真但是不能只是努力很多时候借力比努力更重要然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学什么是电的时候不要觉得这些问题搞笑。哲学是科学之母哲学就是追究终极问题寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览免得骤然跌入幽暗的迷宫找不到来时的路它不足为你揭示全部问题的答案但若能让人胸中升起一朵朵疑云也未尝不会酿成晚霞斑斓的别一番景致万一它居然给你带来了一场精神世界的苦雨那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。或许雨过云收神驰的天地更清朗.......1 概述运动更新运动更新时固定点坐标不变所以只需要根据运动方程更新(x,y,α)及其协方差与互协方差即可。观测到曾经观测过的固定点这时候依次对观测到的特征点信息进行EKF更新Landmark observations are processed in the EKF usually one-by-one观测到新的固定点这时候观测到新的固定点需要进行状态增广。根据逆观测方程使用观测信息推测出新加的增广状态均值与方差然后加入到总体的状态与协方差矩阵中。文章目录同时定位与建图SLAM的本质是一个估计问题它要求移动机器人利用传感器信息实时地对外界环境结构进行估计并且估算出自己在这个环境中的位置Smith 和 Cheeseman在上个世纪首次将 EKF 估计方法应用到 SLAM。以滤波为主的 SLAM 模型主要包括三个方程1运动方程它会增加机器人定位的不确定性2根据观测对路标初始化的方程它根据观测信息对新的状态量初始化。3路标状态对观测的投影方程根据观测信息对状态更新纠正减小不确定度。详细文档见第4部分。EKF实现2维平面上的SLAM——NWPU最优估计课程设计研究一、引言SLAMSimultaneous Localization and Mapping即同时定位与建图是机器人学和人工智能领域的一个重要研究方向。它旨在让机器人或移动设备在未知环境中通过传感器数据进行自定位并构建环境地图。EKF-SLAM扩展卡尔曼滤波器同步定位与建图算法是解决SLAM问题的一种有效方法它利用扩展卡尔曼滤波器EKF处理非线性问题以估计机器人的位置和地图的状态。二、EKF-SLAM算法原理1. 扩展卡尔曼滤波器EKF基础卡尔曼滤波是一种有效的递归滤波器能够从一系列含有噪声的测量中估计动态系统的状态。EKF是其非线性版本适用于处理系统模型非线性的情况。在EKF中系统的非线性模型通过雅可比矩阵线性化处理然后应用标准卡尔曼滤波器的框架。2. EKF-SLAM状态向量构建在EKF-SLAM中状态向量通常包括机器人的位姿位置和方向以及环境中特征点的位置。状态向量的准确构建是保证EKF-SLAM性能的关键因素之一。3. EKF-SLAM观测模型观测模型描述了传感器测量值与状态向量之间的关系。在EKF-SLAM中常见的传感器观测模型包括距离和角度测量。噪声分析是为了理解和建模观测值中的随机误差这对于更新过程中的权重分配至关重要。4. EKF-SLAM工作流程初始化设定机器人的初始状态和协方差矩阵初始化地图为空。运动预测根据机器人的控制输入和运动模型预测机器人下一时刻的位姿变化同时更新地图特征点的位置预测值以及状态向量的协方差矩阵。测量更新获取传感器测量数据将测量值与预测值进行匹配。对于已有的地图特征点计算测量值与预测值之间的误差并通过卡尔曼增益更新状态向量和协方差矩阵。对于新发现的地图特征点将其添加到状态向量中并初始化其协方差。重复迭代不断重复运动预测和测量更新步骤随着机器人的移动和传感器数据的积累逐渐构建出准确的地图并估计出机器人的状态。三、NWPU最优估计课程设计实现1. 系统初始化初始状态设定设定机器人的初始位姿位置和方向以及初始协方差矩阵。初始协方差矩阵表达了状态估计的不确定性通常设置得较为宽泛以反映初期知识的缺乏。地图初始化初始化一个空的地图用于存储后续观测到的特征点位置。2. 运动模型实现运动方程构建根据机器人的运动特性如轮式机器人的差速运动模型构建运动方程。运动方程用于预测机器人下一时刻的位姿变化。协方差传播在运动预测过程中协方差矩阵需要根据运动方程进行更新以反映状态估计的不确定度变化。3. 观测模型实现观测方程构建根据传感器的特性如激光雷达的距离和角度测量模型构建观测方程。观测方程用于描述传感器测量值与状态向量之间的关系。数据关联处理在测量更新过程中需要解决数据关联问题即确定新观测到的特征点与已有地图中的哪一点对应。常见的数据关联策略包括最近邻法、联合概率数据关联和多假设跟踪等。4. EKF更新实现预测阶段根据运动方程预测下一时刻的状态向量和协方差矩阵。更新阶段根据观测方程和新的传感器数据计算测量值与预测值之间的误差并通过卡尔曼增益更新状态向量和协方差矩阵。5. 地图构建与维护特征点添加当观测到新的地图特征点时将其添加到状态向量中并初始化其协方差。地图优化随着机器人的移动和传感器数据的积累地图特征点的位置需要不断更新和优化以减少累积误差。四、实验结果与分析1. 实验环境设置机器人模型采用轮式机器人模型进行仿真实验。传感器配置配置激光雷达作为主要传感器用于获取环境信息。环境设置在仿真环境中设置多个特征点作为地图的初始结构。2. 实验结果展示定位精度通过对比机器人的真实位姿和EKF-SLAM算法估计的位姿评估定位精度。实验结果表明EKF-SLAM算法能够有效地估计机器人的位姿定位误差在可接受范围内。地图构建质量通过观察构建的地图特征点位置与真实环境中的特征点位置之间的匹配程度评估地图构建质量。实验结果表明EKF-SLAM算法能够逐步构建出准确的环境地图。3. 结果分析算法性能EKF-SLAM算法在定位精度和地图构建质量方面表现出色能够满足机器人自主导航的需求。影响因素实验结果表明传感器的精度、数据关联策略的选择以及地图优化算法的性能等因素都会影响EKF-SLAM算法的性能。2 运行结果传感器探测范围与路标点第一次状态增广状态持续扩大状态增广已停止部分代码% 运动噪声q [0.01;0.01];Q diag(q.^2);% 测量噪声m [.15; 1*pi/180];M diag(m.^2);% R: 机器人初始位置% u: 控制量R [0;-2.2;0];u [0.1;0.05];% 设置外界路标点环境% 环形摆放的landmarks% W: 设置所有路标点位置jiaodu_perLandMark 6; %取1,3,6,15,30,60...(360的倍数均可)r12;r23;r33.5;W landmarks(r1,r2,r3,jiaodu_perLandMark);% 传感器探测半径sensor_r 2.5;% Id容器用来判别当前探测到的路标点曾经是否被观测过若没有观测过那么此时需要将其加入Id容器。% 这里使用W中每个点的索引作为路标点的idId初始化为一个足够大的零数组即可。% Id(类型)1表示曾经观测过Id(类型)0表示曾经没有观测过。% 如果用c实现建议使用map结构。Id zeros(1,size(W,2));% y_news表示当前新探测到的路标点y_news(:,i)记录观测量和路标点类型% 同理y_oldsy_olds zeros(3,size(W,2));y_news zeros(3,size(W,2));% 状态量及协方差初始化x zeros(numel(R)numel(W), 1);P zeros(numel(x),numel(x));% id_to_x_mapid------id对应的状态变量在x中的位置id_to_x_map zeros(1,size(W,2));% x和P初始化r [1 2 3];x(r) R;%x(r) [8;-2.5;0];P(r,r) 0;% 每次状态增广在x中的位置s [4 5];%主循环次数% 125/每圈loop 250;% 存放位姿仿真量poses_ zeros(3,loop);% 存放位姿历史估计量poses zeros(3,loop);% 绘图mapFig figure(1);cla;axis([-5 5 -5 5])axis square%axis equal% 所有路标点WG line(parent,gca,...linestyle,none,...marker,.,...color,m,...xdata,W(1,:),...ydata,W(2,:));% 仿真下机器人位置RG line(parent,gca,...marker,,...color,r,...xdata,R(1),...ydata,R(2));% 估计的机器人位置rG line(parent,gca,...linestyle,none,...marker,,...color,b,...xdata,x(r(1)),...ydata,x(r(2)));% 估计的路标点位置lG line(parent,gca,...linestyle,none,...marker,,...color,k,...xdata,[],...ydata,[]);% 估计的路标点协方差eG1 zeros(1,size(W,2));for i 1:numel(eG1)eG1(i) line(...parent, gca,...color,k,...xdata,[],...ydata,[]);end% 估计的机器人位置reG line(...parent, gca,...color,r,...xdata,[],...ydata,[]);% 传感器探测范围以真实位置为圆心sensor1 line(...parent, gca,...color,m,...xdata,[],...ydata,[],...LineStyle,--);sensor2 line(...parent, gca,...color,m,...xdata,[],...ydata,[],...LineStyle,--);%传感器探测范围以估计位置为圆心Sensor1 line(...parent, gca,...color,m,...xdata,[],...ydata,[],...LineStyle,--);Sensor2 line(...parent, gca,...color,m,...xdata,[],...ydata,[],...LineStyle,--);true_pose line(...parent, gca,...color,r,...xdata,[],...ydata,[],...LineWidth,0.8);%LineStyle,--);estimate_pose line(...parent, gca,...color,b,...xdata,[],...ydata,[],...LineWidth,0.8);% LineStyle,--);% II. 主循环% 机器人每前进一步循环一次for t 1:loop% if t 125% u(1) 0.2;% sensor_r 4;% end% if t 375% u(1) 0.2;% sensor_r 5;% end%不同探测半径3参考文献文章中一些内容引自网络会注明出处或引用为参考文献难免有未尽之处如有不妥请随时联系删除。4 Matlab代码、文档讲解资料获取更多粉丝福利MATLAB|Simulink|Python资源获取