CN112584306A - 一种基于卡尔曼滤波的室内机器人定位算法 - Google Patents

Abstract

本发明涉及一种基于卡尔曼滤波的室内机器人定位算法，包括如下步骤：1)建距离模型；2)基于卡尔曼滤波室内机器人定位模型：2.1)卡尔曼滤波过程迭代执行；2.2)卡尔曼滤波的工作；3)卡尔曼滤波室内机器人定位算法优化滤波参数，当网络初始化后，读取相应参数，这是反馈系统的输入，之后求取观测值，实施卡尔曼滤波算法，最后输出最优状态预估值。本发明提出的基于卡尔曼滤波算法，选取恰当的状态误差矩阵及观测误差矩阵，优化定位精度，提高室内机器人的定位性能。

Description

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

技术领域

本发明涉及室内机器人定位算法，具体涉及一种基于卡尔曼滤波的室内机器人定位算法。

背景技术

随着我国科技实力的不断增强及人民生活水平的不断提高，未来将出现一批应用于专业服务领域的机器人和应用于家庭领域的机器人，机器人市场将日渐繁荣兴旺，针对移动机器人在室内环境中定位难的问题。对于定位技术可谓是种类繁多，但是各种技术之间还是有较大差别。

Wi-Fi定位技术应用，在被监测区域内安装无线基站，根据未知节点Wi-Fi设备的信号特征，综合均衡无线基站的拓扑结构，推算并确定未知节点Wi-Fi设备的位置坐标。Wi-Fi定位技术轻松实现定位功能，基于对现有的无线设备充分利用，但是由于Wi-Fi的保密性差，能量消耗又较高，频谱资源利用已经趋于饱和，因此，终端设备无法实现长期携带，以及大规模网络的应用场景。

射频识别(RFID)技术是基于电磁感应原理，通过无线激活射频范围内无线标签，实现对信息进行采集获取的技术，可以实现距离范围从几厘米到十几米之间的射频识别。RFID技术应用识别有典型的员工考勤系统，主要负责辨识人员是否存在于某个区域，但是实现实时跟踪有难度，并且开放的标准化网络体系结构还有待进一步完善，因此，不适用于大型设备的巡检，人员安全的确认等用途。

超宽带(Ultra Wideband，UWB)，是一种可以实现高精度定位的无线载波通信技术，一般由UWB无线射频基站、射频标签、定位系统引擎和应用系统四个部分构成，通常利用到达时间差(TDOA)和时间到达(TOF)算法实现人员或者物品位置信息的定位算法。此外，对所需定位的物品或者人员UWB技术需要铺设基站和佩戴标签。UWB技术最高可实现厘米级的定位水平，但是定位成本很高，适用于定位面积小，高经济附加值的行业，在工厂领域动辄几百万的投入，和对人的厘米级定位有些“大才小用”。

ZigBee技术使用的标准基于IEEE802.15.4网络协议体系结构。它的显著特点是通信距离短、网络复杂度低、自组织网络、能量消耗低，数据传输速率不高。对于自动控制和远程控制领域场景应用比较适合。正是其在工业领域的特点，其干扰性、低功耗、全双工、无线等显著特征，就具备了WSN定位系统的基础性能，其在无线通信协议栈和数据融合算法处理后可实现定位性能比极高的标签定位系统。定位精度可达3m以内，局部区域精度1m以内，完全满足工厂领域在成本控制前提下对人员的定位需求。

传统定位技术运用在室内机器人定位中普遍存在以下问题：

(1)机器人惯性定位算法，受系统模型非线性程度的影响，准确性不高。

(2)基于RSSI定位算法，主要是依赖它所测量得到的信号强度值，很难避免外界噪声等诸多因素的干扰，而产生的误差。

(3)实现实时优化滤波的各项参数，解决过程状态不能直接观测的难度。

卡尔曼提出的递推最优估计理论，采用状态空间为载体描述估计方法，与其他算法不同，采用递推的形式，卡尔曼滤波能够控制和处理随机过程，可以是多维的，或者是不平衡。并且使估计值和真实值两者之间无限接近，这就是所谓的最优估计。真实值与估计值两者之间的差值被称为估计误差。如果两者之间的差值为零，那么这种估计就被称为无偏估计。Kalman滤波的最优估计是要求达到三个条件的，首先，无偏性。即估计值的均值等于监测对象状态的真实值，并且要求能够达到实时性。其次，估计的方差最小。卡尔曼滤波就是在有随机干扰和噪声的影响情况下，基于线性最小方差估计方法给出监测对象状态的最优估计值，卡尔曼滤波是在统计学的基础上推断出最接近监测对象状态真值的估计值。第三，实时性。由于卡尔曼滤波算法的实时性较好，因此在空间技术、导航、测量轨迹、拦截与通讯等方面已经获得了比较深入的广泛应用。

