230 2012，48（8） Computer Engineering and Applications 计算机工程与应用 基于 RSSI 的机器人室内卡尔曼滤波定位算法研究 安 雷，张国良，汤文俊 AN Lei, ZHANG Guoliang, TANG Wenjun 第二炮兵工程学院 301 教研室，西安 710025 Teaching and Research Office 301, The Second Artillery Engineering College, Xi’an 710025, China AN Lei, ZHANG Guoliang, TANG Wenjun. Study of robot indoor Kalman filter positioning algorithm based on RSSI. Comput- er Engineering and Applications, 2012, 48（8）：230-232. Abstract：For the difficult problem of mobile robot positioning in indoor environment, this paper puts forward the Kalman filter posi- tioning algorithm based on RSSI. The positioning algorithm based on RSSI is utilized to forecast the location of user, which the Kalman filter is used to optimize, in order to enhance the performance and stability of indoor positioning system. Experimental results show that the Kalman filter positioning algorithm, which has the robustness, can effectively improve the locational precision of indoor positioning system, and that the prospective objective is reached. Key words：robot positioning; indoor environment; least squares method; Kalman filter 摘 要：针对移动机器人在室内环境中定位难的问题，提出了一种基于 RSSI（Receive Signal Strength Indicator）的卡尔曼滤波定 位算法。利用基于 RSSI 的定位方法估算用户的位置坐标，利用卡尔曼滤波算法对用户的估算位置坐标进行优化处理，以提高室 内定位系统的性能和稳定性。实验结果表明，卡尔曼滤波算法是鲁棒的，可以有效改善系统的定位精度，达到了预期的目的。 关键词：机器人定位；室内环境；最小二乘法；卡尔曼滤波 DOI：10.3778/j.issn.1002-8331.2012.08.065 文章编号：1002-8331（2012）08-0230-03 文献标识码：A 中图分类号：TP242 1 引言 在移动机器人的研究中，机器人的精确定位一直是研究 的热点问题，同时室内定位技术又是室内活动机器人研究的 难题。虽然 GPS 自建立以来就得到了广泛的推广和应用，成 为军事、交通、资源环境、农牧渔业等领域不可或缺的定位系 统，但是目前 GPS 仍无法在室内环境中获取较高的定位精度， 难以满足在室内环境下进行精确定位的要求。因此，针对室 内的环境，必须研究专门的定位方法来进行室内定位。 目前，常见的室内定位技术主要可以划分为两类：基于移 动设备的方法和基于网络的方法。基于网络的室内定位技术 主要包括基于 RSSI 的定位技术、基于信号到达时间（TOA）和 信号到达角度（AOA）的定位技术。ZigBee 作为一种新兴的无 线网络技术，与无线网卡、蓝牙、红外等通信技术相比，它的主 要优点是成本低、时延短、自组网、网络容量大等，它基于 RSSI 的定位技术降低了网路流量和通信延迟[1]。 本文主要研究基于 ZigBee 网络的 RSSI 的定位算法，以及 应用卡尔曼滤波（Kalman filter）对估算出的定位信息进行优 化的问题。首先，利用 ZigBee 网络读取定位节点和各个参考 节点之间的 RSSI 值，通过经验模型求取定位节点和各个参考 节点之间的距离，从而估算出定位节点的位置。然后，以估算 出的定位信息为观测值，考虑室内存在的各种干扰噪声，建立 卡尔曼滤波模型，进一步提高机器人室内定位的精度，从而得 到系统的最优估计。实验结果表明，该室内定位算法的有效 性，且卡尔曼滤波后定位精度有了明显提高。 2 系统架构 本文以家庭服务机器人为平台，选取机器人实验室作为 室内定位实验的场地。该实验室设置了家庭服务机器人工作 所需的基本环境，能够更好地验证定位的效果。实验场地的 长度为 17 米，宽度为 13 米，在坐标系下的布局如图 1 所示。 y o 图 1 实验室布局图 x 该定位系统是由 8 个参考节点和 1 个定位节点组成的 Zig- Bee 无线传感器网络，每个节点都是以德州仪器的 CC2430 芯 片为核心的 ZigBee 无线传感器模块。定位的基本原理是：已 知 ZigBee 无线传感器模块发射信号的功率，同时通过 CC2430 芯片内置的定位引擎可以读取 RSSI 值，进而可以通过建立 RSSI 值和距离的数学模型来确定两节点之间的距离，而参考 节点的坐标已知，因此可以依据参考节点和定位节点之间的 距离建立方程组，并求解出定位节点的坐标。该系统具有 8 个 参考为节点，可以有效提高利用最小二乘算法定位的精度，参 作者简介：安雷（1986—），男，硕士，主要研究领域为先进控制理论及应用、机器人路径规划与导航；张国良（1970—），男，博士，教授；汤文俊 （1986—），男，硕士。E-mail：xsal999@126.com 收稿日期：2010-10-08；修回日期：2010-12-03；CNKI 出版：2011-05-17；http://www.cnki.net/kcms/detail/11.2127.TP.20110517.1449.012.html 

