一种基于RGBD的目标定位方法.pdf

本发明提供了一种基于RGBD的目标定位方法，主要步骤是硬件环境搭设，设备初始化；利用RGBD传感器获取当前帧移动机器人的深度数据和彩色图像，基于深度数据对深度图像进行分割，得到深度图像中的目标区域，设为SearchRect_depth，并将其映射到彩色图像中，对应的彩色图像部分记为SearchRectROI；在彩色图像SearchRectROI中使用CAMSHIFT方法计算目标在彩色图像中的位置(x，y)；结合深度数据计算当前目标的世界坐标(xw，yw，zw)；将当前目标的世界坐标(xw，yw，zw)通过通讯系统发送给移动机器人，机器人控制自身运动。本发明提供的基于RGBD的目标定位方法具有计算速度快、定位准确的优点，对于环境的改变具有较强的适应能力，针对于不同目标，具有可扩展性强的优点。

1.一种基于RGBD的目标定位方法，其特征在于，包括以下步骤：
硬件环境搭设，设备初始化；
利用RGBD传感器获取当前帧移动机器人的深度数据和彩色图像，基于深度数据对深度
图像进行分割，得到深度图像中的目标区域，设为SearchRect_depth，并将其映射到彩色图
像中，对应的彩色图像部分记为SearchRectROI；
在彩色图像SearchRectROI中使用CAMSHIFT方法计算目标在彩色图像中的位置(x，y)；
结合深度数据计算当前目标的世界坐标(xw，yw，zw)；
将当前目标的世界坐标(xw，yw，zw)通过通讯系统发送给移动机器人，机器人控制自身
运动。
2.如权利要求1所述的基于RGBD的目标定位方法，其特征在于所述硬件环境搭设，包括
以下步骤：
将RGB传感器固定在天花板上，使其前面板平行于地面且与地面距离在0.8米到4米之
间；
将彩色标记固定在移动机器人中心。
3.如权利要求1所述基于RGBD的目标定位方法，其特征在于所述基于深度数据对深度
图像进行分割，包括以下步骤：
记前一帧定位结果(xw，yw，zw)中目标深度值zw为depth_pre，当前帧深度图像中图像任
意点(i,j)的深度为depth(i,j)，获得当前帧深度数据；
记深度阈值为T，深度图像宽为depth_cols，深度图像长为depth_rows，深度图像中某
一像素点灰度值为depth_seg(i，j)，对当前帧深度数据进行遍历，按如下公式对深度图像
进行深度图像分割：
$depth\_seg(i,j)=\left\{\begin{array}{c}0,\left|depth\right(i,j)-depth\_pre|\>T\\ 1,\left|depth\right(i,j)-depth\_pre|\<T\end{array}\right.$
其中i＝0,1,2…depth_cols，j＝0,1,2…depth_rows；
在经过分割的深度图像depth_seg(i，j)中进行连通域检测，选取面积最大的连通域，
并获得面积最大的连通域的外接矩形，即为深度图像中的目标区域SearchRect_depth；
通过深度图像与彩色图像坐标对应关系，从彩色图像中得到与SearchRect_depth对应
的搜索矩形区域SearchRectROI。
4.如权利要求3所述的基于RGBD的目标定位方法，其特征在于所述深度图像与彩色图
像坐标对应关系为RGB传感器通过镜头矫正得到的深度数据与彩色图像像素点之间的映射
关系，已知彩色图像中的一点(i,j)，通过这种映射关系可以得到该点的深度信息depth(i,
j)。
5.如权利要求1所述的基于RGBD的目标定位方法，其特征在于在所述彩色图像
SearchRectROI中使用CAMSHIFT方法计算跟踪目标中心位置O，即(x，y)，包括以下步骤：
如果为初次运行，则需要进行目标初始化，手动选取目标跟踪矩形框trackWindow_
init，跟踪目标应具有特定的颜色特征，本发明中跟踪目标为固定于机器人中心的彩色标
记；
对SearchRectROI中进行CAMSHIFT搜索，得到新的目标位置trackWindow，计算
trackWindow的面积trackWindow.area，若trackWindow.area大于阈值T_area，则表示跟踪
成功，获得trackWindow形心坐标(x，y)，若trackWindow.area小于阈值T_area，则表示跟踪
失败，重新初始化trackWindow_init为上一帧的跟踪结果trackWindow_pre，并重新进行
CAMSHIFT搜索，得到新的目标位置trackWindow。
6.如权利要求5所述的基于RGBD的目标定位方法，其特征在于所述阈值为初始
trackWindow_init面积的30％，即
T_area＝0.3*trackWindow_init.area。
7.如权利要求1所述的基于RGBD的目标定位方法，其特征在于所述结合深度数据计算
当前目标的世界坐标(xw，yw，zw)，包括以下步骤：
记W和H分别为RGB传感器彩色图像的宽和高，将图像坐标转换成以图像坐标为原
点的坐标值(x1，y1)，按如下公式计算:
$y_1=\frac{H}{2}-y,x_1=\frac{W}{2}-x;$
记RGBD彩色摄像头的视野范围为水平视角αh，垂直视角αv，由此可求得每像素对应的视
场角度为:
水平：垂直：
进而求得当前像素点(x1，y1)所对应的水平转角θh和竖直转角θv：
$\begin{array}{cc}{\mathrm{\θ}}_h=\stackrel{\‾}{{\mathrm{\α}}_h}\·x_1& {\mathrm{\θ}}_v=\stackrel{\‾}{{\mathrm{\α}}_v}\·y_1\end{array};$
根据深度信息zw可求得当前目标的世界坐标(xw，yw，zw)：
xw＝zw·tanθh yw＝zw·tanθv。

一种基于RGBD的目标定位方法

技术领域

本发明属于视觉定位领域，具体涉及一种基于RGBD的目标定位方法。

背景技术

移动机器人自身定位能力是实现真正高性能高精度导航的基础和关键，其能够进
行自主移动并完成给定的作业任务，必须实时获得自身的精确位置和姿态。目前移动机器
人都配备了GPS和惯性导航系统，其技术已经相对成熟，有很多优秀的产品能够很方便地采
购到。虽然GPS和惯性导航系统有诸多优点，然而也有一些值得关注的缺点:

(1)GPS数据一般是1秒钟更新一次，无法满足机器人快速运动的要求。

(2)当机器人处于一些特定姿态的时候，如背飞、俯冲和失重情况下，往往无法接
收到GPS信号。

(3)在室内环境中，无法接收到GPS信号。

研究移动机器人控制算法和环境感知等方法时往往需要在室内进行实验，而在室
内以及一些GPS无法使用的环境下，机器人往往无法获知自身位置信息，以及天气情况，云
层厚度也有可能影响GPS信号的接收。另外，机器人自身负载的变化也会使得机器人的质量
分布和平衡条件改变，使得移动机器人的控制难度增大。因此，适用于室内的，具有精确，快
速，自主，廉价及适应性好等特点的基于机器视觉定位方法成为当前移动机器人的研究热
点。

发明内容

为克服现有技术的不足，本发明设计了一种基于RGBD的目标定位方法，本方法具
有计算速度快、定位准确的优点，对于环境的改变具有较强的适应能力。

为实现上述技术方案，本发明提供了一种基于RGBD的目标定位方法，包括以下步
骤：

硬件环境搭设，设备初始化；

利用RGBD传感器获取当前帧移动机器人的深度数据和彩色图像，基于深度数据对
深度图像进行分割，得到深度图像中的目标区域，设为SearchRect_depth，并将其映射到彩
色图像中，对应的彩色图像部分记为SearchRectROI；

在彩色图像SearchRectROI中使用CAMSHIFT方法计算目标在彩色图像中的位置
(x，y)；

结合深度数据计算当前目标的世界坐标(xw，yw，zw)；

将当前目标的世界坐标(xw，yw，zw)通过通讯系统发送给移动机器人，机器人控制
自身运动。

优选的，所述硬件环境搭设，包括以下步骤：

将RGB传感器固定在天花板上，使其前面板平行于地面且与地面距离在0.8米到4
米之间；

将彩色标记固定在移动机器人中心。

优选的，所述基于深度数据对深度图像进行分割，包括以下步骤：

记前一帧定位结果(xw，yw，zw)中目标深度值zw为depth_pre，当前帧深度图像中图
像任意点(i,j)的深度为depth(i,j)，获得当前帧深度数据；

记深度阈值为T，深度图像宽为depth_cols，深度图像长为depth_rows，深度图像
中某一像素点灰度值为depth_seg(i，j)，对当前帧深度数据进行遍历，按如下公式对深度
图像进行深度图像分割：

$depth\_seg(i,j)=\left\{\begin{array}{c}0,\left|depth\right(i,j)-depth\_pre|\>T\\ 1,\left|depth\right(i,j)-depth\_pre|\<T\end{array}\right.$

其中i＝0,1,2…depth_cols，j＝0,1,2…depth_rows；

在经过分割的深度图像depth_seg(i，j)中进行连通域检测，选取面积最大的连通
域，并获得面积最大的连通域的外接矩形，即为深度图像中的目标区域SearchRect_depth；

通过深度图像与彩色图像坐标对应关系，从彩色图像中得到与SearchRect_depth
对应的搜索矩形区域SearchRectROI。

优选的，所述深度图像与彩色图像坐标对应关系为RGB传感器通过镜头矫正得到
的深度数据与彩色图像像素点之间的映射关系，已知彩色图像中的一点(i,j)，通过这种映
射关系可以得到该点的深度信息depth(i,j)。

优选的，所述彩色图像SearchRectROI中使用CAMSHIFT方法计算跟踪目标中心位
置O，即(x，y)，包括以下步骤：

如果为初次运行，则需要进行目标初始化，手动选取目标跟踪矩形框
trackWindow_init，跟踪目标应具有特定的颜色特征，本发明中跟踪目标为固定于机器人
中心的彩色标记；

对SearchRectROI中进行CAMSHIFT搜索，得到新的目标位置trackWindow，计算
trackWindow的面积trackWindow.area，若trackWindow.area大于阈值T_area，则表示跟踪
成功，获得trackWindow形心坐标(x，y)，若trackWindow.area小于阈值T_area，则表示跟踪
失败，重新初始化trackWindow_init为上一帧的跟踪结果trackWindow_pre，并重新进行
CAMSHIFT搜索，得到新的目标位置trackWindow。

优选的，所述阈值为初始trackWindow_init面积的30％，即

T_area＝0.3*trackWindow_init.area。

优选的，所述结合深度数据计算当前目标的世界坐标(xw，yw，zw)，包括以下步骤：

记W和H分别为RGB传感器彩色图像的宽和高，将图像坐标转换成以图像坐标
为原点的坐标值(x1，y1)，按如下公式计算:

$y_1=\frac{H}{2}-y,x_1=\frac{W}{2}-x;$

记RGBD彩色摄像头的视野范围为水平视角αh，垂直视角αv，由此可求得每像素对应
的视场角度为:

水平： 垂直：

进而求得当前像素点(x1，y1)所对应的水平转角θh和竖直转角θv：

$\begin{array}{cc}{\mathrm{\θ}}_h=\stackrel{\‾}{{\mathrm{\α}}_h}\·x_1& {\mathrm{\θ}}_v=\stackrel{\‾}{{\mathrm{\α}}_v}\·y_1\end{array};$

根据深度信息zw可求得当前目标的世界坐标(xw，yw，zw)：

xw＝zw·tanθh yw＝zw·tanθv。

与现有技术相比，本发明采用的技术方案具有下述有益效果：

本发明所述方法所使用到的硬件设备简单，成本低廉，仅涉及RGBD传感器和配备
有Windows操作系统和相应RGBD传感器接口的计算机。在目标跟踪方面，通过对CAMSHIFT算
法进行深度数据融合的改进，大大降低了算法对光照变化的敏感性，增强了跟踪算法的鲁
棒性和实时性，显著削弱了背景物体与目标颜色相近的干扰。相比其他接触式机器人定位
方法，本方法采用非接触式测量方法，计算速度可达每秒25帧，具有计算速度快、定位准确
的优点；对于环境的改变具有较强的适应能力，针对于不同目标，本方法具有可扩展性强的
优点。

附图说明

图1为本发明方法整体流程图。

图2为本发明方法融合深度数据的图像分割流程图。

图3为本发明方法改进CAMSHIFT跟踪方法流程图。

具体实施方式

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

实施例：一种基于RGBD的目标定位方法。

本实施例中，采用第二代Kinect传感器，其中包括RGB摄像头，红外线接收器，红外
线发射器和麦克风阵列，记RGBD传感器世界坐标系为OnXnYnZn，简称N坐标系，N坐标系原点
是传感器彩色摄像头中心点，N坐标系的Z轴垂直于Kinect前面板，X轴沿前面板向左，Y轴沿
前面板向下。

参照图1所示，一种基于RGBD的目标定位方法，包括以下步骤：

硬件环境搭设，设备初始化；

利用RGBD传感器获取当前帧飞行器在飞行过程中的深度图像和彩色图像，基于深
度数据对深度图像进行分割，得到深度图像中的目标区域，设为SearchRect_depth，并将其
映射到彩色图像中，对应的彩色图像部分记为SearchRectROI；

在彩色图像SearchRectROI中使用CAMSHIFT方法计算目标在彩色图像中的位置
(x，y)；

结合深度数据计算当前目标的世界坐标(xw，yw，zw)；

将当前目标的世界坐标通过通讯系统ZigBee发送给飞行器，飞行器控制自身运
动。

本实施例中，所述硬件环境搭设，包括以下步骤：

将RGB传感器固定在天花板上，使其前面板平行于地面且与地面距离在0.8米到4
米之间；

将彩色标记固定在移动机器人中心。

参照图2所示，所述基于深度数据对深度图像进行分割，包括以下步骤：

记前一帧定位结果(xw，yw，zw)中目标深度值zw为depth_pre，当前帧深度图像中图
像任意点(i,j)的深度为depth(i,j)，获得当前帧深度数据；

记深度阈值为T，深度图像宽为depth_cols，深度图像长为depth_rows，深度图像
中某一像素点灰度值为depth_seg(i，j)，对当前帧深度数据进行遍历，按如下公式对深度
图像进行深度图像分割：

$depth\_seg(i,j)=\left\{\begin{array}{c}0,\left|depth\right(i,j)-depth\_pre|\>T\\ 1,\left|depth\right(i,j)-depth\_pre|\<T\end{array}\right.$

其中i＝0,1,2…depth_cols，j＝0,1,2…depth_rows；

在经过分割的深度图像depth_seg(i，j)中进行连通域检测，选取面积最大的连通
域，并获得面积最大的连通域的外接矩形，即为深度图像中的目标区域SearchRect_depth；

通过深度图像与彩色图像坐标对应关系，从彩色图像中得到与SearchRect_depth
对应的搜索矩形区域SearchRectROI。

本实施例中，所述深度图像与彩色图像坐标对应关系为RGB传感器通过镜头矫正
得到的深度数据与彩色图像像素点之间的映射关系，已知彩色图像中的一点(i,j)，通过这
种映射关系可以得到该点的深度信息depth(i,j)。

参照图3所示，所述彩色图像SearchRectROI中使用CAMSHIFT方法计算跟踪目标中
心位置O，记其图像坐标(x，y)，包括以下步骤：

目标初始化，手动选取目标跟踪矩形框trackWindow_init，本发明中跟踪目标位
四旋翼飞行器中心的绿色面板；

对SearchRectROI中进行CAMSHIFT搜索，得到新的目标位置trackWindow，计算
trackWindow的面积trackWindow.area，若trackWindow.area大于阈值T_area，则表示跟踪
成功，根据目标中心在彩色图像中的位置即trackWindow矩形框的中心点，即(x，y)，若
trackWindow.area小于阈值T_area，则表示跟踪失败，重新初始化trackWindow_init为上
一帧的跟踪结果trackWindow_pre，并重新进行CAMSHIFT搜索，得到新的目标位置
trackWindow。

本实施例中，所述阈值为初始trackWindow_init面积的30％，即

T_area＝0.3*trackWindow_init.area。

本实施例中，所述结合深度数据计算当前目标在N坐标系中的坐标(xw，yw，zw)，包
括以下步骤：

记W和H分别为RGB传感器彩色图像的宽和高，在Kinect中W＝1920，H＝1080，将图
像坐标转换成以图像坐标为原点的坐标值(x1，y1)，按如下公式计算:

$y_1=\frac{H}{2}-y,x_1=\frac{W}{2}-x;$

记RGBD彩色摄像头的视野范围为水平视角αh，垂直视角αv，在Kinect中αh＝70°，αv
＝60°，由此可求得每像素对应的视场角度为:

水平： 垂直：

进而求得当前像素点(x1，y1)所对应的水平转角θh和竖直转角θv：

$\begin{array}{cc}{\mathrm{\θ}}_h=\stackrel{\‾}{{\mathrm{\α}}_h}\·x_1& {\mathrm{\θ}}_v=\stackrel{\‾}{{\mathrm{\α}}_v}\·y_1\end{array};$

根据Kinect深度信息zw可求得当前目标的世界坐标(xw，yw，zw)：

xw＝zw·tanθh yw＝zw·tanθv。

以上所述为本发明的较佳实施例而已，但本发明不应局限于该实施例和附图所公
开的内容，所以凡是不脱离本发明所公开的精神下完成的等效或修改，都落入本发明保护
的范围。