在无线传感器网络环境下，针对移动机器人定位存在不足问题，在优化的定位算法中，首先对机器人的预估位置实施优化，数据融合时对移动机器人的观测、机器人自身运动控制量，采用带阈值判别和选择高斯分割的方法；其次在计算求解时降低空间维数，提高定位精度。为解决当前工业机器人定位误差大的问题,多维尺度模型的定位精度受噪声影响,现有定位方法较难达到满足要求的定位精度,将卡尔曼滤波和多目标跟踪相结合的方法进行数据融合.建立高斯分布下的预测模型,建立线性跟踪系统。得到了较好的定位效果。现有技术中，有提出一种基于行人轨迹预测的全向移动机器人路径规划算法，主要解决传统全向移动机器人局部路径规划算法无法提前对行人轨迹进行预测，无法二次避障和无局部最优解，而导致的急停和智能性差的问题。有利用卡尔曼滤波算法对行人下一刻全局坐标进行预测，并根据预测结果调整机器人局部路径规划，最后进行行人与机器人交叉的综合实验，算法可行且智能性较高。有提出一种改进的粒子滤波机器人姿态解算方法，基于扩展卡尔曼滤波算法对预测系统状态,主要是针对传统滤波方法在非线性系统模型下误差大的问题。不同的地面环境下系统噪声有较大变化,是将监测到的地面环境信息视为观测信息在系统中进行融合处理,并能够实现实时修正算法中的参数，对粒子滤波算法的重采样过程进行优化,在机器人惯性导航研究中改进算法粒子滤波精度较高且不受系统模型非线性程度的影响，应用此算法进行姿态解算精度较高,且性能优异。

发明内容

本发明目的是提供一种基于卡尔曼滤波的室内机器人定位算法，本发明提出的基于卡尔曼滤波算法，选取恰当的状态误差矩阵及观测误差矩阵，优化定位精度，提高室内机器人的定位性能。

为了实现以上目的，本发明采用的技术方案为：一种基于卡尔曼滤波的室内机器人定位算法，包括如下步骤：

1)建距离模型，包括如下步骤：

建立RSSI值和距离相关联的数学模型为：

RSSI＝-(10nlgd+A) (1)

式(1)中，n表示信号传播的一个常量；d表示未知节点与发射器之间的距离；A表示无线收发节点相距为1m时接收器收到的无线信号强度RSSI的值；

上式(2)中，n表示信标节点个数，(x₁，y₁，z₁)，(x₂，y₂，z₂)，…，(x_n，y_n，z_n)分别表示这些信标节点的坐标；(x_u，y_u，z_u)表示未知节点U的坐标；d₁，d₂，…，d_n表示未知节点U和n个信标节点之间的距离；

信标节点至少4个，线性化处理式(2)得：

在式(3)中，[Δd₁ Δd₂…Δd_n]^T表示实际距离与估计距离的变化量，未知节点U估计位置坐标为

线性化展开成矩阵如式(4)：

上式(4)中，

令Δd＝[Δd₁ Δd₂ ... Δd_n]^T，得：

化简式(5)：

Δd＝HΔx

Δx＝H^-1Δd (6)；

2)基于卡尔曼滤波室内机器人定位模型，采用如下步骤：

2.1)卡尔曼滤波过程迭代执行：根据当前状态和系统方程估算下一状态；获取下一状态的观测结果；使用当前卡尔曼增益加权平均更新估计值；更新卡尔曼增益；

建立离散化的系统状态方程，涵盖了位移和速度，如下公式所示：

X(k+1)＝AX(k)+W(k) (8)

S(k)＝CX(k)+v(k) (9)

式(8)、式(9)中，X(k)表示状态向量，它为待优化的机器人定位信息，A表示系统矩阵；S(k)表示观测向量，它们是观测机器人位置信息；C表示输出矩阵；W(k)和v(k)分别为状态噪声和观测噪声，且满足:

式(10)中，R表示m×m维量测噪声序列v(k)的方差阵，v(k)^T表示v(k)的转置矩阵，即W(k)和V(k)表示互相独立的白噪声序列,且它的均值为零，系统初始状态X(0)的均值向量以及方差阵如式(11)所示：

E[X(0)]＝μ₀ (11)