安 雷，张国良，汤文俊：基于 RSSI 的机器人室内卡尔曼滤波定位算法研究 2012，48（8） 231 考节点在实验场地中的坐标如表 1 所示。 参考节点地址 0x0001 0x0010 0x0011 0x0100 参考节点地址 表 1 参考节点设置表 坐标/m （0，0，2） （6，0，2） （12，0，2） （12，8，2） 0x0101 0x0110 0x0111 0x1000 坐标/m （12，16，2） （6，16，2） （0，16，2） （0，8，2） 3 室内定位模型的建立 本文利用各个参考节点采集定位节点的RSSI值，依据RSSI 值和距离的特定关系，建立 RSSI 值和距离的数学模型为[1-2]： RSSI = -(10n lg d + A) （1） 式中，n 为信号传播常量；d 为距发射器的距离；A 为距离 1 米 时的 RSSI 值。 i 在得到定位节点和参考节点之间相对精确的距离 d 后， 利用测量学中的空间距离后方交会法[3-4]，那么定位节点一定 在以参考节点为球心，以上述方法求出的距离 d 为半径的球 面上，当得到定位节点与三个参考节点之间的距离时，就可以得 到三个球面，进而可以求出三个球面相交的点，即定位节点。 在实际应用中，为了得到更为精确的定位信息，引入了参 ) ， ) ，定位节点和参考节点之 数传输距离误差为 Dρ ，假设参考节点的空间位置为 (x 定位节点的空间位置为 (x u 间的距离为 d ，得到方程： ，y ，y ，z ，z u u i i i i i = (x 1 = (x 2 u 2 2 - z - x - y u - z u - y u - x )2 + Dρ )2 + (z 1 )2 + (z )2 + (y 1 )2 + (y ì d ï 1 ïïï d í ï  ïïï d î 为求解方程，至少需要 4 个参考节点，对公式（2）进行线性 ) 处展开成线性方程， )2 + Dρ )2 + Dρ )2 + (y )2 + (z = (x （2） - y - x - z n u 2 u n u n u n u ŷ u ẑ u u 化处理。将式（2）在近似位置 (x̂ 写成矩阵形式为： a a y2  a Dd é ù 1 ê ú Dd ê ú ê ú 2 êê úú  ê ú Dd ë û n Dx é ù u ú ê Dy ú ê ú ê u úú êê Dz u ú ê -Dρ ë û a é ê ê a ê ê x2 êê  ê ê a ë a a z2  a = y1 x1 z1 式 中 ，r̂ xn = (x yn zn )2 + (y - x̂ 1 ù ú ú 1 ú ú úú  ú ú 1 û - ŷ i - ŷ )/r̂ i u i ；a u = (z i i - ẑ u )/r̂ i u zi a yi = (y i ] -ρ T ，H = Dx =[ Dx u Dy u Dz u 写成下列形式： （3） = (x - x̂ xi Dd 2 i u  Dd ； )/r̂ i ] T ， n 1 ù ú ú 1 ú ú úú  ú ú 1 û ，则式（3）可 )2 ；a Dd 1 i u - ẑ )2 + (z 。 令 Dd =[ a a é ê ê a a ê ê x2 z2 êê   ê ê a a ë a a y2  a x1 y1 xn yn zn z1 Dd = HDx ，即 Dx = H -1Dd （4） 当参考节点多于 4 个时，方程（4）为矛盾方程。另一个计 算方法为： Dx =[H T H]-1H TDd （5） 上 述 等 式 的 求 解 可 以 采 用 最 小 二 乘 法（Least Squares Method，LSM）来进行迭代计算，即先从定位节点位置和传输 距离误差的大概值开始，然后逐步精确到计算结果满足测量 要求并将此值作为定位的最终结果。该方法求解的优点是在 利用计算机求解时，可以尽可能多地利用各种有价值的信息， 减小求解过程中引入的误差。 4 卡尔曼滤波模型的建立 基于 RSSI 定位的重点是距离的测量，而 RSSI 测距容易受 到干扰噪声的影响，上述方法已不能满足机器人对定位精度 的要求。卡尔曼滤波是高斯过程最优滤波的一种有效算法[5-6]， 当对象模型足够准确且系统状态和参数不发生突变时，性能 较好。因此，将卡尔曼滤波与上述方法相结合，利用卡尔曼滤 波器的滤波功能减小噪声对系统的影响，达到提高定位精度 的目的。 首先是根据上述的定位信息建立系统的位移和速度的状 态方程并进行离散化。定位系统状态方程为： X(k + 1) = AX(k) + W (k) S(k) = CX(k) + V(k) （6） （7） 式 中 ，状 态 向 量 X(k) 为 待 优 化 的 机 器 人 定 位 信 息 ， X(k) = é x ë 和 V x k T ，x 、V y k 、V z k ù V z û k 分别 V y k V x k 、y 、z y z k k k k k k 为 k 时刻机器人在坐标系中三个方向的位移和速度估计值；A 为 系 统 矩 阵 ；观 测 向 量 S(k) 为 观 测 的 机 器 人 定 位 信 息 ， S(k) = é s x ë k 为 k 时刻机器人在坐标系中三 T ，s x 、s y k 、s y k ù sz û k s y k k 个方向的位移的观测值；C 为输出矩阵；W (k) 和 V(k) 分别为 状 态 噪 声 和 观 测 噪 声 ，且 满 足 E[ ]V(k) = 0 ， E[W (k)W (k)T]= Q ，E[V(k)V(k)T]= R ，即 W (k) 和 V(k) 是相互 独立的零均值白噪声序列。 ]W (k) = E[ 0 ]X(0) = μ ]X(0) = E[[X(0) - μ 状态向量的初始值 X(0) 的统计特性给定为： E[ Var[ 卡尔曼滤波器计算分为[5]： （1）卡尔曼滤波器方程的预测过程： X̂ (k|k - 1) = AX̂ (k - 1|k - 1) P(k|k - 1) = AP(k - 1|k - 1)AT + Q ][X(0) - μ 0 ]T]= P 0 0 （2）卡尔曼滤波器方程的校正过程： K(k) = P(k|k - 1)C T[CP(k|k - 1)C T + R]-1 X̂ (k|k) = X(k - 1|k - 1) + K P(k|k) =[I - K(k)C]P(k|k - 1) 用户位置移动的三维模型为： k (S(k) - CX̂ (k|k - 1)) k x é ù ú ê ú ê y ú ê k ú ê ú ê z ú ê k ú ê úú êê V x k ê ú ê ú V y ú ê ú ê k ê ú V z ë û k = 1 0 0 0.1 é ê ê 0 0 1 0 ê ê 0 0 0 1 êê 1 0 0 0 ê ê 0 0 0 0 ê ê ë 0 0 0 0 0 0.1 0 0 1 0 0 ù ú ú 0 ú ú 0.1 úú 0 ú ú 0 ú ú û 1 x é ê ê y ê ê ê z ê êê V x ê ê ê ê V y ê ê V z ë ù k - 1 ú ú ú ú k - 1 ú ú k - 1 úú ú ú k - 1 ú ú ú k - 1 ú û k - 1 + é w x ê ê ê w y ê ê wz ê ê êê wV ê ê ê ê wV ê ê ê wV ë ù ú k - 1 ú ú k - 1 ú ú ú ú k - 1 úú ú x ú k - 1 ú ú y ú ú k - 1 ú û z k - 1 k x é ù ú ê ú ê y ú ê k ú ê ú ê z ú ê k ú ê úú êê V x k ê ú ê ú V y ú ê ú ê k ê ú V z ë û k + é ù v x ê ú k ú ê úú êê v y k ê ú ú ê vz ë û k é ù s x ê ú k ú ê úú êê s y k ê ú ú ê s z ë û k = 1 0 0 0 0 0 é ù ú ê 0 1 0 0 0 0 ú ê ë û 0 0 1 0 0 0 初值的选取： （8） （9） （10） （11） （12） （13） （14） 

232 2012，48（8） Computer Engineering and Applications 计算机工程与应用 X(0) = 0 ，P(0|0) = 20 é ê ê 0 ê ê 0 êê 0 ê ê 0 ê ê ë 0 0 20 0 0 0 0 0 0 20 0 0 0 0 0 0 20 0 0 0 0 0 0 20 0 0 ù ú ú 0 ú ú 0 úú 0 ú ú 0 ú ú û 20 由图 2、3 可知，经过卡尔曼滤波后的定位结果与未经卡尔 曼滤波优化的定位结果相比较，未经卡尔曼滤波时定位误差 在 2 米以内的概率为 80% ，而经过卡尔曼滤波后定位误差均 控制在 1 米以内，并且通过实验可知，定位的可重复性很高。 机器人的定位结果如图 4 所示。 5 实验结果与分析 在实验过程中，定位节点始终固定在机器人上，因此定位 节点的坐标 z 值固定。为了简化计算，假设定位节点的 z 轴坐 标为 0，即以定位节点所在的水平面为 xoy 平面建立三维直角 坐标系，在实验中分别对定位节点的 x 和 y 坐标进行分析。 该实验主要分为两部分，一是利用最小二乘法求取定位节点 坐标，二是利用卡尔曼滤波对定位结果进行优化。 为达到实时跟踪机器人运动的目的，ZigBee 模块每隔 0.1 秒往上位机发送一组定位数据，然后经过上位机的处理获得 机器人的定位信息。在实验中让机器人在每个已知实际定位 信息的采样点进行采样，并求得未经卡尔曼滤波优化的定位 信息以及经卡尔曼滤波优化的定位信息。为了验证系统在一 定的路径下的定位精度以及实验可重复性，机器人在给定的 实验室环境中运动。 x 方向的定位结果如图 2 所示，y 方向的 定位结果如图 3 所示。 10 8 6 4 2 0 －2 0 10 20 30 40 50 采样点个数 图 2 x 方向定位效果图 真实值 观测值 滤波值 10 8 6 4 2 0 / m 置 位 向 方 x / m 置 位 向 方 y －2 0 10 20 30 40 50 采样点个数 真实值 观测值 滤波值 60 70 80 60 70 80 10 8 6 4 2 0 / m 置 位 向 方 y －2 0 2 4 6 x 方向位置/m 8 图 4 机器人定位效果图 真实值 观测值 滤波值 10 6 结论 经过实验表明，在未使用卡尔曼滤波进行优化时，由于环 境中存在干扰噪声，从而导致 ZigBee 模块读取的 RSSI 值存在 较大的误差，因此在复杂的室内环境中，系统定位精度存在较 大的偏差。而在特定的室内环境下，只要选取合适的状态误 差阵和观测误差阵，用卡尔曼滤波对定位精度进行优化，定位 精度会得到明显改善。综上所述，卡尔曼滤波算法可以提高 机器人室内的定位精度。 本文主要是研究利用卡尔曼滤波对定位结果进行优化， 这对误差阵的设计具有较高的要求。为了进一步提高室内定 位的精度，今后主要尝试建立定位系统的卡尔曼滤波模型，同 时利用扩展卡尔曼滤波解决定位中存在的非线性问题，进而 得到更可靠的定位信息。 参考文献： [1] 章坚武，张璐，应瑛，等.基于 ZigBee 的 RSSI 测距研究[J].传感技术 学报，2009，22（2）：285-288. [2] 张明华，张申生，曹健.无线局域网中基于信号强度的室内定位[J]. 计算机科学，2007，34（6）：68-71. [3] 谢钢.GPS 原理与接收机设计[M].北京：电子工业出版社，2009. [4] 涂刚毅，金世俊，祝雪芬，等.三种GPS定位优化算法的实现及比较[J]. 中国惯性技术学报，2009，17（2）：170-174. [5] 秦永元，张洪，汪叔华.卡尔曼滤波与组合导航原理[M].西安：西北 工业大学出版社，1998. [6] 王志贤.最优状态估计与系统辨识[M].西安：西北工业大学出版 图 3 y 方向定位效果图 社，2004.