第5期 贺超，等：采用Kinect的移动机器人目标跟踪与避障 ·429 2.2目标跟踪 具体意义是当移动机器人的运动方向与目标产 目标跟踪算法采用基于颜色的粒子滤波算法， 生的吸引力方向相差的角度δ很大时，相应的移动 在光学图像上完成跟踪.在算法中使用数量为N的 机器人旋转的角速度也应该很大：当角度δ相差很 粒子集P.(xy,,),i=1,2,…,V,其中，xy 小时，旋转角速度也很小，δ的符号决定了ω的方 表示第i个粒子在图像上的坐标，：表示当前目标 向.对于直线速度v的模糊控制规则与之类似，但是 区域与初始选取的目标区域的比例尺，地：代表该粒 在计算与目标的距离时需要减去D,以保证移动机 子的权重. 器人与目标的距离限制.具体意义是当移动机器人 目标跟踪算法如下： 距离目标很远时，运动速度很大：距离适中时，运动 1)选取目标计算目标区域的颜色直方图并初 速度很小；随着距离d的不断变小（负向增大），运 始化粒子集P:,其中x:y:的初始值设为选取的目 动速度也负向增大 标区域的中心，：初始值设为1. 对于图5中的模糊规则，为了描述方便，设定对 2)粒子采样.根据高斯分布对当前粒子进行采 应角速度模糊规则的隶属度函数为R,对应隶属 样，分别得到新的xy:和· 度为1时的角速度为w,i=1,2,3,4,5;设定对应直 3)计算权值.根据采样得到的新x:y:和x,计算 线速度的模糊规则的隶属度函数为R,对应隶属度 对应图像区域的颜色直方图，并与1)中获取的原始 为1时的直线速度为，j=1,2,3,4,5.那么根据以 颜色直方图进行比较，采用巴氏距离计算出相应的 上规则可以计算出单纯目标跟踪时，使用的角速度 和直线速度的控制规律为： 权值0，并进行归一化处理，使0，=0，/∑， 5 4)重采样.根据权值e:,舍弃一定数量的小权 ∑u,×R() W= (2) 值粒子并衍生出新的粒子，然后重新回到2)： R(6) 当移动机器人只执行目标跟踪即不存在障碍物 i=1 的影响时，对移动机器人的移动速度和角速度ω ，×Rd 采用模糊规则进行控制，以确保良好的跟踪效果.图 (3) 5为角速度ω和线速度v进行模糊控制的隶属度函 R,(d) i=1 数，其中δ为移动机器人方向与目标之间的夹角，d 2.3人工势场法避障 为移动机器人距离目标的距离减去设定的跟踪距离 移动机器人的避障使用人工势场法完成，该方 D(在目标跟踪时不能将机器人移动到目标所在位 法简单实用、物理意义明确，因而受到了广泛研究与 置，应该使移动机器人与目标之间保持一定的距 使用其基本思想是在规划环境中构造虚拟力场，障 离)，S2、S1、CE、B1、B2表示5条模糊规则的隶属度 碍物对移动机器人产生排斥力，目标点对移动机器 函数 人产生吸引力，最终移动机器人在合力的作用下避 ◆u(cd 开障碍物向目标移动.人工势场法经过多年的研究， S2 S1 cE1.0 B2 在理论上已经比较成熟，这里使用的人工势场法的 具体公式为 Fm=-k(q-qsm）: -400 -200 0 200400 d/mm (a)线速度的隶属度函数 k(-马)19-9 p(q)p.pi(q)p(q) ,p(q)≤p; u(6 0 p(q)≥P。 S2 CE 1.0 B1 B2 (4) 式中：F、F分别为移动机器人受到的吸引力和 -30 -15 0 15 30 δfdeg 斥力，kkp为正的比例因子，9为移动机器人当 (b)角速度ω的求属度函数 前位置，9m为目标所在位置，9为障碍物位置， 图5模糊控制隶属度函数 p(q)为移动机器人当前位置距离障碍物位置的欧 Fig.5 The fuzzy controller's membership functions 式距离.由式(4)可以看出，当移动机器人距离障碍２.２ 目标跟踪 目标跟踪算法采用基于颜色的粒子滤波算法ꎬ 在光学图像上完成跟踪.在算法中使用数量为 Ｎ 的 粒子集 Ｐｉ(ｘｉꎬｙｉꎬｚｉꎬｗｉ)ꎬｉ ＝ １ꎬ２ꎬ􀆺ꎬＮꎬ 其中ꎬ ｘｉ、ｙｉ 表示第 ｉ 个粒子在图像上的坐标ꎬ ｚｉ 表示当前目标 区域与初始选取的目标区域的比例尺ꎬ ｗｉ 代表该粒 子的权重. 目标跟踪算法如下: １)选取目标.计算目标区域的颜色直方图并初 始化粒子集 Ｐｉꎬ 其中 ｘｉ、ｙｉ 的初始值设为选取的目 标区域的中心ꎬ ｚｉ 初始值设为 １. ２)粒子采样.根据高斯分布对当前粒子进行采 样ꎬ分别得到新的 ｘｉ、ｙｉ 和 ｚｉ . ３)计算权值.根据采样得到的新 ｘｉ、ｙｉ 和 ｚｉ 计算 对应图像区域的颜色直方图ꎬ并与 １)中获取的原始 颜色直方图进行比较ꎬ采用巴氏距离计算出相应的 权值 ｗｉ 并进行归一化处理ꎬ使 ｗｉ ＝ ｗｉ /∑ Ｎ ｉ ＝ １ ｗｉ . ４)重采样.根据权值 ｗｉꎬ 舍弃一定数量的小权 值粒子并衍生出新的粒子ꎬ然后重新回到 ２). 当移动机器人只执行目标跟踪即不存在障碍物 的影响时ꎬ对移动机器人的移动速度 ｖ 和角速度 ω 采用模糊规则进行控制ꎬ以确保良好的跟踪效果.图 ５ 为角速度 ω 和线速度 ｖ 进行模糊控制的隶属度函 数ꎬ其中 δ 为移动机器人方向与目标之间的夹角ꎬｄ 为移动机器人距离目标的距离减去设定的跟踪距离 Ｄ(在目标跟踪时不能将机器人移动到目标所在位 置ꎬ应该使移动机器人与目标之间保持一定的距 离)ꎬＳ２、Ｓ１、ＣＥ、Ｂ１、Ｂ２ 表示 ５ 条模糊规则的隶属度 函数. 图 ５ 模糊控制隶属度函数 Ｆｉｇ.５ Ｔｈｅ ｆｕｚｚｙ ｃｏｎｔｒｏｌｌｅｒ’ｓ ｍｅｍｂｅｒｓｈｉｐ ｆｕｎｃｔｉｏｎｓ 具体意义是当移动机器人的运动方向与目标产 生的吸引力方向相差的角度 δ 很大时ꎬ相应的移动 机器人旋转的角速度也应该很大ꎻ当角度 δ 相差很 小时ꎬ旋转角速度也很小ꎬδ 的符号决定了 ω 的方 向.对于直线速度 ｖ 的模糊控制规则与之类似ꎬ但是 在计算与目标的距离时需要减去 Ｄꎬ以保证移动机 器人与目标的距离限制.具体意义是当移动机器人 距离目标很远时ꎬ运动速度很大ꎻ距离适中时ꎬ运动 速度很小ꎻ随着距离 ｄ 的不断变小(负向增大)ꎬ运 动速度也负向增大. 对于图 ５ 中的模糊规则ꎬ为了描述方便ꎬ设定对 应角速度模糊规则的隶属度函数为 Ｒωｉꎬ 对应隶属 度为 １ 时的角速度为 ωｉꎬ ｉ ＝ １ꎬ２ꎬ３ꎬ４ꎬ５ꎻ设定对应直 线速度的模糊规则的隶属度函数为 Ｒｖｊꎬ 对应隶属度 为 １ 时的直线速度为 ｖｊꎬ ｊ ＝ １ꎬ２ꎬ３ꎬ４ꎬ５.那么根据以 上规则可以计算出单纯目标跟踪时ꎬ使用的角速度 和直线速度的控制规律为: ω ＝ ∑ ５ ｉ ＝ １ ωｉ × Ｒωｉ(δ) ∑ ５ ｉ ＝ １ Ｒωｉ(δ) ꎬ (２) ｖ ＝ ∑ ５ ｊ ＝ １ ｖｊ × Ｒｖｊ(ｄ) ∑ ５ ｊ ＝ １ Ｒｖｊ(ｄ) . (３) ２.３ 人工势场法避障 移动机器人的避障使用人工势场法完成ꎬ该方 法简单实用、物理意义明确ꎬ因而受到了广泛研究与 使用.其基本思想是在规划环境中构造虚拟力场ꎬ障 碍物对移动机器人产生排斥力ꎬ目标点对移动机器 人产生吸引力ꎬ最终移动机器人在合力的作用下避 开障碍物向目标移动.人工势场法经过多年的研究ꎬ 在理论上已经比较成熟ꎬ这里使用的人工势场法的 具体公式为 Ｆａｔｔ ＝ － ｋａｔｔ(ｑ － ｑｇｏａｌ)ꎻ Ｆｒｅｐ ＝ ｋｒｅｐ( １ ρ(ｑ) － １ ρｏ ) １ ρ ２ (ｑ) ｑ － ｑｏｂｓｔａｃｌｅ ρ(ｑ) ꎬ ρ(ｑ) ≤ ρｏꎻ ０ꎬ ρ(ｑ) ≥ ρｏ . ì î í ï ï ïï (４) 式中: Ｆａｔｔ、Ｆｒｅｐ 分别为移动机器人受到的吸引力和 斥力ꎬ ｋａｔｔ、ｋｒｅｐ 为正的比例因子ꎬｑ 为移动机器人当 前位置ꎬｑｇｏａｌ为目标所在位置ꎬｑｏｂｓｔａｃｌｅ为障碍物位置ꎬ ρ(ｑ)为移动机器人当前位置距离障碍物位置的欧 式距离.由式(４)可以看出ꎬ当移动机器人距离障碍 第 ５ 期 贺超ꎬ等:采用 Ｋｉｎｅｃｔ 的移动机器人目标跟踪与避障 􀅰４２９􀅰