E[[X(0)-μ₀][X(0)-μ₀]^T]＝P₀ (12)

上式(12)中，P表示误差协方差，P₀是它的初始值；

2.2)卡尔曼滤波的工作流程包括如下分步骤：

2.2.1)Kalman滤波预测方程：

上式(13)中，X(k|k-1)表示当前时刻(k)系统状态预测值，

表示上一时刻(k-1)系统状态的最优值；

P(k|k-1)＝AP(k-1|k-1)A^T+Q (14)

上式(14)中，P(k|k-1)表示X(k|k-1)状态对应的协方差矩阵，P(k-1|k-1)表示X(k-1|k-1)对应的协方差矩阵，A表示系统矩阵；A^T表示A的转置矩阵，Q表示系统工作过程的协方差矩阵；

2.2.2)Kalman滤波方程校正过程：

卡尔曼滤波增益矩阵：

K(k)＝P(k|k-1)C^T[CP(k|k-1)C^T+R]^-1 (15)

上式(15)中，K(k)表示k时刻的卡尔曼增益矩阵，是使状态估计值

的均方误差值最小；R表示观测方程中V(k)的方差矩阵；P(k|k-1)表示k时刻和(k-1)时刻预测协方差矩阵；C为输出矩阵，C^T表示C的转置矩阵；

K时刻状态更新：

上式(16)中，

X(k-1|k-1)分别表示输出(k)和(k-1)时刻的状态向量；S(k)表示k时刻RSSI信号的测量值；C为输出矩阵，此时，若对象模型准确度高且系统状态和参数没有发生突变时，卡尔曼滤波的性能好；

估计协方差矩阵P(k|k)更新:

P(k|k)＝[I-K(k)C]P(k|k-1)(I-K(k)C)^T+K(k)RK(k)^T (17)

化简为：

P(k|k)＝[I-K(k)C]P(k|k-1) (18)

式(17)、式(18)中，P(k|k)表示输出滤波协方差；I表示n×n阶单位矩阵；

用户位置移动的三维模型为：

上式(19)中，

和

分别表示k时刻、(k-1)时刻机器人在空间坐标系中估计的三个运动方向的位移和速度；

分别代表(k-1)时刻机器人在空间坐标系中估计的三个运动方向位移噪声和运行速度噪声；

上式(20)中，

表示机器人在k时刻居于空间坐标系中三个方向的位移向量；

表示机器人在k时刻在空间三个方向的运动速度向量；

分别代表k时刻机器人在空间坐标系中估计的三个运动方向的位移和速度向量；

初值选取：

机器人坐标转移矩阵：

上式(22)中,(x，y，θ)表示机器人在时间点t-1时刻的位置,(v_t，+ω_t)表示机器人运动的线速度和角速度,(x′，y′，θ′)表示机器人按弧线轨迹移动到当前时刻t时新的位置；

2.2.3)预处理加速度：

对加速度进行预处理，即任意时刻三轴总加速度表示为：

上式(23)中，a_i表示第i个采样点的加速度，a_ix、a_iy、a_iz分别表示加速度计X轴、Y轴，Z轴上的对应值，去除加速度采样值中的重力加速度值，得下式：

上式(24)中，a_g表示重力加速度，N表示采样点数，2N+1表示滑动窗口的大小；

2.2.4)步长估计模型：

本步骤算法中采用线性模型估计步长，如下式：

上式(25)中，i＝1,2,3,…,n表示实验次数，F表示实验中采用的频率，S表示每次实验中取得的步长值；a和b都是常量系数；

步长用矩阵表示为：

S＝AK (26)

上式(26)中，

由最小二乘法，可得：

K＝(A^TA)^-1(A^TS) (27)

3)卡尔曼滤波室内机器人定位算法优化滤波参数，当网络初始化后，读取相应参数，这是反馈系统的输入，之后求取观测值，实施卡尔曼滤波算法，最后输出最优状态预估值。

进一步的，步骤1)中，当信标节点数量多于4时，公式(6)即为矛盾模型：采用另一种计算模型，当HH^T为非奇异矩阵时，Δx为：

Δx＝[H^TH]^-1H^TΔd (7)

通过未知节点位置以及估计传输距离误差两个方面着手，然后逐步优化计算结果直到达到测量标准条件，此值也是定位的最终结果。

