一种用于机器人的目标柔性跟随控制方法

技术领域

本发明涉及机器人控制，更具体地说，涉及一种用于机器人的目标柔性跟随控制方法，属于机器人控制技术领域。

背景技术

随着时代的进步，各种前沿高新技术进行了多学科交叉升级，机器人就是其中的典型代表。机器人被誉为“制造业皇冠顶端的明珠”，其研发、制造、应用是衡量一个国家科技创新以及高端制造业水平的重要标准。机器人能通过传感器与周围环境进行感知交互并监测自身运行状态，能够以优化的跟随策略跟随任务目标完成动态生产任务。现有城市的物流中心以及机场等场所需要运输货物，如果采用人工搬运的方式不但效率低下，而且运营成本高、人工劳动强度大。而能够智能跟随的运输机器人则能够很好的解决这些问题。目前，许多物流中心的仓储机器人可以实现人员跟随，障碍闪避等功能。机场采用跟随机器人，用来帮助乘客搬运行李，提升运输效率，节省运输成本。

现有工业机器人虽然在服务不同行业中发挥了越来越大的作用，但在实际工作过程中依然存在一些问题。其中之一就是行走速度，特别是在上下坡的过程中，现有跟随技术并未考虑上坡阻力和下坡惯性力，如果速度控制不好，在坡道跟随时会有明显的顿挫感，不能保证在坡道场景中以平缓速度维持跟随运动，且在坡度较大时容易发生安全事故。另一方面，现有跟随技术机器人身上的传感器(如相机等)容易受到光照、遮挡等因素的影响(特别是坡道跟随更容易出现)，导致跟随目标丢失，丢失后如何快速准确地找回目标也是跟随技术需要解决的问题。

发明内容

针对现有技术存在的上述不足，本发明的目的是提供一种用于机器人的目标柔性跟随控制方法，本方法能够在各种路面实现良好的柔性跟随，特别能够避免坡道跟随的顿挫感，且在目标丢失后能够快速准确地找回。

本发明的技术方案是这样实现的：

一种用于机器人的目标柔性跟随控制方法，步骤如下，

1)确定实际跟随距离L

L

其中L

2)根据人机距离差L

误差函数e(t)表示为：e(t)＝f(x)-f

其中，f(x)为期望的线速度和角速度，f

3)PID控制器输出需要补偿的线速度和角速度给差速运动模型，差速运动模型通过分解，得到机器人驱动电机相应的转速，然后对机器人各行走轮轮速进行计算分配，从而驱动机器人按分配后的轮速行走；

4)重复步骤1)-3)，即实现机器人对目标的柔性跟随。

其中，步骤1)中，确定实际跟随距离L

如果没有丢失，则通过机器人身上的视觉传感器获得的观测值确定实际跟随距离；

如果丢失，则通过卡尔曼滤波器得到当前时刻的预测值，基于预测值来确定实际跟随距离；同时将机器人视觉传感器获取的当前观测值与上一时刻预测值进行交并比计算，大于阈值则表示目标找回，下一时刻以机器人视觉传感器获得的观测值确定实际跟随距离，同时将机器人视觉传感器获得的当前观测值作为卡尔曼滤波器下一时刻的输入；如果小于阈值，则表示目标没有找回，将卡尔曼滤波器得到的当前时刻的预测值作为卡尔曼滤波器下一时刻的输入。

具体地，本发明通过下述方法判断机器人跟随目标是否丢失，

在跟随目标上预先设有目标框，机器人通过对目标框的跟随实现对跟随目标的跟随；如果机器人视觉传感器得到的观测值中没有目标框的任何信息，则认为目标丢失，否则目标没有丢失。

进一步地，本发明通过卡尔曼滤波器得到当前时刻预测值的过程为，

A)将目标框的中心点坐标x

β＝(x

B)初始化卡尔曼滤波器参数；包括状态转移矩阵A、状态观测矩阵H、过程噪声矩阵Q、状态噪声矩阵R、状态估计协方差P；

