CN108168552A - 一种看护机器人路标定位系统 - Google Patents

Abstract

本发明公开一种看护机器人路标定位系统，所述系统包括三个模块：S1：建立助老机器人模型：通过建立状态方程与观测方程建立，使机器人系统工作在未知路标下；S2：实现精确定位：采用未知路标的多传感器信息融合的无迹卡尔曼滤波的定位方法；S3：提高滤波精度：采用改进的Sage‑Husa自适应滤波算法对其未知噪声方差阵进行估计。未知路标下，本发明的定位方法能够在系统噪声和量测噪声均未知时实时估计噪声统计特性，使噪声维持在非常小的范围内。本发明采用的算法不需要路标参数，使助老机器人在开放环境中运行也能实现良好定位，扩大了其应用场合，具有广泛的应用价值。

Description

一种看护机器人路标定位系统

技术领域

本发明涉及路标定位系统，特别是涉及一种看护机器人路标定位系统。

背景技术

针对我国人口老龄化日趋严重的问题，研究开发助老机器人，助老机器人的特点为速度慢，所需检测精度低，且运行在开放环境。而助老机器人服务的特殊性决定了其工作环境的开放性，且在开放环境中要求精确定位，保证助老机器人安全工作。因此，对助老机器人定位技术的研究引起关注。

目前，机器人定位大多针对已知路标，通过各种传感器来识别定位，或者在未知环境中假设其自然特征易于提取，机器人通过对自然特征测量估计实现自身定位，这就容易受外界因素干扰，使环境特征或已知路标特征不易提取造成测量误差，从而限制了机器人的应用场合，影响了定位的准确性与实时性。

其次，提高定位精度常用的方式是采用多传感器信息融合。而在对多传感器信息进行融合时，扩展卡尔曼滤波(EKF)获得了广泛而成功的应用。但是在处理非线性问题时，EKF存在较大的误差且计算复杂。

申请公布号为CN102183959A的发明专利申请公开了一种“移动机器人的自适应路径控制方法”，该发明专利使用至少两个路标定位器，并通过算法求得虚拟坐标，从而使移动机器人实现自主路径定位，但其装置需要设置路标定位器搜索信号，所需硬件多且成本较高。

论文《未知环境下的移动机器人SLAM方法》，华中科技大学学报(自然科学版).2011，(7).公开了一种“一种应用于未知环境下的移动机器人同步定位与地图创建(SLAM)方法”，该方法通过一种基于人工鱼群算法的粒子滤波算法，提高了机器人的SLAM性能，但是仍然无法改变SLAM问题中定位误差总体逐渐增大的趋势。

发明内容

本发明的目的在于克服现有技术的不足，提供一种看护机器人路标定位系统，该系统提出一种采用未知路标的多传感器信息融合的无迹卡尔曼滤波的定位方法，并采用改进的Sage-Husa自适应滤波算法对其未知噪声方差阵进行估计，从而提高了助老机器人定位的精确度。

本发明解决上述技术问题的技术方案为：

一种看护机器人路标定位系统，其特征在于，其中：

所述系统包括：建立助老机器人模型模块、精确定位模块、提高滤波精度模块，其中：

所述的建立助老机器人模型模块用于建立助老机器人的工作状态，使其工作在未知路标下；

所述的实现精确定位模块用于实现助老机器人自主导航过程中处于未知环境的精确定位；

所述的提高滤波精度模块用于克服传统的多传感器融合定位算法处理非线性系统时所产生的滤波不稳，计算复杂，难以实现最优定位等难题，从而解决助老机器人非线性系统的定位优化问题。

本发明的看护机器人路标定位系统中所述的实现精确定位模块，采用未知路标的多传感器信息融合的无迹卡尔曼滤波的定位方法。

本发明的看护机器人路标定位系统中所述的提高滤波精度模块，采用UKF(出自论文《自适应UKF算法在目标跟踪中的应用》，作者石勇，韩崇昭，自动化学报，2011，(6):755－759.)结合改进的Sage-Husa自适应滤波算法对其未知噪声方差阵进行估计，从而提高滤波的精度。

本发明与现有技术相比具有以下的有益效果：

1、助老机器人进行定位时增加了冗余信息——超声波定位，与采用里程计进行的航迹推算定位信息进行信息融合，从而减小甚至消除定位误差。

2、利用多传感器信息融合算法实现定位，提高了助老机器人定位的精确度。

3、采用UKF结合改进的Sage-Husa自适应滤波，更好地解决助老机器人非线性系统的定位优化问题。

附图说明

图1为本发明的看护机器人路标定位系统的一个具体实施方式的系统流程示意图。