本发明的技术效果在于：(1)卡尔曼滤波是一种基于高斯过程最优的滤波算法，当对象模型能够达到足够准确，并且系统状态和状态参数比较稳定，几乎不会发生突变，滤波性能必然较好。因此，将Kalman滤波与RSSI方法协同，充分发挥两种技术的优势，在采用RSSI获得距离的同时，利用Kalman滤波的功能去除系统中由于其他因素干扰而产生的定位误差，可以达到提高室内机器人定位精度的目标。

(2)用卡尔曼滤波算法，时间更新方程向前推算k时刻(当前时刻)状态向量和误差协方差估计的值，为k+1时刻(下一时刻)状态构成先验估计。

(3)用卡尔曼滤波算法，更新测量方程综合先验估计和新的测量变量，成为优化后的后验估计。

通过本发明以下步骤，提高室内机器人定位精度。

附图说明

图1为本发明应用到的状态估计向量的运算框图；

图2为本发明应用到的误差协方差的运算框图；

图3为本发明卡尔曼滤波室内机器人定位流程图；

图4为本发明卡尔曼滤波子程序流程图；

图5为本发明实施例中机器人匀速运动线路图；

图6为本发明实施例中观测位置信息图；

图7为本发明实施例中匀速运动X方向定位效果图；

图8为本发明实施例中匀速运动y方向定位效果图；

图9为本发明实施例中机器人匀加速运动真实、观测位置信息线路图；

图10为本发明实施例中机器人匀加速运动x、y观测位置信息图；

图11为本发明实施例中X方向定位效果图；

图12为本发明实施例中Y方向定位效果图；

图13为本发明实施例中定位系统的预测值图。

具体实施方式

下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的一种基于卡尔曼滤波的室内机器人定位算法的有益效果：

(1)卡尔曼滤波是一种基于高斯过程最优的滤波算法，当对象模型能够达到足够准确，并且系统状态和状态参数比较稳定，几乎不会发生突变，滤波性能必然较好。因此，将Kalman滤波与RSSI方法协同，充分发挥两种技术的优势，在采用RSSI获得距离的同时，利用Kalman滤波的功能去除系统中由于其他因素干扰而产生的定位误差，可以达到提高室内机器人定位精度的目标。

(2)用卡尔曼滤波算法，时间更新方程向前推算k时刻(当前时刻)状态向量和误差协方差估计的值，为k+1时刻(下一时刻)状态构成先验估计。

(3)用卡尔曼滤波算法，更新测量方程综合先验估计和新的测量变量，成为优化后的后验估计。

通过本发明以下步骤，提高室内机器人定位精度。

一种基于卡尔曼滤波的室内机器人定位算法，包括如下步骤：

1)建距离模型：

建立RSSI值和距离相关联的数学模型为：

RSSI＝-(10nlgd+A) (1)

式(1)中，n表示信号传播的一个常量；d表示未知节点与发射器之间的距离；A表示无线收发节点相距为1m时接收器收到的无线信号强度RSSI的值；

上式(2)中，n表示信标节点个数，(x₁，y₁，z₁)，(x₂，y₂，z₂)，…，(x_n，y_n，z_n)分别表示这些信标节点的坐标；(x_u，y_u，z_u)表示未知节点U的坐标；d₁，d₂，…，d_n表示未知节点U和n个信标节点之间的距离；

求解上式方程组，信标节点至少4个，线性化处理式(2)得：

在式(3)中，[Δd₁ Δd₂ … Δd_n]^T表示实际距离与估计距离的变化量，未知节点U估计位置坐标为

线性化展开成矩阵如式(4)所示：

上式(4)中，

令Δd＝[Δd₁ Δd₂ ... Δd_n]^T，得：

化简式(5)：

Δd＝HΔx

Δx＝H^-1Δd (6)

当信标节点数量多于4时，公式(6)即为矛盾模型，

采用另一种计算模型，当HH^T为非奇异矩阵时，Δx为：

Δx＝[H^TH]^-1H^TΔd (7)

通过未知节点位置以及估计传输距离误差两个方面着手，然后逐步优化计算结果直到达到测量标准条件，此值也是定位的最终结果。

采用这种方法计算，在计算过程中可以最大化地综合各种有价值的信息，从而减小求解过程中产生的误差，保证高性能的定位效果。

2)基于卡尔曼滤波室内机器人定位模型：

基于RSSI定位思想，依赖它测量得到的信号强度值，从而根据信号强度值计算出距离，但是RSSI测距很难避免外界噪声等干扰因素，而产生影响定位性能。

即仅通过上述方法很难满足室内机器人定位性能的要求。