C)计算机器人视觉传感器得到的当前观测值与卡尔曼滤波器上一时刻的预测值的交并比IOU，若大于阈值则将机器人视觉传感器获得的当前观测值作为卡尔曼滤波器下一时刻的输入；否则将卡尔曼滤波器得到的当前时刻的预测值作为卡尔曼滤波器下一时刻的输入；

D)按下式更新先验估计

式中

E)按下式计算状态协方差矩阵：

式中P

F)根据步骤E)得到的状态协方差矩阵，按下式计算卡尔曼增益：

式中

G)按下式计算预测值；

式中Y

进一步地，步骤G)后还包括如下步骤，

H)按下式更新协方差矩阵：

其中，I是单位矩阵，

I)判断滑动窗口数是否达到设定值；如否，则转入步骤C)；如是，则进入步骤J)；

J)按下式计算新息估计方差Φ

其中，

K)按下式计算新息自适应状态噪声矩阵R

在目标没有丢失的情况下，同时通过卡尔曼滤波器不断得到当前时刻的预测值和新息自适应状态噪声矩阵R

进一步地，步骤2)中，使用目标中心偏移量来等价角速度的变化，所述期望的线速度和角速度利用下式计算得到，

其中ω为期望的角速度，ω

步骤3)中，PID控制器输出需要补偿的线速度和角速度按下式计算得到，

其中u(t)代表需要补偿的线速度，ω(t)代表需要补偿的角速度，K

相比现有技术，本发明具有如下有益效果：

1、本发明柔性跟随策略使得跟随机器人在跟随过程中行驶的平顺性更好，能够避免跟随的顿挫感，即使在较大坡度上行驶也具有较好的安全性，能够更好的保持人车跟随距离。

2、当视觉目标丢失时，通过采用卡尔曼滤波器可以对目标框的位置进行预测并提供目标的大致方位，通过预测实现连续跟随，同时有助于跟随机器人丢失目标后完成重识别，保障跟随的紧密性以及流畅性。本发明设计了一种基于新息自适应卡尔曼滤波器的目标重识别方法，利用在跟随过程中的新息项数据对噪声进行自适应，在目标丢失后能够准确快速地找回，更好地实现人机跟随。

本发明基于新息自适应卡尔曼滤波器的目标跟随与重识别鲁棒性与实时性更好，柔性跟随策略的鲁棒性更好，具有响应速度较快，短时间内变速较快，实时性较好，可以更快的缩短人机跟随距离，降低目标丢失机率等优点。

附图说明

图1-本发明目标跟随控制方法示意图。

图2-本发明基于新息自适应卡尔曼滤波器的目标重识别流程图。

图3为传统卡尔曼滤波目标跟踪效果图。

图4为本发明新息自适应卡尔曼滤波目标跟踪效果图。

图5为传统卡尔曼滤波与本发明新息自适应卡尔曼滤波的误差对比图。

图6为传统卡尔曼滤波与本发明新息自适应卡尔曼滤波精度对比图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明用于机器人的目标柔性跟随控制方法，步骤如下，同时参见图1，

1)跟随机器人的速度V与在短时间内所需要跟随的距离变化量有关，为此先确定实际跟随距离L

L

其中L

2)根据人机距离差L

误差函数e(t)表示为：e(t)＝f(x)-f

其中，f(x)为期望的线速度和角速度，f

3)PID控制器输出需要补偿的线速度和角速度给差速运动模型，差速运动模型通过分解，得到机器人驱动电机相应的转速，然后对机器人各行走轮轮速进行计算分配，从而驱动机器人按分配后的轮速行走；

4)重复步骤1)-3)，即实现机器人对目标的柔性跟随。

为了避免机器人跟随时倒车或急停急起，在保障紧密跟随的过程中安全距离的设定十分重要。本发明根据空间关系学将人机跟随的期望跟随距离L

步骤2)中，使用目标中心偏移量来等价角速度的变化，所述期望的线速度和角速度利用下式计算得到，

其中ω为期望的角速度，ω

步骤3)中，PID控制器输出需要补偿的线速度和角速度按下式计算得到，