图2为本发明的一个具体实施方式中的助老机器人运动模型的示意图。

图3为机器人量测模型的示意图。

图4为基于超声波器测距定位的原理示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

参见图1，本发明的看护机器人路标定位系统由图中三个模块组成，包括建立助老机器人模型模块、实现精确定位模块和提高滤波精度模块，其中：

S1：建立助老机器人模型模块：建立助老机器人的工作状态，使其工作在未知路标下，步骤包括S11,建立状态方程；S12,观测方程的建立，详细描述如下：

S11,建立状态方程：设助老机器人在k时刻的全局位姿为X_k，即为机器人状态变量，定义如下：

X_k＝[x_k y_k v_x(k) v_y(k) a_x(k) a_y(k)]^T (1)

其中，x_k和y_k分别表示助老机器人在相对导航坐标下的东向和北向的位置，v_x(k)和v_y(k)分别表示助老机器人在相对导航坐标下的东向和北向的速度，a_x(k)和a_y(k)分别表示助老机器人在相对导航坐标下的东向和北向的加速度分量。

则表示系统状态X_k+1的方程为：

X_k+1＝FX_k+W_k (2)

式中，W为过程噪声，F为机器人采样周期的定义矩阵：

S12，观测方程的建立：设助老机器人车轮的直径为D，光码盘的线数为p线/转，Δt时间内光码盘输出脉冲数为N。参见图2机器人运动模型的示意图，可知：

式中，ω为机器人行驶角速度，Δθ为机器人转过的角度，ΔS为采样周期，NL，NR为机器人左右轮光码盘输出脉冲数。联立方程组得

若传感器测得的车到假设路标的距离为d。参见图3的机器人量测模型，机器人从k-1处移动到k处再移动到k+1处，(x_i,y_i)为任一路标。

由图3知，

如上，式(6)中含有(6a)、(6b)、(6)三式，用(6b)式减去(6a)式，得

同理(6c)式减(6b)式得

将式(8)和式(7)再做差，得：

由于助老机器人行走速度慢，k时刻与k-1时刻机器人在x，y轴速度分量近似相等，即v_x(k)-v_x(k-1)和v_y(k)-v_y(k-1)的值都近似为0，为了避免路标测量误差，令ω_d(k+1)＝-2Tx_i(v_x(k)-v_x(k-1))-2Ty_i(v_y(k)-v_y(k-1))为噪声项，从而使系统工作在未知路标下。将助老机器人左右轮安装的光电编码器输出脉冲NL，NR和超声波测得的到环境路标的距离平方作为观测量，即k+1时刻的量测信息为则系统的离散观测方程为

Z_k+1＝h(X_k+1,k+1)+V_k+1 (10)

式中，h(X_k+1)＝[h1 h2 h3]^T，V_k+1为测量噪声，

V_k+1＝[ω_l(k+1) ω_r(k+1) ω_d(k+1)]^T (11)

S2：实现精确定位：助老机器人实现自主导航最基本且最关键的过程，它是助老机器人在未知环境中确定当前位置的一个过程，步骤包括S21，采用里程计进行航迹推算定位；S22，基于超声波测距定位；S23，多传感器信息融合实现精确定位；详细描述如下：

S21，采用里程计进行航迹推算定位：通过安装在左右两轮的编码器和陀螺仪确定机器人行进的距离和偏移角度，假设机器人从k时刻M(x_k,y_k,θ_k)点移动到k+1时刻点M'(x_k+1,y_k+1,θ_k+1)，如图2所示，机器人左右轮距为B，车轮移动的距离为ΔS。

则运动模型方程如下:

式中，Δt分别为机器人左右轮移动距离，u_k＝(ΔS_k,Δθ_k)T是移动机器人里程计模型的输入，Δθ和Δθ_k是机器人在Δt时间内运动所经过的弧长和转过的角度。采用编码型脉冲速度计进行测速，则ΔS_k和Δθ_k与计数脉冲数的关系为

式中NL,NR为机器人左右轮光码盘输出脉冲数。

对于ΔS_k和Δθ_k可通过对Δθ的积分获得。即通过测量驱动轮的转速，可实现定位。由式(15)和式(16)可见，机器人位置与驱动轮转速是非线性关系，且存在角速度的积分运算，则随着时间的延长，其定位精度迅速恶化，以致最终无法定位。

故仅用该方法进行定位不可取。因此，本系统增加了冗余信息——超声波定位，与之进行信息融合，可减小甚至消除定位误差。

S22，基于超声波测距定位：基于超声波器测距定位的原理参见图4。机器人处于OR处，路标处于A点且坐标已知。则机器人的位置坐标为

式中a为机器人距路标A的距离，为机器人与路标的连线与X轴正方向的夹角。

