专利名称：一种智能移动机器人的定位与环境建模方法
技术领域：
本发明涉及自动控制领域，特别涉及一种移动机器人的定位与环境建模方法。
背景技术：
移动机器人是传感器技术、控制技术、信息处理技术、机械加工技术、电子技术、计算技术等多门技术相互结合而形成的。在这些技术的基础上，移动机器人主要研究以下内容机器结构、多机器人系统、体系结构、路径规划、人工智能、导航与定位、多传感器信息融合技术、人机交互、反馈镇定以及跟踪控制。在以上的研究内容中，有一项技术是移动机器人的实际应用中不可或缺的，这项技术就是导航与定位的研究。移动机器人，顾名思义这种机器人不是固定在某一地方不动，而是根据任务的要求随时的采取相应措施从起点到达目标点。而在这个过程中，移动机器人需要知道自己该往哪儿走，需要知道自己在哪儿，需要知道自己是不是到了目的地。而对这些问题的回答，就形成了移动机器人的导航与定位研 究内容。Leonard和Durrant-Whyte两位研究人员对这个问题做了一个具有纲领性的总结，也即移动机器人在具备以下几个条件的情况下能够自主的进行移动，避免时刻受到人为因素的控制=(I)Where am I (我在哪里？)在移动机器人的导航过程中，移动机器人需要时刻的知道自己所处的位置，只有在知道了自己所处的位置，才能为下一步需要采取何种动作提高判断依据。这个问题就是移动机器人的定位问题，在很好的回答了这个问题的基础上，才有可能解决后面两个问题；(2) Where am I going (我要去哪里？)在移动机器人获得任务后，它就得考虑在完成一步之后，下一步应该往哪里走，或者是它的目的地在什么地方。这个问题就需要移动机器人对所处环境进行建模，也就是环境建模或者说是地图构建的研究；(3)How should I get there (我应该如何到那儿？)一旦机器人在知晓了自己的当前位置和下一步要去哪里之后，它就需要考虑自己在从当前位置到达下一步位置应该怎么走，也就是在这个过程中，需要走的路径尽可能的短并且不能碰到障碍物。这就是移动机器人的路径规划研究，是机器人的运动控制中的一部分研究内容。因此，对于移动机器人的定位与环境建模进行研究，可以扩展移动机器人的运动范围、使用功能，能够对移动机器人的信息无障碍研究和自主式移动提供巨大的技术支撑。在移动机器人的定位和环境建模研究方面，传统上的方法都是对这两个问题进行单独的分析。因此，没有考虑过将两者进行统一的研究。我们知道，在移动机器人的定位中，如果在有精确的地图的情况下，机器人的定位准确性就很高。但是，由于移动机器人本身的工作特性，我们不可能每次都能知道机器人工作环境的具体情况。在这种情况下，移动机器人对环境的认知就只有依靠自己携带的传感器来采集环境信息。因此，当机器人所处的环境比较陌生的情况下，机器人会面临这一个两难的问题即为了构建环境地图，机器人需要清楚的知道自己在每个时刻所处的位置，为了精确的定位，移动机器人需要有一个高准确率的地图。而这个问题，可以用一个非常形象的比喻来描述，即“鸡与蛋”的问题。为了准确并且高效的感知环境，移动机器人需要将定位问题与环境建模问题同时考虑。在未知环境模型下移动机器人的定位导航需要构造环境的模型，这种在确定自身位置的同时并构造环境模型的过程，被称为SLAM (Simultaneous Localizationand Mapping)。移动机器人的同时定位与环境建模(SLAM)最早由Smith、Self和Cheeseman提出，由于其重要的理论与应用价值，被诸多研究人员认为是实现真正的全自主移动机器人的关键。SLAM问题归纳起来是一个“预测一观测一更新”的过程，主要包括如下迭代步骤
(1)根据当前的机器人位姿估计和地图信息对下一步的机器人位姿和路标位置进行预测；
(2)在下一步中通过外部传感器观测环境中的路标；(3)利用观测结果校正第一步预测的定位和建模结果。上述步骤迭代进行，在此过程中机器人对自身位姿和路标位置的估计逐渐准确。SLAM中使用得最广泛的扩展卡尔曼滤波算法(Extended Kalman Filter, EKF)在线性化过程中对非线性函数进行Taylor级数展开，忽略二次及其以上阶次得到近似的线性模型，但是该算法在对于Taylor展开式的高阶次项的忽略，线性化所产生的误差就会比较大。所以，此时应用于SLAM的EKF就会增大误差。需要对EKF进行相应的改进。 因此急需一种适用于移动机器人同时进行定位与环境建模的高效率的计算方法。