其中u(t)代表需要补偿的线速度，ω(t)代表需要补偿的角速度，K

本发明通过视觉传感器获取实际跟随距离，进而获取人机距离差，根据不同的人机距离差对跟随速度进行调控，避免跟随时丢失跟随目标，输出误差函数进入PID控制器，输入差速运动模型，对各轮轮速进行计算分配，并反馈实际线速度与角速度，更新人机距离差，则可实现人机紧密跟随。

特别是在坡道跟随时，考虑受到惯性力及坡道阻力的影响，为了保持人车紧密跟随，在上坡路段需要增大驱动力，而下坡路段需要减小驱动力。同时考虑轮式跟随机器人制动时可能会产生滑移，所以为了保障人车跟随的安全性需要在下坡时对速度进行提前干涉，以保证人车紧密跟随。

上述实际跟随距离是基于视觉传感器而获得。然而在实际跟随过程中，由于视觉传感器角度问题容易受到光照、遮挡等影响，可能出现目标跟丢现象，在坡道跟随中这种影响更明显。为了保障跟随的紧密性以及流畅性，当视觉目标丢失时，需要快速准确地找回，并且在没有找回前需要预测目标所在位置以帮助机器人通过预测结果进行跟随，避免机器人原地等候导致目标更难找回。

基于此，在跟随目标丢失的情况下，本发明则通过卡尔曼滤波器得到当前时刻的预测值，基于预测值来确定实际跟随距离。同时将机器人视觉传感器获取的当前观测值与上一时刻预测值进行交并比计算，大于阈值则表示目标找回，下一时刻以机器人视觉传感器获得的观测值确定实际跟随距离，同时将机器人视觉传感器获得的当前观测值作为卡尔曼滤波器下一时刻的输入。如果小于阈值，则表示目标没有找回，将卡尔曼滤波器得到的当前时刻的预测值作为卡尔曼滤波器下一时刻的输入，以继续预测目标位置。

为了方便机器人跟随，本发明在跟随目标上预先设有目标框，机器人通过对目标框的跟随实现对跟随目标的跟随。同时基于是否检测到目标框来判断跟丢与否，如果机器人视觉传感器得到的观测值中没有目标框的任何信息，则认为目标丢失，否则目标没有丢失。

通过卡尔曼滤波器可以对目标框的位置进行预测并提供目标的大致方位，确保跟随机器人在丢失目标后能够基于预测信息继续跟随并尽快完成重识别以找回目标。该部分见图1虚线框所示。同时参见图2。

A)将目标框的中心点坐标x

β＝(x

B)初始化卡尔曼滤波器参数；包括状态转移矩阵A、状态观测矩阵H、过程噪声矩阵Q、状态噪声矩阵R、状态估计协方差P；

C)计算机器人视觉传感器得到的当前观测值与卡尔曼滤波器上一时刻的预测值的交并比IOU，若大于阈值则将机器人视觉传感器获得的当前观测值作为卡尔曼滤波器下一时刻的输入；否则将卡尔曼滤波器得到的当前时刻的预测值作为卡尔曼滤波器下一时刻的输入；

D)根据式(7)更新先验估计

其中，下标k为当前时刻，k-1为上一时刻；

E)根据式(8)计算状态协方差矩阵：

Q

F)根据式(9)计算卡尔曼增益：

式中

G)根据式(10)计算得到预测值：预测值由目标框的中心点坐标x

Y

H)根据式(11)更新协方差矩阵：

其中，I是单位矩阵，