卡尔曼滤波是一种基于高斯过程最优的滤波算法，如果对象模型能够达到足够准确，并且系统状态和状态参数比较稳定，很难发生突变，滤波性能必然较好。因此，将Kalman滤波与RSSI方法相协同，充分发挥两种技术的优势，在采用RSSI获得距离的同时，利用Kalman滤波的功能去除系统中由于其他因素干扰而产生的定位误差，可以达到提高定位精度的目标。

具体步骤如下：

2.1)卡尔曼滤波过程迭代执行：根据当前状态和系统方程估算下一状态；获取下一状态的观测结果；使用当前卡尔曼增益加权平均更新估计值；更新卡尔曼增益；

建立离散化的系统状态方程，涵盖了位移和速度，如下公式所示：

X(k+1)＝AX(k)+W(k) (8)

S(k)＝CX(k)+v(k) (9)

式(8)、式(9)中，X(k)表示状态向量，它为待优化的机器人定位信息，[x_k y_k z_k V_k ^xV_k ^y V_k ^z]^T分别代表k时刻(当前时刻)机器人在空间坐标系中估计三个运动方向的位移和速度；A表示系统矩阵；S(k)表示观测向量，它们是观测机器人位置信息；C表示输出矩阵；W(k)和v(k)分别为状态噪声和观测噪声，且满足:

式(10)中，R表示m×m维量测噪声序列v(k)的方差阵，v(k)^T表示v(k)的转置矩阵，即W(k)和V(k)表示互相独立的白噪声序列,且它的均值为零，系统初始状态X(0)的均值向量以及方差阵如式(11)所示：

E[X(0)]＝μ₀ (11)

E[[X(0)-μ₀][X(0)-μ₀]^T]＝P₀ (12)

上式(12)中，P表示误差协方差，P₀是它的初始值；

2.2)卡尔曼滤波的工作流程包括如下分步骤：

2.2.1)Kalman滤波预测方程：

上式(13)中，X(k|k-1)表示当前时刻(k)系统状态预测值，

表示上一时刻(k-1)系统状态的最优值；

P(k|k-1)＝AP(k-1|k-1)A^T+Q (14)

上式(14)中，P(k|k-1)表示X(k|k-1)状态对应的协方差矩阵，同理可得，P(k-1|k-1)表示X(k-1|k-1)对应的协方差矩阵，A表示系统矩阵；A^T表示A的转置矩阵，Q表示系统工作过程的协方差矩阵；

2.2.2)Kalman滤波方程校正过程：

卡尔曼滤波增益矩阵：

K(k)＝P(k|k-1)C^T[CP(k|k-1)C^T+R]^-1 (15)

上式(15)中，K(k)表示k时刻的卡尔曼增益矩阵，是使状态估计值

的均方误差值最小；R表示观测方程中V(k)的方差矩阵；P(k|k-1)表示k时刻和(k-1)时刻预测协方差矩阵；C为输出矩阵，C^T表示C的转置矩阵；

K时刻状态更新：

上式(16)中，

X(k-1|k-1)分别表示输出(k)和(k-1)时刻的状态向量；S(k)表示k时刻RSSI信号的测量值；C为输出矩阵，此时，若对象模型准确度高且系统状态和参数没有发生突变时，卡尔曼滤波的性能较好；

估计协方差矩阵P(k|k)更新:

P(k|k)＝[I-K(k)C]P(k|k-1)(I-K(k)C)^T+K(k)RK(k)^T (17)

化简为：

P(k|k)＝[I-K(k)C]P(k|k-1) (18)

式(17)、式(18)中，P(k|k)表示输出滤波协方差；I表示n×n阶单位矩阵；

用户位置移动的三维模型为：

上式(19)中，

和

分别表示k时刻(当前时刻)、(k-1)时刻机器人在空间坐标系中估计的三个运动方向的位移和速度；

分别代表(k-1)时刻机器人在空间坐标系中估计的三个运动方向位移噪声和运行速度噪声；

上式(20)中，

表示机器人在k时刻居于空间坐标系中三个方向的位移向量；

表示机器人在k时刻在空间三个方向的运动速度向量；

分别代表k时刻机器人在空间坐标系中估计的三个运动方向的位移和速度向量；

初值选取：

卡尔曼滤波算法的一个优点是k时刻(当前时刻)的值仅于k-1时刻(前一时刻)的值相关，由k-1时刻推测k时刻的值，并考虑被估测值的方差在系统反馈中做修正。计算机内存中不会存储大量冗余数据，计算效率提有所提高；

机器人坐标转移矩阵：