发明内容
有鉴于此，本发明所要解决的技术问题是提供一种适用于移动机器人同时进行定位与环境建模的高效率的计算方法。该算法以常规的EKF算法为基础，对EKF在数学上的缺陷进行优化、修改，使之能够很适用于移动机器人的同时定位与环境建模中，提高算法在SLAM中的估计效果。本发明的目的是这样实现的本发明提供的一种智能移动机器人的定位与环境建模方法，包括以下步骤SI :形成修正迭代扩展卡尔曼滤波算法，并确定迭代次数；S2 :建立移动机器人的运动模型Xk ；S3 :建立移动机器人的观测模型Zk ；S4 :初始化移动机器人的先验状态变量乓、先验协方差矩阵ZT、系统的过程噪声协方差Qlri及观测噪声协方差Rlri ；S5 :计算移动机器人运动模型的位置雅可比矩阵S6 :计算移动机器人控制输入雅可比矩阵▽/S7 :计算观测模型的观测雅可比矩阵Jh ；S8 :计算卡尔曼增益Kk ；S9 :更新后验状态估计毛和后验状态估计协方差矩阵Pk ；SlO :重复循环步骤S5至S9。进一步，所述修正迭代扩展卡尔曼滤波算法，具体如下时间更新(预测)计算先验状态变量 : = fh, ,计算先验状态估计协方差矩阵..Pk = AkPk^Al + WkQkJVf，测量更新(校正)计算卡尔曼增益-.Kk= P-Jl [JhPhJji + VkRlJf )-'，由观测变量Zk更新后验状态估计 二 XiT + Kk{zk-Hk)，
后验状态估计协方差矩阵更新
权利要求
1.一种智能移动机器人的定位与环境建模方法，其特征在于它包括以下步骤 Si:形成修正迭代扩展卡尔曼滤波算法，并确定迭代次数； 52:建立移动机器人的运动模型Xk ； 53:建立移动机器人的观测模型Zk ； 54:初始化移动机器人的先验状态变量％、先验协方差矩阵iT、系统的过程噪声协方差Qk-!及观测噪声协方差Rlri ； 55:计算移动机器人运动模型的位置雅可比矩阵V/Xi 56:计算移动机器人控制输入雅可比矩阵V/ ; 57:计算观测模型的观测雅可比矩阵Jh ； S8:计算卡尔曼增益Kk; S9 :更新后验状态估计毛和后验状态估计协方差矩阵Pk ； SlO :重复循环步骤S5至S9。
2.根据权利要求I所述的智能移动机器人的定位与环境建模方法，其特征在于形成修正迭代扩展卡尔曼滤波算法，具体如下 时间更新-预测 计算先验状态变量 =， 计算先验状态估计协方差矩阵-A: =AA-—A+WkQk^wI， 测量更新-校正计算卡尔曼增益:Kk = P-J1h (JhPhJ7h + VkRkVk7 Y1, 由观测变量Zk更新后验状态估计■太—=xk+Kk(zk-Hk)， 后验状态估计协方差矩阵更新-A =(J-KkJk)Pk， 其中，4表示Xk在k时刻的先验估计,毛是Xk在k时刻的后验估计是系统从k-1时刻的状态向量变化到k时刻的控制输入，Zk是k时刻的观测向量，f( )是系统的状态转移函数表示k时刻的协方差估计，Ak和Wk分别是f(_)对xk和对W的偏导雅可比矩阵，和灰/分别是它们的转置矩阵，Qk-!表示过程激励噪声协方差，Kk表示卡尔曼增益，Jh表示状态向量Xk对测量向量Zk的增益，Vk是f ( )对V的偏导雅可比矩阵，Rk表示k时刻的噪声协方差，0—1表示矩阵的逆，Zk表示测量向量，Hk表示非线性函数h对X的偏导雅可比矩阵，I表示单位矩阵。
3.根据权利要求2所述的智能移动机器人的定位与环境建模方法，其特征在于建立移动机器人的运动学模型具体如下
4.根据权利要求3所述的智能移动机器人的定位与环境建模方法，其特征在于建立移动机器人观测模型具体如下 zk = h (xk, vk)
5.根据权利要求4所述的智能移动机器人的定位与环境建模方法，其特征在于计算移动机器人的位置雅可比矩阵如下
6.根据权利要求5所述的智能移动机器人的定位与环境建模方法，其特征在于计算移动机器人的控制输入的雅可比矩阵7/ 如下
7.根据权利要求6所述的智能移动机器人的定位与环境建模方法，其特征在于观测模型实际观测数据通过移动机器人上的传感器获得，所述观测模型在第i个坐标下的雅可比矩阵Jh的计算具体如下
8.根据权利要求I所述的智能移动机器人的定位与环境建模方法，其特征在于所述迭代处理次数根据优化效果和计算速度确定。
全文摘要
本发明公开了一种智能移动机器人的定位与环境建模方法，首先形成修正迭代扩展卡尔曼滤波算法并确定迭代次数，然后建立移动机器人的运动模型和观测模型，初始化移动机器人的状态，计算位置雅可比矩阵和控制输入雅可比矩阵计算、观测雅可比矩阵等；最后通过求解卡尔曼增益矩阵，更新状态估计方程和协方差矩阵，并重复以上部分步骤。本发明以在移动机器人的同时定位与环境建模领域中使用最为广泛的扩展卡尔曼滤波算法为核心，通过对该算法进行改进，使得算法的性能得到极大提高，能够更好的满足在SLAM中的应用。也为移动机器人在未知环境中的自主导航、完成复杂智能任务提供强大的技术支撑。
文档编号G01C21/00GK102706342SQ20121017787
公开日2012年10月3日 申请日期2012年5月31日 优先权日2012年5月31日
发明者唐贤伦, 张毅, 徐晓东, 李敏, 罗元, 胡章芳, 蔡军, 谢颖 申请人:重庆邮电大学