若IOU匹配失败(小于阈值，认定目标丢失还没找回)，将当前时刻预测值作为下一时刻卡尔曼滤波器的输入，此时每帧更新状态，直至IOU大于阈值，匹配成功。若在设定的时间内(如5秒后)仍未完成目标重识别，则跟随机器人停止跟随。因为通过预测值进行跟随的误差往往会越来越大，如果在5秒内目标还没有找回，那大概率就难以再找回了，所以停止跟随是较佳选择。同时在目标丢失期间由于无法确定深度信息，换作锚框面积进行等价替换。通过目标框的面积计算线速度大小，预测的目标框中心偏移量作为角速度大小。因为在跟随过程中是通过相机成像原理和深度图像计算跟随目标的距离，但是目标丢失后仅仅只有预测值缺乏深度图像信息无法进行跟随距离的求解，所以本发明根据图像离相机越近目标框面积越大、离相机越远面积越小的原理来预测跟随目标的距离(目标框的面积可以通过上述6个状态量x

卡尔曼滤波器能在符合线性高斯模型的条件下给出最优估计，主要是由预测与更新两部分组成，在预测的过程当中主要对系统的状态、误差协方差进行预测，而在更新过程中，则通过测量信息更新协方差矩阵以及增益矩阵。使用卡尔曼滤波器对目标进行预测需要获取精确的系统模型以及噪声参数。若系统模型的环境信息发生变化，则噪声参数将会发生变化导致滤波器性能下降或直接发散。

在目标跟随过程中，噪声主要来源两方面，一方面来自硬件设备，如视觉传感器的内部参数、边缘数据平台等，另一方面则是在目标跟随过程中，由于目标被遮挡、阳光强度变化等因素造成。由于跟随过程中无法精确获取噪声参数，容易导致滤波器稳定性大幅降低，影响跟随效果。

为了降低噪声统计特性未知带来的影响，本发明提出一种基于新息自适应卡尔曼滤波器的目标重识别方法，利用在跟随过程中的新息项数据对噪声进行自适应，从而实现人机紧密跟随。具体方法如下所示。

物理学预测值x

x

Y

上式中，W

在K时刻时，过程噪声矩阵Q与状态噪声矩阵R满足如下关系：

E[W

假设K时刻通过视觉传感器获取的新息φ

将式(13)带入式(17)，得式(18)：

对式(18)两边同时取方差可得：

结合滑动窗口法，判断滑动窗口数是否达到设定值；如否，则转入步骤C)进行交并比计算；如是，则进入下一步；即按下式计算新息估计方差Φ

其中m为窗口长度，k≥m，实施例中m根据经验值取6。

综合式(19)与式(20)可得新息自适应状态噪声矩阵R

在实际跟随过程中，过程噪声矩阵Q与状态噪声矩阵R同时存在于新息序列，若过程噪声矩阵Q与状态噪声矩阵R中仅有一种噪声矩阵方差发生变化，噪声估计算法是相对有效且可靠的，因此本发明仅对状态噪声矩阵进行自适应估计。

传统的卡尔曼滤波器在更新过程中，通过测量信息更新协方差矩阵以及增益矩阵。使用卡尔曼滤波器对目标进行预测需要获取精确的系统模型以及噪声参数。若系统模型的环境信息发生变化，则噪声参数将会发生变化导致滤波器性能下降或直接发散。其实时性较差、响应速度较慢、不能在短时间内变速，且目标丢失的机率较大，与坡道跟随技术的结合较差。本发明设计了一种基于新息自适应卡尔曼滤波器的目标重识别方法，利用在跟随过程中的新息项数据对噪声进行自适应，在目标丢失后能够准确快速地找回，并与坡道跟随系统相结合，更好地实现人机跟随。

本发明通过数据集对改进前后的卡尔曼滤波进行了实验对比验证，图3为传统卡尔曼滤波目标跟踪效果图。图4为本发明新息自适应卡尔曼滤波目标跟踪效果图。图5为传统卡尔曼滤波与本发明新息自适应卡尔曼滤波的误差对比图。图6为传统卡尔曼滤波与本发明新息自适应卡尔曼滤波精度对比图。通过对比表明，本发明新息自适应卡尔曼滤波比传统卡尔曼滤波在滤波误差和精度上都得到了明显提升。

上述卡尔曼滤波器本质上是对目标丢失情况下的预测和找回。事实上，在目标没有丢失的情况下，卡尔曼滤波器也一直在后台运行，只是此时没有使用其输出的预测值而已，但预测值和新息自适应状态噪声矩阵R

最后需要说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管申请人参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，均应涵盖在本发明的权利要求范围当中。