上式(22)中,(x，y，θ)表示机器人在时间点t-1时刻的位置,(v_t，+ω_t)表示机器人运动的线速度和角速度,(x′，y′，θ′)表示机器人按弧线轨迹移动到当前时刻t时新的位置；

2.2.3)预处理加速度：

机器人运动各轴上的加速度由于外界噪声等干扰可能会出现误判情况，因此对加速度进行预处理，即任意时刻三轴总加速度表示为：

上式(23)中，a_i表示第i个采样点的加速度，a_ix、a_iy、a_iz分别表示加速度计X轴、Y轴，Z轴上的对应值，去除加速度采样值中的重力加速度值，得下式：

上式(24)中，a_g表示重力加速度，N表示采样点数，2N+1表示滑动窗口的大小；

2.2.4)步长估计模型：

本步骤算法中采用线性模型估计步长，如下式：

上式(25)中，i＝1,2,3,…,n表示实验次数，F表示实验中采用的频率，S表示每次实验中取得的步长值；a和b都是常量系数；

步长用矩阵表示为：

S＝AK (26)

上式(26)中，

由最小二乘法，可得：

K＝(A^TA)^-1(A^TS) (27)

3)卡尔曼滤波室内机器人定位算法流程：

3.1)算法描述：

参照状态估计向量的运算框图，如图1所示；

3.2)误差协方差的运算框图

参照误差协方差的运算框图，如图2所示：

3.3)优化滤波参数：

在实际中实现卡尔曼滤波算法时，一般通过观测可以测量获取噪声，它是进行滤波的条件；

通过离线实验方式来获取观测测量噪声协方差，但是确定过程激励噪声协方差值有一定难度，因为过程状态很难直接观测到，当然，可以通过人工干预的方式手动选择，人为地引入达到一个不确定因素，创立一个可以满足条件的结果。

3.4)绘制程序流程图；

如图3所示，基于卡尔曼滤波的室内机器人定位流程图，当网络初始化后，读取相应参数，这是反馈系统的输入，之后求取观测值，实施卡尔曼滤波算法，最后输出最优状态预估值。

本实施例仿真实验使用MATLAB平台，仿真实验设定在一个20m×20m的正方形平面区域进行，采用如下步骤：

步骤1):机器人的角度为θ，以X轴正向为0度，逆时针旋转，该定位系统的工作过程，先通过未知节点采集8个信标节点的RSSI值，通过RSSI值和无线电相关理论能够求解出未知节点与8个信标节点之间的距离；

步骤2):建立定位模型，通过距离信息和信标节点的位置信息，从而推算出未知节点的位置信息；

步骤3):建立卡尔曼滤波模型，滤波处理未知节点的位置信息。

再进行如下步骤：

1)机器人匀速运动状态仿真结果与分析：

机器人定位算法包括机器人位置坐标和运动方向两个部分；从图5中,机器人从点(0,3)沿直线运动到点(14,3)，运动方位角θ＝0°，很明显的看出来，经过Kalman滤波算法融合处理过的加速度值与加速度采集值的区别很鲜明，得到的数据相比于测量数据更稳定，由于移动物体姿态放置有区别，所以加速度受到重力影响，因而测量值为一个重力加速度g，很清楚地观测到初始状态由于对真实目标状态估计不佳，误差较大，然而，随着采样次数的增加，误差逐渐减小，卡尔曼滤波靠近真实运行轨道。

图6是仿真产生的x、y方向的观测位置信息图，它是机器人室内二维路线图，展示了机器人匀速运动真实位置和观测位置信息，将铺着鹅卵石地面作为实验平台，设置好全局坐标，并且将机器人的起始航向角状态设置为0，再由编码器让机器人实现直线运动和转弯运动，在机器人的行走运动过程中，由于无线通信、外界噪声、里程计等因素影响产生误差，导致机器人实际运动轨迹与预设定的行走轨迹产生偏差。

如图7所示，在有效探测距离内，跟踪效果好，对比预估机器人运动轨迹曲线和机器人实际所处的位置，结果是比较准确的,基本能够反映机器人此刻的真实运动状态，包括位置和运动方向。

如图8所示，由于在Y方向上存在速度分量，导致在Y方向的估计误差均值波动略大于X方向的估计误差值，达到真实运动轨迹附近之后，误差水平保持在常值附近，从而看出卡尔曼滤波器对于机器人目标运动轨迹的估计预测确实达到较好的效果。

2)机器人匀加速运动状态仿真结果与分析：

如图9所示，卡尔曼滤波靠近真实运行轨道，室内机器人在匀加速运动状态下仿真产生的二维路线图，它是机器人匀加速运动真实、观测位置信息。

