毕业设计（论文）-基于家庭服务机器人的定位方法研究.doc

目录摘 要11 家庭服务机器人系统建模21.1 平台-“旅行家”自主移动机器人21.2 机器人坐标系统建模31.3 运动控制模型31.3.1 圆弧模型41.3.2 直线模型51.4 环境地图模式61.5 传感器观测模型61.6 环境噪声和传感器噪声模型82 基于家庭自然环境特征的局部环境地图建立92.1 LRG-04LX激光传感器92.2 家庭自然环境特征识别102.2.1组成棱角平面的直线提取102.2.2棱角识别与提取113 基于家庭自然环境特征的定位方法研究133.1 SLAM模型描述133.2卡尔曼滤波算法153. 3打一展卡尔曼滤波定位算法173.3.1 EKF算法原理173.3.2 EKF 算法定位模型193.3.3 数据关联分析20结束语21摘 要基于家庭服务机器人的定位方法研究家庭服务机器人是一种能为人类日常家庭生活服务的特种智能机器人。集助老、护老、做家务、娱乐等多功能于一体的家庭服务机器人越来越受到人们重视，是近几年机器人领域研究的一个热点。解决家庭环境中服务机器人的定位问题是服务机器人能顺利完成其它家庭生活任务的基础。本文围绕家庭服务机器人利用家庭自然环境特征进行定位研究，其主要内容如下：1、针对家庭服务机器人系统建模和实验环境建模等问题，本文以“旅行家 II”自主移动机器人系统（本文简称“机器人”）为基础实验平台，建立家庭服务机器人系统的运动模型和确定家庭服务机器人系统控制输入模型，定义环境特征表示形式，建立传感器观测模型。分析家庭服务机器人系统建模时产生的不确定性和误差，阐述环境噪声模型和传感器观测噪声模型，完成实验所需模型的建立，为家庭服务机器人定位研究奠定基础。2、研究家庭自然环境特征的提取与表示方法，基于家庭自然环境特征建立家庭环境特征地图。主要利用家庭服务机器人自身携带的激光传感器探测家庭环境内形状规则物体的棱角，并将已探测到的规则物体的棱角作为家庭服务机器人在家庭环境内定位时的自然路标，定义这些自然路标为自然环境特征的特征点，建立环境特征地图。最后通过“旅行家 II”自主移动机器人系统进行实验，验证了所提出方法的有效性。3、研究解决家庭服务机器人系统定位问题的理论方法。结合家庭服务机器人SLAM 模型和家庭自然环境特征提取方法，利用扩展卡尔曼滤波（EKF）算法和无迹卡尔曼滤波（UKF）算法来预测和估计家庭服务机器人各时刻的位姿状态和自然环境路标的位置，并更新自然环境特征地图。在理论计算方面，利用 UKF 算法定位的误差要比 EKF 算法定位的误差小。关键词：家庭服务机器人，定位，家庭自然环境特征，扩展卡尔曼滤波 1 家庭服务机器人系统建模1.1 平台-“旅行家”自主移动机器人 博创科技“旅行家II自主移动机器人系统用的是模块化设计方法，包括底层运动底盘模块、电源及驱动设备模块、传感器模块、计算机模块，具有高运动精度的直流伺服控制系统和高负载能力，拥有开放式的系统架构，可以扩展其它功能的辅助设备，如激光、红外、视觉和陀螺仪等传感器。 图“旅行家”自主移动机器人系统标准尺寸 长*宽*高 470*440*440mm 重量 30KG 额定电压 24VDC 工作电流 2A 驱动方式 双轮差动式私服点击直接驱动 电池 24V18Ah动力蓄电池 最大速度 3m/s 最小速度 0.001m/s 最大负载 40KG 工作时间 6小时 爬坡能力 15 越障能力 3cm高度障碍物 表 “旅行家”自主移动机器人系统基本系能参数1.2 机器人坐标系统建模 主要应用三种坐标系统:全局坐标系统(世界坐标系统)，以 表示；机器人坐标系统，以 表示；激光传感器坐标系统，以 表示。1.3 运动控制模型 机器人的定位过程是机器人移动时利用采集到的信息进行位姿状态估计的过程，也是确定机器人在全局坐标系统下移动时位置和方向的过程。 机器人在二维的平面环境中移动时，位姿状态可以用一个三维向量 表示。其中 表示在K时刻机器人在全局坐标系下的位置坐标； 表示机器人的前进方向以偏离全局坐标系中轴所成的夹角，定义夹角 的方向是以纵轴正向为零度角，逆时针旋转为正方向，顺时针旋转为负方向。旋转所成夹角 的范围在180到-180 。运动控制模型可用一个离散时间差分方程表示： 其中 表示K时刻机器人的位姿状态； 为已知输入控制量； 为噪声干扰，用以表示机器人移动时传感器的误差漂移、打滑、外界干扰和系统建模误差等未知影响。 里程计传感器模型 机器人运动控制模型是在里程计传感器模型基础上建立的。里程计传感器能很好地应用在轮式机器人的相对定位中，它依靠机器人的两个驱动轮电机上安装的光电编码，测定机器人移动过程中车轮在固定时间段内转过的弧度，从计算出机器人在行进中的相对位姿的变化。 分为两种：直线模型和圆弧模型 1.3.1 圆弧模型 设机器人的车轮半径为r，光电编码盘为p线/转，在T时间段内光电编码盘输出的脉冲个数为N，则此机器人车轮转过的距离d(弧度)为: 假设，已知机器人两车轮之间的距离为a，如果机器人从k-1时刻运动到k时刻，由光电编码盘测定的左右车轮的前进距离分别为 和 ，机器人位姿状态由 变为 ，则机器人移动的距离为 转过的角度为 若在K时刻以 作为机器人里程传感器的控制输入，则机器人在全局坐标系内的运动半径可以表示为： 由式（1.1）和式（1.3），由图可以推导出机器人沿着圆弧模型移动时的运动控制方程为： 机器人进行圆弧移动示意图1.3.2 直线模型 若机器人是沿着直线进行移动， ，由下图可以推导出机器人沿着直线移动时的运动控制方程为: 里程计能很方便地记录机器人前进移动的信息，但是随着时间的增加，外界的干扰不断，如机器人本身建模计算误差和轮子打滑等不确定因素产生的误差，里程计的累积误差也在不断地增大。因此，短距离移动时应用里程计模型对机器人位姿估计是准确的，如果进行长距离的运动和进行机器人位姿估计计算，则需要在机器人本身安装其它外部传感器，如激光传感器、超声传感器、红外传感器等，依靠外部传感器进行外部数据的探测和回馈，对里程计模型计算带来的累积误差进行补偿和校正。另一种控制输入模型：若机器人的控制输入为 其中，V为机器人的前向速度， 为机器人转弯时的角速度，机器人移动的时间间隔为 ，则机器人在T时间内移动的距离为D(k)=V x T，前进方向的偏角为 。这样就可以从另一方面计算出机器人的移动距离 D(k)和机器人前进方向的偏角 。进而可推导出机器人在全局坐标系下的另一种运动控制方程: 1.4 环境地图模式目前国内外学者进行家庭服务机器人定位实验研究时，所采用环境地图的表示方法主要有二种:特征地图、栅格地图和拓扑地图。 本文主要采用特征地图进行家庭服务机器人定位实验研究。实验时，家庭环境地图是利用激光传感器提取家庭自然环境特征在笛卡尔坐标系构成环境特征地图。环境地图中的特征点以 表示，i=1,2,3,.,x, 表示第i个环境特征点在全局坐标系中的真实坐标位置。1.5 传感器观测模型机器人对家庭自然环境特征测取实验数据的过程中，主要依靠外部传感器和内部传感器。依靠传感器测取的观测信息来进行自身的定位。本文应用激光传感器作为机器人的观测传感器来测取实验数据，以超声传感器和红外传感器作为辅助探测传感器。 假设k时刻机器人的位姿状态是知道的，根据激光传感器采集到家庭自然环境特征的特征点数据后，机器人便可利用采集到的这些环境特征点相对于激光传感器的方向和距离数据，对采集到的环境特征点与实际环境特征点进行数据关联和匹配，建立基于家庭自然环境特征点与激光传感器之间的局部地图，局部地图中特征点的位置坐标为 ，也就能解决机器人在局部地图中的定位问题，也是机器人的相对定位问题。 机器人利用已建立的局部地图和k时刻位姿状态便能计算出探测到的家庭自然环境特征点在全局坐标系中的位置坐标 。机器人移动到k+1时刻位置时，再利用k时刻家庭自然环境特征点在全局坐标系中的位置坐标和k+1时刻的观测数据信息，能计算出k+1刻机器人的位姿状态，进而能完成k时刻和k+1时刻两个环境特征地图的叠加，以此循环，便能利用激光传感器的观测数据信息计算出机器人每个时刻的位姿状态和建立家庭自然环境特征的全局地图，解决了机器人在家庭环境中的绝对定位问题。假设激光传感器与环境特征点组成的局部坐标系中，激光传感器所观测到的环境特征点的位置坐标，以笛卡尔坐标系形式表示为 ，则激光传感器的观测量为 同理，假设激光传感器与环境特征点组成的局部坐标系下，激光传感器所观测到的环境特征点的位置坐标，以极坐标形式表示为，则激光传感器的观测量则为 激光传感器的观测模型可以直观地显示出环境特征点的位置坐标和机器人位姿状态之间的关系。这种关系可以用一种观测方程表示: 其中，Z(k)表示在k时刻机器人在全局坐标系下的观测量， 是激光传感器的测量函数，v(k)是激光传感器的观测噪声，表示激光传感器在采集数据时外界的干扰和模型本身的误差。设k时刻环境特征点在全局坐标系中的位置坐标为 ，激光传感器位置坐标为 机器人位姿状态为 ，结合激光传感器直接探测到的数据包括激光传感器到环境特征点的相对极坐标距离 和激光束横扫过夹角 ，可以计算出激光传感器观测环境特征点的观测量分别在激光传感器、机器人坐标系和全局坐标系中的表示形式: 传感器坐标系中，激光传感器观测环境特征点的观测量为: 机器人坐标系中，激光传感器观测环境特征点的观测量为: 全局坐标系中，激光传感器观测环境特征点的观测量为: 1.6 环境噪声和传感器噪声模型机器人能在家庭环境中通过探测家庭自然环境特征进行定位，依靠自身的传感器进行实验数据的采集，如里程计、激光传感器、红外传感器和超声波传感器等，进行的这些过程都会产生传感器的观测噪声和测量的不确定性，测量结果也很容易受到各种复杂环境的干扰。 此外，机器人模型建立的过程中，机器人本身常具有非完整的约束，其模型是一个高阶的非线性模型系统，要想精确地建立这样高阶的非线性模型也是很不易的，往往都是通过简化复杂高阶模型的方法来获取近似的模型，在获取近似模型的过程中，也要产生误差，为模型噪声。 所以，在机器人系统相关模型和环境模型建立时要考虑噪声模型。 在实际应用中，最常用的噪声模型是高斯白噪声模型，均值为，标准差为。2 基于家庭自然环境特征的局部环境地图建立依据机器人建立的环境地图表示方法主要可分为二种:栅格地图、特征地图和拓扑地图。目前国内外学者已经提出的建立家庭环境地图解决方案可以概括为两种: 一种是在家庭环境内除服务机器人本体外安装人工辅助设备，如无线射频标签、QRcode人工地标、无线传感器网络设备等，利用这些人工的辅助设备家庭服务机器人便能读取环境信息并建立家庭环境地图。另一种是不依靠在除服务机器人本体外安装人工服务设备，而是仅依靠自身安装的设备(如双目立体视觉系统、超声波传感器、红外传感器等)探测家庭环境内物体的特征和距离信息，并根据该环境特征和采集到的距离信息建立地图。 这里我们采用URG-04LX激光传感器探测家庭环境内规则物体棱角的方法，并利用该棱角作为家庭环境自然特征路标建立家庭局部环境特征地图。2.1 LRG-04LX激光传感器Hokuyo URG-04LX激光传感器的扫描频率为l 0Hz，激光扫描的范围是240度，扫描的角度分辨率为0.36度，测距范围在0.02m-6m,测距精度为1 0mm，功耗约为3W。基于以上的参数，URG-04LX激光传感器能很好地应用在解决家庭服务机器人的同时定位与地图创建问题中。 如图表示为URG-04LX激光传感器在特定的环境空问内探测两个圆柱形障碍物，在上位机的激光传感器显示界面能实时显示已经探测到的无障碍区域和未知探测区域，可以通过激光传感器已探测到的距离和坐标系间转换关系，来计算和确定圆柱的直径和相对特定坐标系中的位置坐标，进而能实现解决机器人的定位导航、避障和路径规划等问题。2.2 家庭自然环境特征识别 在家庭环境中，机器人要依靠自身的传感器探测家庭自然环境的特征作为定位导航的路标。如用视觉传感器识别出环境特征的方位、形状和大小等，再利用激光传感器探测机器人到环境特征的距离，并将其选定为机器人在移动时家庭环境中的自然路标，根据机器人与环境特征的坐标系转换关系，确定此家庭自然特征路标在机器人坐标系中的位置坐标，机器人再根据这个环境特征(自然路标)进行定位和建图，进而能更好地完成机器人为人类生活服务的目标。家庭环境中，有多种物体可以被选作为机器人定位时的自然路标。如固定不变的墙棱、墙角和墙面等，此外家用物品如结构规则的木制床和衣柜的边和角、桌子和凳子的棱角等都可以作为机器人定位时的自然路标。2.2.1组成棱角平面的直线提取 当URG-04LX激光传感器探测到棱角规则的结构化物体表面时，一个扫描周期内会在组成棱角的两个平面内扫描一个离散的点集，利用如图所示的分割方法将这个离散的点集分割成多个目标点集，然后再利用式2.1到式2.5所述的直线拟合方法将分割后的目标点集进行直线拟合，完成组成棱角平面的直线提取。 假设URG-04LX激光传感器扫描棱角规则的结构化物体表面产生一个包含n个采样点的点集 ，在采样点集C中，由起点 到终点 能确定一条直线 ，搜索采样点集C中距离直线L最大的点 ，如果 到直线的距离 大于已设定值 ，则定义 为断点，将采样点集C分割成 和 两个采样点集，然后在采样点集 和 中再用上述的方法进行数据分割，直到分割后的所有采样点集满足设定的条件为止。 经过分割得到目标采样点集后，需要对目标采样点集进行直线拟合。采样点集C极坐标表示形式为 若激光传感器采到第i个点 ，其极坐标为 ，可以得到此采样点相对十在激光传感器坐标系中的位置坐标 和 假设先不考虑采样点受外界干扰的误差，则激光传感器扫过物体表面的所有采样点都应该落在一条直线上，根据 可以确定一条直线，可得采样点与直线的关系下式所示: 其中， 表示坐标原点与式2.1表示的关系直线之间的最短距离， 表示关系直线与全局坐标系横轴之间的夹角。但是在实际测量中，外界的干扰和测量误差是存在的，所以式2.1并不是0，对于已采样的点 其与拟合直线的正交距离为 如下式所示： 激光传感器在测量每个采样点时的不确定性都是相同的，对所有的采样点的测量误差的平方和加在一起，然后再将全部采样点到直线之间的所有误差进行拟合量化，如下式所示： 需要拟合的直线是由以坐标原点到直线距离 和直线与横轴夹角 确定，所以要精确地拟合这些点集的直线把误差减到最小，就需要找到最适合的一组 和 使得S最小，也就是对式2.3进行计算求解。 2.4解方程2.4可得 和 的直角坐标形式:由式2.5和式2.6所得到的参数 和参数 ，可确定分割后点集的拟合直线方程。2.2.2棱角识别与提取在家庭环境中，假设墙、结构化床或箱体等的棱角处定义为是两条直线的交点，所以棱角的位置坐标可以利用组成这个拐角的两条直线进行计算，所计算出的交点即为被选定的家庭环境自然特征的位置坐标，也是自然路标的位置坐标。假设已计算出组成棱角的两条直线方程，如式 2.7然后计算公式2.7，便可得到组成棱角的两条直线相交的交点 即为棱角的坐标点，如式2.8 2.8其中， 和 可以由式2.5和式2.6得知。3 基于家庭自然环境特征的定位方法研究家庭服务机器人在家庭环境中进行精确定位，需要以该家庭环境地图为基础，也就是说在家庭服务机器人精确定位之前，家庭环境地图是需要提前知道。然而，要想建立家庭环境地图，也必须要知道家庭服务机器人在任意时刻的位姿。因此，在研究家庭服务机器人在室内的定位和导航的问题时，就需要同时解决服务机器人定位和建立家庭环境地图这两个并存的问题。这也就回归到了解决家庭环境中服务机器人的同时定位与地图建立( The Simultaneous Localization and Mapping, SLAM)的问题，也被称为CLM ( Concurrent Mapping and Localization)问题。SLAM问题最早是在1988年由Smith Self和Cheese man等人提出的。从机器人SLAM问题模型的描述中可以看到，SLAM问题从统计学的角度来说也是一个滤波估计的问题，就是根据机器人系统的初始时刻的状态、当前时刻的运动控制量和对家庭环境特征的观测量估计出下一时刻机器人的运动状态信息和对家庭环境特征的观测信息。 机器人在家庭环境中移动时，利用自身携带的传感器已经能探测到家庭环境的自然特征，再根据此时刻采集到的数据信息和机器人的初始状态便能计算出当前的位姿状态，但是，如何利用这一时刻机器人的位姿状态和自然路标的位置计算出下一时刻机器人的位姿状态和如何利用传感器探测的数据进行自然路标特征点的匹配问题，这就需要选择有效的算法来解决。3.1 SLAM模型描述SLAM过程是一个机器人能建立环境地图并目_同时能利用此环境地图推断其位置的过程5。在SLAM研究的问题里，机器人在环境中的移动轨迹和所有环境特征路标的位置都是在线估计出来的，不需要先验的预定位置。考虑到机器人在环境中移动时要观测很多未知的环境特征路标，机器人需要安装多种传感器(如激光传感器、视觉传感器、超声测距传感器、红外传感等)。对十SLAM题描述，一般是采用基十概率的SLAM模型描述。在采用概率的形式描述SLAM模型中，需要计算k时刻的概率分布: 其中，在K时刻定义： X(k):表示状态向量，包括机器人在全局坐标系中的位置坐标和方向； U(k):表示控制输入向量，控制机器人由k-1时刻的位姿状态X(k-1)变化到k时刻的位姿状态X(k) ; 表示第i(i=1, 2, 3.)个环境特征路标在全局坐标系中真实坐标位置; :表示在k时刻机器人观测到第i个环境特征路标的观测向量。当在某一时刻有多个环境特征路标的观测值或者在讨论特殊的不相关的环境特征时，观测向量也可以用Z(k)表示。此外，定义集合 和m: 表示机器人基于初始时刻运动到k时刻的所有位姿状态向量集合； 表示机器人基于初始时刻运动到k时刻的所有控制输入向量集合； 表示机器人基于初始时刻运动到k时刻的所有已观测到的环境特征路标的观测向量集合； 表示为环境特征路标的集合。 通常，应用递归的方法解决机器人同时定位与地图建立问题，如图所示，在k-1时刻估计出的概率分布为 ，再利用k时刻的控制量U(k)、观测量Z(k)和贝叶斯定理就能计算出k时刻的概率分布。在这个递归的计算过程中，需要一个运动状态模型和一个观测模型来显示控制输入和观测的作用效果。当机器人坐标位置和环境特征的坐标位置被计算出来时，观测模型描述了产生观测量Z(k)的概率，此时，观测量Z(k)的概率分布为: 由此可以看出，当机器人的位姿状态和地图确定时，观测量根据独立的环境特征地图和当前的机器人的位姿状态就能被计算出来。机器人的运动模型可以用位姿状态的概率分布进行描述，其形式为: 其中，位姿状态的变化过程可以假设成马尔可夫过程，当观测量和地图相互独立时，仅依靠k-1时刻的运动状态X(k-1)和k时刻的控制输入量U(k)就能推算出k时刻移动机器人的运动状态X(k)。SLAM算法主要由两个步骤实现: A、 预测(时间的更新) B、 修正(测量值的更新) 由此可以分析出，根据基于初始时刻到k时刻的观测值 和控制输入 利用式(3.4)和式(3.5)递归循环过程可以计算出在k时刻机器人的运动状态X(k)和环境地图m的联合后验概率分布 。这个递归循环是机器人运动模型 和观测模型 的一个更新过程。3.2卡尔曼滤波算法卡尔曼滤波算法(Kalman Filter)是一种高效的最优线性递归估计算法，KF算法主要用于线性系统估计，无论获得的数据是否精确，KF算法都能较好地估计出动态系统的运行状态。 假设某线性离散时间系统的状态方程和观测方程可用公式(3.6)和公式(3.7)表示: 其中，X(k)表示k时刻线性系统的状态向量，Z(k)表示k时刻线性系统的观测向量，U(k)表示k时刻线性系统的控制输入，(k)表示k时刻线性系统的系统噪声，v(k)表示k时刻线性系统的观测噪声，F表示线性系统的状态转移矩阵，B表示线性系统的输入控制矩阵，H表示线性系统的状态观测矩阵。假设(k)和v(k)为均值为零的高斯白噪声，于是有: 其中，Q(k)表示线性系统的过程噪声(k)的协方差矩阵，R(k)表示线性系统的观测噪声v(k)的协方差矩阵。 KF是一种典型的贝叶斯滤波算法，已知线性系统状态初始值协方差矩阵P(0 I 0)和k时刻的传感器的观测值Z(k)，再应用已给定的线性系统的运动模型公式(3.6)和观测模型公式(3.7)递推计算出k时刻线性系统的状态 和协方差P(k I k) ,以下各步骤给出了具体的计算过程: 首先，利用线性系统的k-1时刻的状态估计 与k-1时刻协方差矩阵P(k-11|k-1)预测k时刻的线性系统的状态 和协方差矩阵P(k I k 1)，可得: 其次，利用已经预测到的线性系统的协方差矩阵P(k I k -1)和观测噪声协方差R(k)来推导出卡尔曼增益W (k)即: 最后，利用已经预测到的线性系统的状态 和实际传感器观测值Z(k)更新线性系统的状态 ，同时能推导出对应的协方差P(k I k)，即: 卡尔曼滤波算法主要应用在线性系统中，但是在实际系统中，很多的系统模型是非线性的，即非线性系统。为了适用于非线性系统，在基本的卡尔曼滤波算法的基础之上发展了扩展卡尔曼滤波算法(EKF)，扩展卡尔曼滤波算法就能较好地处理非线性系统的系统估计和预测问题。3. 3打一展卡尔曼滤波定位算法3.3.1 EKF算法原理扩展卡尔曼滤波算法与卡尔曼滤波算法区别在于用 Taylor 级数展开项的一阶项来进行近似系统的非线性方程，将复杂系统的非线性函数，展开成 Taylor 级数的形式，并省略掉二次项以及以上的多次项，进而可以得到与非线性系统相似的线性化模型系统，最后根据获得近似的线性化模型按照标准的卡尔曼滤波算法进行系统的状态估计和协方差矩阵的预测。基于扩展卡尔曼滤波算法解决机器人在家庭环境中的 SLAM 问题，机器人的非线性离散时间系统的运动模型可以用公式(3.17)表示： 其中， X ( k)表示k 时刻非线性系统的状态向量， U ( k)表示k 时刻非线性系统的控制输入， ( k)表示k 时刻非线性系统的系统噪声，即零均值的高斯白噪声，其协方差矩阵为 Q ( k )。机器人的非线性离散时间系统的观测模型可以用公式(4.18)表示： 其中， Z ( k)表示k 时刻非线性系统的观测向量， ( k)表示k 时刻非线性系统的观测噪声，即零均值的高斯白噪声，其协方差矩阵为 R ( k )。应用扩展卡尔曼滤波算法对非线性离散时间系统的运动状态和观测状态的预测和估计过程如下：已知非线性离散时间系统状态初始值 、协方差矩阵 P (0 | 0)和 k 时刻的传感器的观测值 Z ( k)，再应用已给定的系统运动模型公式(3.12)和观测模型公式(4.13)递推计算出k 时刻非线性系统的状态X ( k | k)和协方差 P ( k | k )，以下各步骤给出了具体的计算过程。首先，利用非线性系统的 k-1时刻的状态估计 与 k-1时刻协方差矩阵 P ( k-1| k-1)进行预测k 时刻的非线性系统的状态、协方差矩阵 P ( k | k-1)和观测量 Z ( k | k-1)，可得：其次，利用已经预测到的非线性系统的协方差矩阵 P ( k | k-1)和观测噪声协方差R ( k )来推导出卡尔曼增益 W ( k)，即： 最后，利用已经预测到的非线性系统的状态 和实际传感器观测值 Z( k)更新非线性系统的状态 ，同时能推导出对应的协方差矩阵 P ( k | k )，即： 以上的各个步骤迭代更新，就实现了扩展卡尔曼滤波算法的估计和预测的过程。器人基于家庭自然环境特征地图利用扩展卡尔曼滤波定位算法的详细流程图如图所示3.3.2 EKF 算法定位模型基于环境特征的机器人 EKF 的定位系统中，系统状态为： 其中， 表示机器人的位姿状态。其中， 表示传感器能观测到的自然环境特征在全局坐标系中的位置坐标。 由前面已经推出的机器人系统运动模型可知，当给机器人施加一个控制输入 时，机器人的运动模型为：环境特征中的路标是固定不动的，有，由前面的公式确定激光传感器能