基于RSSI的机器人室内卡尔曼滤波定位算法

中北大学课程设计说明书2012-2013学年第一学期学院：信息与通信工程学院专业：信息对抗技术专业学生姓名：韩潇洒学号：0905094109课程设计题目：信息处理实践基于RSSI的机器人室内卡尔曼滤波定位算法起迄日期:8月20日~8月26日课程设计地点:校内指导教师:杨光、王鉴、李凯、王玉、姚金杰系主任：赵冬娥下达任务书日期:2012年8月20日中北大学课程设计任务书2012/2013学年第一学期学院：信息与通信工程学院专业：信息对抗技术专业学生姓名：韩潇洒学号：0905094109课程设计题目：信息处理实践基于RSSI的机器人室内卡尔曼滤波定位算法起迄日期:8月20日～8月26日课程设计地点:校内指导教师:杨光、王鉴、李凯、王玉、姚金杰系主任：赵冬娥下达任务书日期:2012年8月20日信息处理实践课程设计-1-课程设计任务书1．设计目的：1、通过本课程设计的学习，学生将复习所学的专业知识，使课堂学习的理论知识应用于实践，通过本课程设计的实践使学生具有一定的实践操作能力；2、掌握Matlab使用方法，能熟练运用该软件设计并完成相应的信息处理；3、通过信息处理实践的课程设计，掌握设计信息处理系统的思维方法和基本开发过程。2．设计内容和要求（包括原始数据、技术参数、条件、设计要求等）：一、学习Matlab软件1、掌握Matlab软件的安装；2、熟悉Matlab的运行环境；3、根据所设计系统的需要会合理设定系统定时窗口的各项参数；4、根据所设计系统的需要，优化模块设计最优系统。二、实践设计要求：1、根据所选题目，设计实现系统的原理框图；2、建立系统的模型；3、每人一组，写出设计报告；4、根据给定位置信息，绘制二维路线图；5、设计卡尔曼滤波算法；6、仿真卡尔曼滤波定位算法。三、参考题目基于RSSI的机器人室内卡尔曼滤波定位算法信息处理实践课程设计-2-课程设计任务书3．设计工作任务及工作量的要求〔包括课程设计计算说明书(论文)、图纸、实物样品等〕：每个同学独立完成自己的任务，每人写一份设计报告，在课程设计论文中写明自己设计的部分，给出设计结果。4．主要参考文献：1.樊昌信等.通信原理（第三版）.北京：国防工业出版社.20042.郑君里等.信号与系统（第二版）.北京：高等教育出版社.20003.安雷、张国良、汤文俊.基于RSSI的机器人室内卡尔曼滤波定位算法研究5．设计成果形式及要求：设计的原理框图，仿真模型。课程设计说明书6．工作计划及进度：8月20日～8月21日8月21日～8月22日8月22日～8月24日8月24日～8月26日8月26日相关资料查阅；相关软件的学习；方案设计，完成并实现信息处理；书写论文；答辩、成绩考核。系主任审查意见：签字：年月日信息处理实践课程设计-3-目录一、设计目的…………………………………………4二、设计任务与要求及软件介绍………………………4三、设计步骤……………………………………………51、实验预习…………………………………………52、系统原理图以及系统模型……………………………63、设计结果…………………………………………10四、设计总结…………………………………………16五、实验心得…………………………………………18六、参考文献…………………………………………18七、附录（源代码）……………………………………18信息处理实践课程设计-4-一、设计目的1、通过本课程设计的学习，学生将复习所学的专业知识，使课堂学习的理论知识应用于实践，通过本课程设计的实践使学生具有一定的实践操作能力；2、掌握Matlab使用方法，能熟练运用该软件设计并完成相应的信息处理；3、通过信息处理实践的课程设计，掌握设计信息处理系统的思维方法和基本开发过程。4、在上学期的基础上，使定位节点（机器人）在室内做匀速运动和匀加速运动，实现拉尔曼滤波定位算法。二、设计任务与要求及软件介绍任务：用RSSI方法实现机器人室内定位，继而进行卡尔曼滤波定位算法。要求：首先根据所选题目，设计实现系统的原理框图，并建立系统的模型：装用八个测量节点的实验室（长17m，宽7m）。其次用Matlab仿真二维路线图（,xsys），产生模拟距离数据dn,进而用定位算法得到机器人位置,xdyd,以及根据给定位置信息，绘制二维路线图。再次设计卡尔曼滤波算法，用Matlab仿真卡尔曼滤波定位算法，得到机器人位置,xfyf。最后写出设计报告。软件介绍：MATLAB是由美国mathworks公司发布的主要面对科学计算、可视化以及交互式程序设计的高科技计算环境。它将数值分析、矩阵计算、科学数据可视化以及非线性动态系统的建模和仿真等诸多强大功能集成在一个易于使用的视窗环境中，为科学研究、工程设计以及必须进行有效数值计算的众多科学领域提供了一种全面的解决方案，并在很大程度上摆脱了传统非交互式程序设计语言（如C、Fortran）的编辑模式，代表了当今国际科学计算软件的先进水平。MATLAB和Mathematica、Maple并称为三大数学软件。它在数学类科技应用软件中在数值计算方面首屈一指。MATLAB可以进行矩阵运算、绘制函数和数据、实现算法、创建用户界面、连信息处理实践课程设计-5-matlab开发工作界面接其他编程语言的程序等，主要应用于工程计算、控制设计、信号处理与通讯、图像处理、信号检测、金融建模设计与分析等领域。MATLAB的基本数据单位是矩阵，它的指令表达式与数学、工程中常用的形式十分相似，故用MATLAB来解算问题要比用C，FORTRAN等语言完成相同的事情简捷得多，并且MATLAB也吸收了像Maple等软件的优点，使MATLAB成为一个强大的数学软件。在新的版本中也加入了对C，FORTRAN，C++，JAVA的支持。可以直接调用,用户也可以将自己编写的实用程序导入到MATLAB函数库中方便自己以后调用，此外许多的MATLAB爱好者都编写了一些经典的程序，用户可以直接进行下载就可以用。三、设计步骤1、实验预习根据此次课程设计要求：首先，根据所选题目，设计实现系统的原理框图，并建立系统的模型。其次，查阅有关Matlab的相关书籍、文章以及网络资源。熟悉Matlab软件的使用。其次，根据系统原理图和系统模型，用Matlab仿真产生机器人二维路线图并获得仿真机器人位置数据[,]xsys，继而与节点结合产生距离数据dn，在用定位算法求得机器人位置数据[,]xdyd，并加入随机序列白噪声。最后，设计卡尔曼滤波算法。并用Matlab仿真卡尔曼滤波定位算法，对加有噪声[,]xdyd进行滤波。写出设计报告；3、系统原理框图以及系统模型信息处理实践课程设计-6-图1.系统原理框图该定位系统是由8个参考节点和1个定位节点组成的Zig-Bee无线传感器网络。如图2，室内节点布置俯视图，1—8为8个固定参考节点，还有一个定位节点。参考节点地址以及坐标见表一。该定位系统的工作过程，先通过定位节点采集8个参考节点的信号强度，根据信号强度以及相关无线电理论求得定位节点与8个参考节点之间的距离。然后利用距离信息和参考节点的位置建立定位模型，从而确定定位节点的位置。再者建立卡尔曼滤波模型，对定位节点的位置信息滤波处理。表一参考节点设置表参考节点地址坐标/m参考节点地址坐标/m0X0001(0,0,2)0X0101(16,6,2)0X0010(0,3.5,2)0X0110(16,3.5,2)0X0011(0,6,2)0X0111(16,0,2)0X0100(8,6,2)0X1000(8,0,2)室内定位模型建立本文利用各个参考节点采集定位节点的RSSI值，依据RSSI值和距离的特定关系，建立RSSI值和距离的数学模型为：(10lg)RSSIndA（1）Matlab仿真产生二维路径，即二维位置信息根据系统节点位置，获得距离信息根据系统节点和距离信息，用定位模型求解位置信息随机序列噪声获得加有噪声的位置信息通过卡尔曼滤波器，对位置信息处理信息处理实践课程设计-7-式中，n为信号传播常量；d为距发射器的距离；A为距离1米时的RSSI值。假设参考节点的空间位置为(,,)iiixyz，定位节点的空间位置为(,,)uuuxyz，定位节点和参考节点之间的距离为id，得到方程：222112222222222()()()()()()...()()()uikikukknnunknkdxxyyzzdxxyyzzdxxyyzz（2）为求解方程，至少需要4个参考节点，对公式（2）进行线性化处理。将式（2）在近似位置(,,)uuuxyz处展开成线性方程，写成矩阵形式为：11112222...............xyzuxyzuxnynznnuaaadxaaadxaaadx（3）式中，222()()()iiuiiuurxxyyzz；()/iuxiiaxxr；()/iyiiuayyr；()/uiziiazzr。令12...Tndddd，Tuuuxxyz，111222.........xyzxyzxnynznaaaaaaHaaa，则式（3）可写成下列形式：dHx，即1xHd（4）当参考节点多于4个时，方程（4）为矛盾方程。另一个计算方法为：1TTxHHHd（5）上述等式的求解可以采用最小二乘法（LeastSquaresMethod，LSM）来进行信息处理实践课程设计-8-迭代计算，即先从定位节点位置和传输距离误差的大概值开始，然后逐步精确到计算结果满足测量要求并将此值作为定位的最终结果。该方法求解的优点是在利用计算机求解时，可以尽可能多地利用各种有价值的信息，减小求解过程中引入的误差。卡尔曼滤波模型的建立基于RSSI定位的重点是距离的测量，而RSSI测距容易受到干扰噪声的影响，上述方法已不能满足机器人对定位精度的要求。卡尔曼滤波是高斯过程最优滤波的一种有效算法，当对象模型足够准确且系统状态和参数不发生突变时，性能较好。因此，将卡尔曼滤波与上述方法相结合，利用卡尔曼滤波器的滤波功能减小噪声对系统的影响，达到提高定位精度的目的。首先是根据上述的定位信息建立系统的位移和速度的状态方程并进行离散化。定位系统状态方程为：(1)()()XkAXkWk（6）()()()SkCXkVk（7）式中，状态向量()Xk为待优化的机器人定位信息，()TxyzkkkkkkXkxyzVVV，kkkxyz、、和xyzVVkkkz、、分别为k时刻机器人在坐标系中三个方向的位移和速度估计值；A为系统矩阵；观测向量S(k)为观测的机器人定位信息，S(k)=Txyzkkksss，zksxykkss、、为k时刻机器人在坐标系中三个方向的位移的观测值；C为输出矩阵；()Wk和()Vk分别为状态噪声和观测噪声，且满足()()0EWkEVk，(k)(k)=TEWWQ，V(k)V(k)=TER，即W(k)和V(k)是相互独立的零均值白噪声序列。状态向量的初始值X(0)的统计特性给定为：0(0)=EX000(0)(0)(0)TVarXEXXP卡尔曼滤波器计算分为：（1）卡尔曼滤波器方程的预测过程：信息处理实践课程设计-9-(|1)(1|1)XkkAXkk（8）(|1)(1|1)TPkkAPkkAQ（9）（2）卡尔曼滤波器方程的校正过程：1()(|1)(|1)TTKkPkkCCPkkCR（10）(|)(1|1)(()(|1))kXkkXkkKSkCXkk（11）(|)()(|1)PkkIKkCPkk（12）用户位置移动的三维模型为：1111111