如图10所示，因为在Y方向上存在速度分量，所以导致Y方向的估计误差均值大于X方向的估计误差，达到真实运动轨迹附近之后，误差水平保持在0附近，从而可以看出卡尔曼滤波器对于机动目标运动轨迹的估计预测确实达到比较好的效果，由图可看出,X方向和Y方向上的滤波误差标准差，估计的机器人运动状态曲线对比机器人真实位置，结果能够预估的效果,能够基本反映机器人真实位置和状态，Y方向定位的在曲线存在一些定位误差,导致产生这些误差的主要原因是移动机器人失去了观测信息。

由图11、图12可看出,X方向和Y方向的定位效果，预估计的运动轨迹曲线是比较准确的,基本能够反映机器人真实位置和运动状态,用卡尔曼滤波算法，并用其对观测位置信息进行滤波，采用多传感器信息融合技术，主要优点是在于室内机器人运动观测信息的重复性和互补性上可以提供更好的性能，采用等间隔分布的声纳来测定机器人周围障碍物的距离，进行多传感器信息融合，根据多传感器融合数据建立机器人的导航图谱，实现室内机器人运动准确定位。

从图12中看出,卡尔曼滤波靠近真实运行轨道，达到真实运动轨迹附近之后，误差水平保持在0附近，从而看出卡尔曼滤波器对于机动目标运动轨迹的估计预测确实达到比较好的效果，经过研究并用仿真表明，在没有使用卡尔曼滤波对系统做出优化时，因为室内环境有大量干扰噪声的影响，RSSI值误差会非常大，室内环境越复杂混乱，系统定位精度偏差也会非常大，给定一个室内环境，状态误差矩阵及观测误差矩阵的恰当选取，定位精度可以被卡尔曼滤波优化，改善效果明显。

从图13定位系统的预测值，可见，经过采样次数增加，系统的误差近趋于O。

本实施例提出的基于Kalman滤波的室内机器人定位算法，正确性通过Floyd良序集方法证明，Floyd不变式断言法可以证明该算法的部分正确性，运用良序集法可以证明该算法的终止性，这个算法是部分正确且是可终止的，结论证实该算法是完全正确的，该定位算法的时间复杂度是O(n³)，空间复杂度是O(n)。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于卡尔曼滤波的室内机器人定位算法，其特征在于，包括如下步骤：

1)建距离模型，包括如下步骤：

建立RSSI值和距离相关联的数学模型为：

RSSI＝-(10n lg d+A) (1)

式(1)中，n表示信号传播的一个常量；d表示未知节点与发射器之间的距离；A表示无线收发节点相距为1m时接收器收到的无线信号强度RSSI的值；

上式(2)中，n表示信标节点个数，(x₁，y₁，z₁)，(x₂，y₂，z₂)，…，(x_n，y_n，z_n)分别表示这些信标节点的坐标；(x_u，y_u，z_u)表示未知节点U的坐标；d₁，d₂，…，d_n表示未知节点U和n个信标节点之间的距离；

信标节点至少4个，线性化处理式(2)得：

在式(3)中，[Δd₁ Δd₂ … Δd_n]^T表示实际距离与估计距离的变化量，未知节点U估计位置坐标为

线性化展开成矩阵如式(4)：

上式(4)中，

令Δd＝[Δd₁ Δd₂ ... Δd_n]^T，得：

Δx＝[Δx_u Δy_u Δz_u]^T，

化简式(5)：

Δd＝HΔx

Δx＝H^-1Δd (6)；

2)基于卡尔曼滤波室内机器人定位模型，采用如下步骤：

2.1)卡尔曼滤波过程迭代执行：根据当前状态和系统方程估算下一状态；获取下一状态的观测结果；使用当前卡尔曼增益加权平均更新估计值；更新卡尔曼增益；

建立离散化的系统状态方程，涵盖了位移和速度，如下公式所示：

X(k+1)＝AX(k)+W(k) (8)

S(k)＝CX(k)+v(k) (9)

式(8)、式(9)中，X(k)表示状态向量，它为待优化的机器人定位信息，A表示系统矩阵；S(k)表示观测向量，它们是观测机器人位置信息；C表示输出矩阵；W(k)和v(k)分别为状态噪声和观测噪声，且满足:

式(10)中，R表示m×m维量测噪声序列v(k)的方差阵，v(k)^T表示v(k)的转置矩阵，即W(k)和V(k)表示互相独立的白噪声序列,且它的均值为零，系统初始状态X(0)的均值向量以及方差阵如式(11)所示：