在开放环境中，路标A的坐标往往是未知的，所以上述(17)式无法直接应用。但考虑到助老机器人的运行速度慢，相邻时刻的状态变化小，故可认为，在相邻时刻机器人与路标的相对位置变化很小，并将其变化量分解成两部分，一部分为与路标无关的项，另一部分为与路标相关项，且该项可作为噪声处理。因此，在路标坐标未知时，就可借助于超声波测距实现定位。最后，再借助于信息融合技术实现机器人的精确定位。

S23，多传感器信息融合实现精确定位：多传感器信息融合算法是助老机器人实现精确定位的关键。传统的多传感器融合定位算法对非线性系统的处理会产生滤波不稳，且计算复杂，难以实现最优定位。本系统采用UKF结合改进的Sage-Husa自适应滤波，从而解决助老机器人非线性系统的定位优化问题。下面结合S3提高滤波精度模块做详细描述。

S3：提高滤波精度：本系统的状态方程是线性的，观测方程是严重非线性的，采用EKF进行滤波和信息融合，滤波不稳甚至使系统发散，究其原因是EKF先进行一阶泰勒级数展开，忽略高次项，然后再进行卡尔曼滤波优化。其高次项的忽略导致一步预测估计误差和滤波估计误差较大所造成。

因此，本系统选用无迹卡尔曼滤波进行多传感器信息融合并结合改进的

Sage-Husa自适应滤波对系统噪声方差阵进行估计优化。

滤波过程如下：

S31，初始化：

状态变量的状态估计值和估计误差协方差为P₀：

S32，计算sigma点与权值：

式中，为采样点；分别为状态一步预测和一步预测误差协方差的权值；λ＝α²(n+k)-n；n为增广状态变量的维数；α值决定了sigma点的分布范围；β为调节参数；k为沉余量。本文选取α＝0.8，β＝2，k＝0。

S33，状态预测：计算sigma点沿状态方程传播的状态一步估计P_k+1,k和一步预测误差协方差P_k+1，

式中，Q_k为过程噪声协方差阵。

S34，进行滤波：根据状态扩维，计算符合一步预测和预测误差协方差M_k+1,k的sigma点与其沿量测方程的传播值

计算sigma点沿量测方程传播的量测均值、量测误差协方差和与状态误差的互协方差

定义新息由于系统在实际应用中扰动噪声对无迹卡尔曼滤波定位精度有较大影响，所以在滤波过程中必须考虑对系统过程噪声和量测噪声协方差阵Q，R的实时更新。为了提高助老机器人定位精度，本文采用一种改进的加权比例修正法实现对Q，R的估计。

估计过程如下：

K_k+1＝C_k+1(S_k+1+R_k+1)^-1 (29)

式中，d＝(1-b)/1-b^k+1，b为遗忘因子，本系统取b＝0.5。

更新状态估计与预测误差协方差:

该优化方法是先估计优化噪声协方差阵R，得出R阵最优值，再通过R阵最优值估计更新Q阵，更进一步地提高了对噪声值的估计，从而更好地控制噪声，提高定位精度。

上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (5)

1.一种看护机器人路标定位系统，其特征在于，该系统包括三个模块：S1：建立助老机器人模型；S2：实现精确定位；S3：提高滤波精度，其中：

所述的建立助老机器人模型模块用于建立助老机器人的工作状态，使其工作在未知路标下；

所述的实现精确定位模块用于实现助老机器人自主导航过程中处于未知环境的精确定位；

所述的提高滤波精度模块用于克服传统的多传感器融合定位算法处理非线性系统时所产生的滤波不稳，计算复杂，难以实现最优定位的难题，从而解决助老机器人非线性系统的定位优化问题。

2.根据权利要求1所述的一种看护机器人路标定位系统，其特征在于，所述的建立助老机器人模型模块，包括下列两个步骤：

(1)建立状态方程；

(2)观测方程的建立。

3.根据权利要求1所述的一种看护机器人路标定位系统，其特征在于，所述的实现精确定位模块，包括下列三个步骤：

(1)采用里程计进行航迹推算定位；

(2)基于超声波测距定位；

(3)多传感器信息融合实现精确定位。

4.根据权利要求1所述的一种看护机器人路标定位系统，其特征在于，所述的实现精确定位模块，采用未知路标的多传感器信息融合的无迹卡尔曼滤波的定位方法来实现助老机器人对路标的精确定位。

5.根据权利要求1所述的一种看护机器人路标定位系统，其特征在于，所述的提高滤波精度模块，采用改进的Sage-Husa自适应滤波算法对其未知噪声方差阵进行估计，从而提高滤波的精度。