一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法与流程

文档序号:15702748发布日期:2018-10-19 20:08阅读:957来源:国知局

本发明属于移动机器人领域,尤其是一种用于跟随机器人的人体目标跟踪方法。



背景技术:

随着社会生产和生活要求的日益提高,移动机器人产业发展非常迅速并逐步渗透于社会服务、医疗健康以及资源探测等领域。在未知复杂的动态环境中,实现对服务对象的识别与跟踪是提高移动机器人环境适应能力的主要途径之一,同时也是其通过人机协作完成复杂指定任务的前提和基础。

为实现移动机器人对人体目标的有效检测与跟踪,需要设计一个稳定的、性能良好的人体目标跟踪估计器。考虑到人体目标跟踪过程中存在的非线性滤波问题,一些常用的非线性滤波方法有扩展卡尔曼滤波方法、无迹卡尔曼滤波方法以及容积卡尔曼滤波方法等。然而,上述非线性估计器的拟合精度十分有限,当系统具有强非线性时,会引入较大的线性化误差,从而破坏目标跟踪估计器的稳定性与收敛性。特别地,受环境变化、传感器性能等因素的影响,在人体目标跟踪过程中可能会存在目标遮挡、丢失等情况。这将引起某一采样时刻的量测信息缺失,使得系统的线性化误差增大,从而导致机器人无法准确地跟踪服务对象。为了减小线性化误差对系统的影响,可采用迭代卡尔曼滤波方法或高斯渐进滤波方法对系统状态进行估计。此类方法通过对其中的量测更新过程进行多次迭代运算,可一定程度地提高系统对人体目标的跟踪精度。然而,现有的迭代方法没有充分考虑到线性化误差的补偿问题,易导致量测迭代次数过多,进而产生过自信的估计结果。目前,尚未有人体目标跟踪方法能够克服上述因素的影响,使得跟随机器人能可靠、稳定地实现人体跟随功能。



技术实现要素:

为了克服现有人体目标跟踪方法的精度较低、稳定性较差的不足,本发明提供了一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法,该方法在保证计算复杂度的前提下,考虑线性化误差对系统的影响,有效地提高人体目标的跟踪精度。

本发明解决其技术问题所采用的技术方案是:

一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法,所述方法包括以下步骤:

步骤1,建立跟随系统的状态模型,以及相关服务对象的量测模型;

步骤2,初始化参数,确定机器人跟随系统的状态向量以及对应的噪声协方差;

步骤3,对系统状态进行更新,得到k时刻系统的状态预测值及其对应协方差pk|k-1;

步骤4,对系统的量测更新过程进行迭代。根据迭代过程中系统的似然函数以及采样粒子的权重自适应地调整迭代步长δ;同时,令γn=γn-1+δ,判断γn是否大于等于1,结果为是,则令δ=1-γn-1,结果为否,则不做修改;然后,将量测噪声rk放大δ倍,并计算第γn次更新后的量测预测值及其协方差判断第γn次更新后的状态估计误差是否在均方意义下有界,结果为是,则执行步骤5;结果为否,则k时刻系统的后验状态向量及其协方差pk为第γn-1次目标后验状态向量与协方差并令k=k+1,执行步骤3;

步骤5,根据所得的量测预测值及其协方差以及互协方差得到第γn次量测更新后系统的后验状态向量与协方差

步骤6,判断γn是否等于1,结果为是,则k时刻目标后验状态向量及其协方差pk为第γn次目标后验状态向量与协方差并令k=k+1,执行步骤3;结果为否,则令n=n+1,执行步骤4。

进一步,在所述步骤1中,建立跟随机器人在未知动态环境中的运动学模型,同时选取系统的状态向量为其中,为k时刻人体目标的状态向量,分别为人体目标在x轴和y轴上的位置,为人体目标在x轴和y轴上的速度,为跟随机器人的状态向量,为机器人在x轴和y轴上的位置,为机器人在x轴和y轴上的速度。

再进一步,所述步骤1中,机器人与人体目标间的位置关系通过视觉、激光等传感器得到,进而根据二者之间的量测信息建立系统的量测模型。

更进一步,在所述步骤4中,由判定条件判断系统的状态估计误差是否在均方意义下有界,其中,为第γn次量测更新后的新息;当不满足判定条件时,说明没有必要对量测更新过程再进行迭代。

本发明的有益效果主要体现在:本发明提出了一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法。针对线性化误差过大引起的系统跟踪性能下降的问题,该方法在量测更新过程中,通过渐进地引入当前量测信息以减小线性化误差,从而有效地提高人体目标的跟踪精度。其次,在量测渐进过程中引入判定条件,进一步地补偿系统中存在的线性化误差。同时,给出了渐进步长的自适应调整策略,有效地降低系统的计算复杂度。相比于现有的目标跟踪方法,该方法充分考虑线性化误差对系统的影响,在保证计算复杂度的前提下,提高了对人体目标的跟踪精度。

附图说明

图1是跟随机器人与人体目标的位置示意图,其中,1为激光,2为跟随机器人,3为跟踪人物。

图2是机器人跟随系统的坐标转换图,其中,4为跟踪人物坐标系,5为机器人坐标系。

图3是本方法的算法流程图。

具体实施方式

下面结合附图对本发明做进一步说明。

参照图1~图3,一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法,由图1所示,跟随机器人采用激光传感器对人体目标进行探测,获得服务机器人与人体目标间的距离与角度;如图2所示,根据激光传感器返回的量测信息,通过坐标转换得到在全局坐标下机器人与人体目标间的位置关系,跟随系统的状态空间模型如公式(1)所示,传感器量测模型如公式(2)所示:

其中,系统的状态向量为分别为k时刻人体目标在x轴和y轴上的位置,为人体目标在x轴和y轴上的速度。为机器人在x轴和y轴上的位置,为机器人在x轴和y轴上的速度。ω为角速度,采样时间为δtk=tk-tk-1。过程噪声wk为零均值协方差为qk的高斯噪声。量测噪声vk是与过程噪声wk无关的,均值为零且协方差rk的高斯噪声。

参照图3,一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法,包括以下步骤:

步骤1,建立跟随系统在未知动态环境中的状态模型,并根据机器人与跟踪人体目标间的位置关系建立相应的量测模型;

步骤2,初始化参数,确定机器人跟随系统的状态向量以及对应的噪声协方差qk;

步骤3,对系统状态进行更新,结合ut变换得到k时刻的状态预测值及其对应协方差pk|k-1;

其中,为系统的sigma点,为对应的权值。

步骤4,对量测更新过程进行迭代,根据迭代过程中系统的似然函数以及采样粒子的权重自适应地调整迭代步δ

其中,

令γn=γn-1+δ,判断γn是否大于等于1,结果为是,则令δ=1-γn-1,结果为否,则不做修改;然后,将量测噪声rk放大δ倍,并计算第γk次更新后的量测预测值协方差与互协方差

然后,判断第n次更新后的新息是否满足其中,为第γn次迭代后的量测新息;结果为是,则执行步骤5;结果为否,则k时刻目标后验状态向量及其协方差pk为第γn-1次目标后验状态向量与协方差并令k=k+1,执行步骤3;

步骤5,根据所得的量测预测值及其协方差以及互协方差得到第γn次更新后的目标后验状态向量与协方差

其中,系统的权值为

步骤6,判断γn是否等于1,结果为是,则k时刻目标后验状态向量及其协方差pk为第γn次目标后验状态向量与协方差并令k=k+1,执行步骤3;结果为否,则令n=n+1,执行步骤4。

当前第1页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1