E[X(0)]＝μ₀ (11)

E[[X(0)-μ₀][X(0)-μ₀]^T]＝P₀ (12)

上式(12)中，P表示误差协方差，P₀是它的初始值；

2.2)卡尔曼滤波的工作流程包括如下分步骤：

2.2.1)Kalman滤波预测方程：

上式(13)中，X(k|k-1)表示当前时刻(k)系统状态预测值，

表示上一时刻(k-1)系统状态的最优值；

P(k|k-1)＝AP(k-1|k-1)A^T+Q (14)

上式(14)中，P(k|k-1)表示X(k|k-1)状态对应的协方差矩阵，P(k-1|k-1)表示X(k-1|k-1)对应的协方差矩阵，A表示系统矩阵；A^T表示A的转置矩阵，Q表示系统工作过程的协方差矩阵；

2.2.2)Kalman滤波方程校正过程：

卡尔曼滤波增益矩阵：

K(k)＝P(k|k-1)C^T[CP(k|k-1)C^T+R]^-1 (15)

上式(15)中，K(k)表示k时刻的卡尔曼增益矩阵，是使状态估计值

的均方误差值最小；R表示观测方程中V(k)的方差矩阵；P(k|k-1)表示k时刻和(k-1)时刻预测协方差矩阵；C为输出矩阵，C^T表示C的转置矩阵；

K时刻状态更新：

上式(16)中，

X(k-1|k-1)分别表示输出(k)和(k-1)时刻的状态向量；S(k)表示k时刻RSSI信号的测量值；C为输出矩阵，此时，若对象模型准确度高且系统状态和参数没有发生突变时，卡尔曼滤波的性能好；

估计协方差矩阵P(k|k)更新:

P(k|k)＝[I-K(k)C]P(k|k-1)(I-K(k)C)^T+K(k)RK(k)^T (17)

化简为：

P(k|k)＝[I-K(k)C]P(k|k-1) (18)

式(17)、式(18)中，P(k|k)表示输出滤波协方差；I表示n×n阶单位矩阵；

用户位置移动的三维模型为：

上式(19)中，

和

分别表示k时刻、(k-1)时刻机器人在空间坐标系中估计的三个运动方向的位移和速度；

分别代表(k-1)时刻机器人在空间坐标系中估计的三个运动方向位移噪声和运行速度噪声；

上式(20)中，

表示机器人在k时刻居于空间坐标系中三个方向的位移向量；

表示机器人在k时刻在空间三个方向的运动速度向量；

分别代表k时刻机器人在空间坐标系中估计的三个运动方向的位移和速度向量；

初值选取：

X(0)＝0，

机器人坐标转移矩阵：

上式(22)中,(x，y，θ)表示机器人在时间点t-1时刻的位置,(v_t，+ω_t)表示机器人运动的线速度和角速度,(x′，y′，θ′)表示机器人按弧线轨迹移动到当前时刻t时新的位置；

2.2.3)预处理加速度：

对加速度进行预处理，即任意时刻三轴总加速度表示为：

上式(23)中，a_i表示第i个采样点的加速度，a_ix、a_iy、a_iz分别表示加速度计X轴、Y轴，Z轴上的对应值，去除加速度采样值中的重力加速度值，得下式：

上式(24)中，a_g表示重力加速度，N表示采样点数，2N+1表示滑动窗口的大小；

2.2.4)步长估计模型：

本步骤算法中采用线性模型估计步长，如下式：

上式(25)中，i＝1,2,3,…,n表示实验次数，F表示实验中采用的频率，S表示每次实验中取得的步长值；a和b都是常量系数；

步长用矩阵表示为：

S＝AK (26)

上式(26)中，

由最小二乘法，可得：

K＝(A^TA)^-1(A^TS) (27)

3)卡尔曼滤波室内机器人定位算法优化滤波参数，当网络初始化后，读取相应参数，这是反馈系统的输入，之后求取观测值，实施卡尔曼滤波算法，最后输出最优状态预估值。

2.根据权利要求1所述的一种基于卡尔曼滤波的室内机器人定位算法，其特征在于：步骤1)中，当信标节点数量多于4时，公式(6)即为矛盾模型：采用另一种计算模型，当HH^T为非奇异矩阵时，Δx为：

Δx＝[H^TH]^-1H^TΔd (7)

通过未知节点位置以及估计传输距离误差两个方面着手，然后逐步优化计算结果直到达到测量标准条件，此值也是定位的最终结果。

Images