CN109872372B - 一种小型四足机器人全局视觉定位方法和系统 - Google Patents

Abstract

本发明公开了一种小型四足机器人全局视觉定位方法和系统，该方法包括以下步骤：标定双摄像头系统，计算双摄像头系统运动模型参数；获取标定好的鱼眼摄像头拍摄的目标区域图像；对目标区域图像进行处理，得到待定位目标在鱼眼摄像头坐标系中的位置，通过双摄像头系统运动学模型计算待定位目标在云台坐标系中的空间位置，并进行二自由度运动学反解，得到二自由度关节转角；获取待定位目标图像，并对待定位目标图像中的二维码进行检测，得到二维码中心点在待定位图像中的二维坐标，经过变换得到二维码中心点在地面坐标系中二维坐标；对二维码中心点在地面坐标系中二维坐标进行校正，得到待定位目标在地面坐标系中的位置和朝向。

Description

一种小型四足机器人全局视觉定位方法和系统

技术领域

本公开涉及一种小型四足机器人全局视觉定位方法和系统。

背景技术

移动机器人是机器人家族的重要成员，在军事对抗、农业工作、工业生产、安防巡检等领域都有重要应用。而移动机器人的定位和导航是实现机器人智能化自主化的关键技术，是机器人自身设计功能正常运作的基础。实现移动机器人自主导航，首先要解决机器人的定位定姿问题，即确定移动机器人的位置、朝向等。

在定位定姿方法中，视觉定位应用较为广泛。视觉定位与导航获取的信息丰富，应用灵活，视觉传感器被动接受能量，系统能耗低，且易于隐蔽，因此成为移动机器人导航的一个重要研究方向。

全局视觉通过架设在高处的摄像头，对机器人及其环境进行全局观测。在全局视觉定位导航过程中，有时既需要宽广的观察视野，对机器人周围大范围内的环境信息进行观测，规划可行路径，又需要能对某一局部细节进行清晰的观测，根据机器人本体的细节特征进行定位定姿，在多机协同问题中区分不同个体。发明人在研发过程中发现，由于单一摄像机覆盖范围有限，增加全局相机高度可以扩大视野，但是目标在图像中会变小变模糊，观测视野范围和细节高清晰度很难兼顾。

发明内容

为了克服上述现有技术的不足，本公开提供了一种小型四足机器人全局视觉定位方法和系统，扩大了全局视觉定位的有效范围，提高了大视野环境中的定位精度，解决了全局视觉观测范围有限的问题。

本公开所采用的技术方案是：

一种四足机器人全局视觉定位方法，该方法包括以下步骤：

标定由鱼眼摄像头和长焦摄像头组成的双摄像头系统，搭建双摄像头系统运动模型，计算双摄像头系统运动模型参数；

获取标定好的鱼眼摄像头拍摄的目标区域图像；

利用基于核函数加权的粒子滤波算法对所述目标区域图像进行处理，得到待定位目标在图像上的位置数据；

将待定位目标的位置数据经过变换得到待定位目标在鱼眼摄像头坐标系中的位置，通过双摄像头系统运动学模型计算待定位目标在云台坐标系中的空间位置，并进行二自由度运动学反解，得到二自由度关节转角；

获取根据二自由度关节转角控制标定好的长焦摄像头拍摄的待定位目标的放大图像，并对待定位目标放大图像中的二维码进行检测，得到二维码中心点在待定位目标放大图像中的二维坐标；经过变换得到二维码中心点在地面坐标系中二维坐标；

对二维码中心点在地面坐标系中二维坐标进行校正，得到待定位目标在地面坐标系中的位置和朝向。

一种四足机器人全局视觉定位系统，该系统包括：

双摄像头系统，用于拍摄待定位目标所在的目标区域图像以及待定位目标图像；

处理器，用于标定由鱼眼摄像头和长焦摄像头组成的双摄像头系统，搭建双摄像头系统运动模型，计算双摄像头系统运动模型参数；获取标定好的鱼眼摄像头拍摄的目标区域图像；利用基于核函数加权的粒子滤波算法对所述目标区域图像进行处理，得到待定位目标在图像上的位置数据；将待定位目标的位置数据经过变换得到待定位目标在鱼眼摄像头坐标系中的位置，通过双摄像头系统运动学模型计算待定位目标在云台坐标系中的空间位置，并进行二自由度运动学反解，得到二自由度关节转角；获取根据二自由度关节转角控制标定好的长焦摄像头拍摄的待定位目标图像，并对待定位目标图像中的二维码进行检测，得到二维码中心点在待定位图像中的二维坐标；经过变换得到二维码中心点在地面坐标系中二维坐标；对二维码中心点在地面坐标系中二维坐标进行校正，得到待定位目标上的二维码中心在地面坐标系中的三维空间坐标。

通过上述的技术方案，本公开的有益效果是：

(1)本公开解决了全局视觉定位导航过程中存在的增大观测视野导致目标细节模糊的问题，通过双摄像头分工配合，将观测范围与观测精度解耦，能够在大视野观测的同时进行高精度的细节观测；

(2)本公开扩大了全局视觉定位的有效范围，提高了大视野环境中的定位精度，解决了全局视觉观测范围有限的问题；

(3)本公开具备较好的扩展性，通过提高系统安装高度和换装更长焦距的长焦摄像头，可以进一步扩展观测视野，提高定位精度；

(4)本公开结构简单，应用灵活，使用两个普通高清摄像头即可达到极强的细节分辨率；

(5)本公开不仅可以应用于大场景下移动目标的定位导航、多机协作、全局视觉指导下腿足机器人复杂地形的自动通过等研究领域，也可以应用于道路监控、军事侦察等特殊领域。

附图说明

构成本公开的一部分的说明书附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。

图1是根据一种或多种实施方式的小型四足机器人全局视觉定位方法流程图；

图2是根据一种或多种实施方式的双摄像头系统运动模型示意图一；

图3是根据一种或多种实施方式的双摄像头系统运动模型参数标定示意图；

图4是根据一种或多种实施方式的旋转标定模型示意图；

图5是根据一种或多种实施方式的云台运动学反解模型；

图6是根据一种或多种实施方式的二维码检测过程流程图；

图7是根据一种或多种实施方式的二维码中心坐标与朝向示意图；

图8是根据一种或多种实施方式的双摄像头系统运动模型示意图二；

图9是根据一种或多种实施方式的机器人站立状态示意图。

图10是根据一种或多种实施方式的双摄像头系统结构图。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本公开提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本公开的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

一种或多种实施例提供了一种小型四足机器人全局视觉定位方法，获取目标区域内大范围场景图像，采用基于检测核函数加权RGB颜色直方图的粒子滤波算法对目标区域图像进行处理，得到目标区域内四足机器人跟踪结果，根据目标跟踪结果采集四足机器人的图像，通过轮廓检测算法对采集的图像进行处理，检测出四足机器人背部粘贴的二维码，识别二维码中心和朝向，通过系统坐标转换与空间位置关系，实时计算四足机器人在地面上的位置与朝向。

请参阅附图1，该小型四足机器人全局视觉定位方法包括以下步骤：

S101，标定双摄像头系统内短焦鱼眼摄像头和长焦摄像头，并利用标定好的短焦鱼眼镜头采集大范围目标区域图像。

在本实施例中，采用苏黎世大学DavideScaramuzza编写的OCamCalib全视角相机模型标定矫正工具对短焦鱼眼摄像头进行标定。

具有地，标定短焦鱼眼摄像头的具体实现方式如下：

设ρ为入射光线在图像上的成像点(u，v)与图像主点(0，0)之间的距离，入射光线方向向量为[X，Y，Z]^T，按照正交模型，入射光线方向向量与在图像上的成像点坐标(u，v)之间的关系为：

入射光线的入射角与其像点到光轴像点间的距离ρ有关，因此F是关于ρ的函数。

可对F进行泰勒展开，写成一般形式并取4次幂，即

F＝f(ρ)＝a₀+a₁ρ+a₂ρ²+a₃ρ³+a₄ρ⁴(2)

其中

通过OcamCalib工具对鱼眼摄像头进行标定，获取多项式系数a₀、…、a₄的值。

在本实施例中，使用Matlab相机标定工具标定长焦摄像头内参数矩阵M，获取长焦摄像头的镜头焦距f、图像主点坐标(u₀,v₀)等参数。

长焦摄像头内参数矩阵M为：

其中，f为长焦摄像头的镜头焦距，(u₀,v₀)为图像主点坐标。

在本实施例中，采用标定好的鱼眼摄像头采集待定位四足机器人所在目标区域内的图像。

S102，搭建双摄像头系统运动模型，计算双摄像头系统运动模型参数。

请参阅附图2，所述双摄像头系统运动模型包括云台坐标系、鱼眼摄像头坐标系和长焦摄像头坐标系，所述双摄像头系统运动模型参数包括地面坐标系W与鱼眼镜头坐标系F的变换矩阵

鱼眼镜头坐标系F与云台无转动时长焦镜头坐标系L₀之间的变换矩阵

以及云台坐标系和与其固接的长焦摄像头坐标系之前的变换矩阵

通过标定板标定和空间解算，得到双摄像头系统运动模型内部各坐标系的相对位置关系，获得精确的运动模型参数。

请参阅附图3，所述步骤102中，计算双摄像头系统运动模型参数，其具体采用如下方式实现：

(1)在两个标定板一角分别建立地面坐标系W与W’，坐标系轴向与标定板矩形边框重合，同朝向放置两个标定板使x_W轴与x_w‘轴平行，y_w轴与y_w‘轴平行，并确定W坐标系与W’坐标系的变换矩阵

此时，控制云台处于零转动角度状态。

(2)标定地面坐标系W与鱼眼摄像头坐标系F之间的位置关系，计算变换矩阵

在实施例中，对地面坐标系W与鱼眼摄像头坐标系F之间的位置关系标定，使用OCamCalib标定工具标定鱼眼摄像头与该地面坐标系W的变换矩阵

(3)标定鱼眼摄像头坐标系F与云台无转动时长焦摄像头坐标系L₀之间的位置关系，计算变换矩阵

在本实施例中，对鱼眼摄像头坐标系F与云台无转动时长焦摄像头坐标系L₀之间的变换矩阵

标定，采用双棋盘格标定方法，通过Matlab相机标定工具标定长焦摄像头与W’标定板的转换矩阵

根据下式可计算得到

(4)标定云台坐标系H和与其固接的长焦摄像头坐标系L之间的位置关系，计算变换矩阵

请参阅附图4，控制云台末端关节角度为0度，此时云台末端连杆坐标系为H₀，与云台坐标系H重合。长焦长焦摄像头坐标系为L₀，拍摄标定板W′的图像；控制云台末端关节旋转90度，云台末端连杆坐标系转动至H′，长焦标定板坐标系转动到L′，再次拍摄标定板W′的图像，并标定两个标定板图像的外参矩阵

由于云台旋转90度，因此坐标系H′相对于H₀的变换矩阵

已知，为：

因此可以获得如下方程：

由于长焦摄像头固定在云台末端，因此云台末端连杆坐标系与长焦镜头坐标系之间的位姿关系为常值矩阵，即

上式可改写为

该式为Sylvester方程，通过迭代算法求解其最小二乘解，可解得

继而，结合式(5)可以求得云台坐标系H相对于鱼眼镜头坐标系F的变换矩阵为：

S103，对目标区域图像进行处理，得到目标跟踪结果。

在本实施例中，采用基于检测核函数加权RGB颜色直方图的粒子滤波算法进行动态待定位四足机器人跟踪，获取待定位四足机器人在目标区域图像中的坐标。

具体地，所述步骤103中，对目标区域内图像进行处理，得到目标跟踪结果，其具体实现方式如下：

S103-1，采用核函数对目标区域图像内像素点进行加权处理，得到目标区域图像的RGB颜色直方图。

由于目标区域图像边缘可能是背景或者其他非目标像素点，为防止目标区域图像边缘像素的变动对颜色直方图模型的影响，采用核函数对目标区域图像内的像素点进行加权处理，加权过程如下所示：

其中，r是某像素点相对于目标区域图像中心的距离，H_x是目标区域图像的宽度，H_y是目标区域图像的长度。

计算目标点所在位置的区域图像RGB颜色直方图，采用巴氏距离(Bhattacharyya距离)描述该颜色直方图分布与目标图像直方图模板的相似程度。巴氏距离(Bhattacharyya距离)表示为：

其中，p、q为两个离散分布。

S103-2，采用粒子滤波算法对目标区域图像的RGB颜色直方图进行处理，得到待定位目标的位置数据。

具体地，采用粒子滤波算法对目标区域图像的RGB颜色直方图进行处理，得到待定位四足机器人的位置坐标，其具体实现方式如下：

(1)给定t-1时刻的N个采样点组成粒子集合S_t-1，各个采样点的权重

设为1/N；获取t-1时刻目标区域图像的RGB颜色直方图。

(2)重采样：在集合S_t-1中，舍弃掉权重较小的采样点，在权重较大的采样点中选择N个样本(粒子可重复选择)组成一个新的集合S′_t-1。

(3)预测：根据系统模型状态方程，预测集合S′_t-1中各个采样点在t时刻的状态，构成新的粒子集合S_t。

(4)测量：获取集合S_t中每一个采样点所在位置处的颜色直方图，并与t-1时刻的目标区域颜色直方图进行比较，计算巴氏距离

确定各个采样点的权重值。

在本实施例中，t时刻，采样点的权重值为：

(5)估计：根据各个采样点的权重值，采用蒙特卡洛方法估算集合S_t的期望，该期望为目标跟踪输出的目标位置数据。

(6)更新目标模型：在获得的目标跟踪输出的位置数据，计算该位置区域图像的RGB颜色直方图，作为t+1时刻的目标区域颜色直方图，计算t+1时刻的目标区域颜色直方图与t时刻的标区域颜色直方图之间的巴氏距离，更新目标点权重。循环执行上述步骤，即可对运动目标点进行跟踪，得到连续N帧目标位置数据。

在本实施例中，为提高目标跟踪的稳定性和抗干扰能力，采用基于历史运动参数的轨迹预测算法和基于均匀采样的丢失重检算法对目标跟踪结果进行处理，解决目标被遮挡时跟踪算法结果不稳定、不平滑的问题，以及目标跟踪丢失之后的快速重检测问题。

具体地，所述采用基于历史运动参数的轨迹预测方法对目标跟踪结果进行处理的具体实现过程如下：

建立一个固定长度N的二维缓存区，保存粒子滤波算法输出的连续N帧目标位置数据；

求取N帧数据的一阶导数和二阶导数，获得目标区域图像中待定位四足机器人在x轴、y轴方向的速度与加速度离散曲线，并进行快速线性拟合；

根据拟合曲线预测待定位四足机器人在被遮挡过程中的走向，并作为目标跟踪结果参与后续定位计算。

具体地，采用基于均匀采样的丢失重检算法对目标区域图像进行处理的具体实现过程如下：

当追踪待定位四足机器人丢失或被遮挡，向目标区域图像中均匀撒播采样点，并实时计算每一个目标点处的RGB颜色直方图；

将每一个采样点处的RGB颜色直方图与目标图像模板直方图进行比较，计算巴氏距离，并更新每一个采样点的权重；

设置重检测阈值，当某个采样点的权重超过阈值，则判断采样点复现，根据所有采样点权重大小进行重采样，恢复粒子滤波算法正常跟踪过程。

S104，根据目标跟踪结果，计算待定位四足机器人在云台坐标系中的空间位置，并进行二自由度运动学反解，得到二自由度关节转角。

在本实施例中，将步骤103得到的目标跟踪结果经过变换得到待定位四足机器人在鱼眼摄像头坐标系中的粗略位置，通过双摄像头系统运动学模型计算待定位四足机器人在云台坐标系中的空间位置，并对二自由度关节舵机进行运动学反解，得到两个关节舵机的转动角度。

具体地，根据目标跟踪结果计算待定位四足机器人在鱼眼摄像头坐标系中的粗略位置，其具体实现方式如下：

待定位四足机器人在图像上的成像点为(u,v)，根据公式(1)、(2)、(3)得到鱼眼摄像头坐标系中待定位四足机器人与坐标系原点之间的向量(X_F,Y_F,Z_F)。待定位四足机器人在鱼眼摄像头坐标系中的位置为该向量与地面坐标系O-X-Y平面的交点。

由标定板上任意三个非共线角点在地面坐标系中的坐标(x_w1,y_w1,0)、(x_w2,y_w2,0)、(x_w3,y_w3,0)，通过公式(14)可计算得到这三点在鱼眼摄像头坐标系中的三维空间坐标(x_F1,y_F1,z_F1)、(x_F2,y_F2,z_F2)、(x_F3,y_F3,z_F3)。

通过三个非共线角点确定地面坐标系O-X-Y平面在鱼眼摄像头坐标系中的方程。

Ax+By+Cz+D＝0(15)

其中

将向量所在空间直线方程与O-X-Y平面方程联立可求得交点(x_w,y_w,z_w)。

其中

请参阅附图5，通过双摄像头系统运动学模型，计算待定位四足机器人在云台坐标系中的空间位置(x_H,y_H,z_H)，并进行二自由度运动学反解，得到水平关节转角

和俯仰关节转角θ，其表达式为：

S105，采集待定位四足机器人的放大图像，并对待定位四足机器人放大图像中的二维码进行检测。

在本实施例中，采用OpenCV开源计算机视觉库，对待定位四足机器人的图像中的二维码进行检测。

请参阅附图6，所述步骤105中，采集待定位四足机器人的放大图像，并对待定位四足机器人的放大图像中的二维码进行检测，其具体实现方式如下：

S105-1，采集待定位四足机器人的放大图像。

在本实施例中，根据两个自由度关节的转动角度，控制两个关节舵机转动，使得长焦摄像头锁定运动待定位四足机器人，采集待定位四足机器人的放大图像。

S105-2，对采集到的待定位四足机器人放大图像进行预处理。

在本实施例中，所述步骤105-2，分别对采集到的待定位四足机器人的放大图像进行滤波去噪、灰度化、二值化处理。

S105-3，采用边缘检测算子(Canny算子)对预处理后的待定位四足机器人放大图像进行边缘检测。

S105-4，采用OpenCV库函数对边缘提取处理后的放大图像进行轮廓提取。

S105-5，保存得到的轮廓信息，通过树结构链表进行检索，得到二维码三个角部的“回”形定位标中心坐标。

在本实施中，所述树结构链表中保存了本轮廓ID、父轮廓、子轮廓和同层轮廓的ID。

S105-5，根据二维码三个角部的“回”形定位标志中心坐标，计算二维码中心在待定位四足机器人图像中的位置坐标与朝向。

请参阅附图7，经过图像预处理、边缘检测、轮廓提取和链表检索后，得到二维码三个定位标志中心坐标分别为A(x_A,y_A)、B(x_B,y_B)、C(x_C,y_C)，由公式(18)可得二维码中心点P坐标(x_P,y_P)。

计算AB边中点P′坐标(x_P′,y_P′)，计算PP′线的斜率，通过Atan2函数求得二维码朝向角。

S106，根据双摄像头系统运动学模型对待定位四足机器人进行位姿计算，结合待定位四足机器人的高度数据，求解待定位四足机器人在地面坐标系中的精确位置。

请参阅附图8，坐标系F为鱼眼摄像头坐标系，坐标系H为云台坐标系，坐标系H′为云台末端连杆坐标系，其原点与H坐标系重合，z_h′轴方向为云台末端连杆方向。

当云台位于零角度状态时，H′系与H系重合。当云台开始转动时，H′坐标系首先相对于z_h′轴旋转

角，然后相对于x_h′轴旋转θ角。因此H′相对于H的旋转变换矩阵

位置矢量^Hp可写为

由上式可得H′坐标系相对于H坐标系的变换矩阵为：

通过式(9)(19)(21)，可以计算云台转动之后长焦摄像头坐标系相对于鱼眼摄像头坐标系的变换矩阵：

式中，

为经标定得到的常值矩阵。

由上述步骤计算得到二维码中心点P在图像中的坐标(x_P,y_P)，采用长焦摄像头的参数矩阵M计算得到二维码中心点P与长焦摄像头坐标系原点O之间的向量

向量

起点为长焦摄像头坐标系原点O，其坐标为(0,0,0)。将向量

变换至鱼眼摄像头坐标系中，求其坐标，具体实现如下：

将向量

起点O变换至鱼眼摄像头坐标系中，为：

向量变换至鱼眼摄像头坐标系中，为：

根据变换后的向量与向量起点，确定

向量在鱼眼摄像头坐标系中的空间直线方程，与地面坐标系O-X-Y平面方程联立求解交点坐标，即可获得P点在鱼眼摄像头坐标系中的精确空间位置。

将P点从鱼眼摄像头坐标系中变换至地面坐标系，即可得到P点的定位结果。其中，P点从鱼眼摄像头坐标系中变换至地面坐标系为：

其中，(x_PW,y_PW)即为四足机器人的定位结果。

由于四足机器人站立有一定的高度，而标定板放置的位置是紧贴地面，因此需要结合四足机器人站立高度，对定位结果进行校正。

请参阅附图9，经过之前的计算，图中的向量

坐标

及其所在直线在鱼眼镜头坐标系中的方程、地平面方程及该平面法向量

坐标(A,B,C)、二维码中心点P距离地平面的高度h、向量

与地平面的交点P′坐标(x_P′,y_P′,z_P′)均已求知。

C点为二维码中心点，在鱼眼镜头坐标系中坐标为(x_C,y_C,z_C)；P点为二维码中心在地平面的投影点。由几何关系可知向量

在平面法向量

中的投影长度为h。由此可得方程

将此方程与

所在直线方程联立，可求得C点在鱼眼镜头坐标系中的三维空间坐标，为：

其中

通过坐标变换，得到C点在地面坐标系中的三维空间坐标(x_CW,y_CW,z_CW)，即获得定位结果。

定姿计算采用同样的方法，计算二维码中A、B两定位标连线中点P′在地面坐标系中的三维空间坐标，计算其与二维码中心点的向量

方向，即可确定实际的朝向角。

本实施例提出的小型四足机器人全局视觉定位方法，解决了全局视觉定位导航过程中存在的增大观测视野导致目标细节模糊的问题，通过双摄像头分工配合，将观测范围与观测精度解耦，能够在大视野观测的同时进行高精度的细节观测；扩大了全局视觉定位的有效范围，提高了大视野环境中的定位精度，解决了全局视觉观测范围有限的问题。

一种或多种实施例还提供了一种小型四足机器人全局视觉定位系统，采用短焦鱼眼摄像头采集目标区域内大范围场景图像，通过基于检测核函数加权RGB颜色直方图的粒子滤波算法对目标区域图像进行处理，得到目标区域内四足机器人跟踪结果，实现对目标区域内四足机器人进行动态跟踪，根据目标跟踪结果转动二自由度云台，控制二自由度云台搭载的长焦摄像头锁定运动的四足机器人，进行放大成像，采集小型四足机器人的图像，通过轮廓检测算法对长焦摄像头采集的图像进行处理，检测出四足机器人背部粘贴的二维码，识别二维码中心和朝向，通过系统坐标转换与空间位置关系，实时计算四足机器人在地面上的位置与朝向。

该小型四足机器人全局视觉定位系统包括双摄像头系统和处理器，其中：

所述双摄像头系统，用于拍摄待定位目标所在的目标区域图像以及待定位目标图像。

所述处理器，用于标定由鱼眼摄像头和长焦摄像头组成的双摄像头系统，搭建双摄像头系统运动模型，计算双摄像头系统运动模型参数；获取标定好的鱼眼摄像头拍摄的目标区域图像；利用基于核函数加权的粒子滤波算法对所述目标区域图像进行处理，得到待定位目标在图像上的位置数据；将待定位目标的位置数据经过变换得到待定位目标在鱼眼摄像头坐标系中的位置，通过双摄像头系统运动学模型计算待定位目标在云台坐标系中的空间位置，并进行二自由度运动学反解，得到二自由度关节转角；获取根据二自由度关节转角控制标定好的长焦摄像头拍摄的待定位目标图像，并对待定位目标图像中的二维码进行检测，得到二维码中心点在待定位图像中的二维坐标；经过变换得到二维码中心点在地面坐标系中二维坐标；对二维码中心点在地面坐标系中二维坐标进行校正，得到待定位目标上的二维码中心在地面坐标系中的三维空间坐标。

请参阅附图10，所述双摄像头系统包括云台、短焦鱼眼摄像头和长焦摄像头，短焦鱼眼摄像头与二自由度云台平行安装，长焦镜头固定在云台末端。云台使用高精度伺服舵机搭建，伺服精度可达0.088度。短焦鱼眼摄像头焦距为1.78mm，视野范围170度；长焦摄像头焦距12mm。双摄像头系统的运动模型建模如图2所示，坐标系F为短焦鱼眼摄像头坐标系，H为云台坐标系，L为长焦摄像头坐标系，W为世界坐标系。

本实施例提出的小型四足机器人全局视觉定位系统，具备较好的扩展性，通过提高系统安装高度和换装更长焦距的长焦摄像机，可以进一步扩展观测视野，提高定位精度，结构简单，应用灵活，使用两个普通高清摄像机即可达到极强的细节分辨率，不仅可以应用于大场景下移动目标的定位导航、多机协作、全局视觉指导下腿足机器人复杂地形的自动通过等研究领域，也可以应用于道路监控、军事侦察等特殊领域。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (8)

1.一种四足机器人全局视觉定位方法，其特征是，包括以下步骤：

标定由鱼眼摄像头和长焦摄像头组成的双摄像头系统，搭建双摄像头系统运动模型，计算双摄像头系统运动模型参数；

所述双摄像头系统运动模型包括云台坐标系、鱼眼摄像头坐标系和长焦摄像头坐标系；

计算双摄像头系统运动模型参数的步骤包括：

以两个标定板所在空间构建地面坐标系W与W’，确定地面坐标系W与地面坐标系W’的变换矩阵

标定地面坐标系W与鱼眼摄像头坐标系F之间的位置关系，计算变换矩阵

标定鱼眼摄像头坐标系F与云台无转动时长焦摄像头坐标系L₀之间的位置关系，计算变换矩阵

标定云台坐标系H和与其固接的长焦摄像头坐标系L之间的位置关系，计算变换矩阵

获取标定好的鱼眼摄像头拍摄的目标区域图像；

利用基于核函数加权的粒子滤波算法对所述目标区域图像进行处理，得到连续N帧待定位目标的位置数据，采用基于历史运动参数的轨迹预测方法对得到的N帧待定位目标的位置坐标进行处理，得到待定位目标在图像上的位置数据；

所述采用基于历史运动参数的轨迹预测方法对得到的N帧待定位目标的位置数据进行处理的步骤包括：

建立一个固定长度的二维缓存区，保存得到的连续N帧待定位目标的位置坐标；求取N帧待定位目标的位置坐标数据的一阶导数和二阶导数，得到目标区域图像中待定位目标在x轴、y轴方向的速度与加速度离散曲线，并进行线性拟合；根据拟合曲线预测待定位目标在被遮挡过程中的走向；

将待定位目标的位置数据经过变换得到待定位目标在鱼眼摄像头坐标系中的位置，通过双摄像头系统运动学模型计算待定位目标在云台坐标系中的空间位置，并进行二自由度运动学反解，得到二自由度关节转角；

获取根据二自由度关节转角控制标定好的长焦摄像头拍摄的待定位目标的放大图像，并对待定位目标放大图像中的二维码进行检测，得到二维码中心点在待定位目标放大图像中的二维坐标；经过变换得到二维码中心点在地面坐标系中二维坐标；

对二维码中心点在地面坐标系中二维坐标进行校正，得到待定位目标在地面坐标系中的位置和朝向。

2.根据权利要求1所述的四足机器人全局视觉定位方法，其特征是，所述利用基于核函数加权的粒子滤波算法对所述目标区域图像进行处理的步骤包括：

利用核函数对目标区域图像内像素点进行加权处理，得到t-1时刻目标区域图像的颜色直方图；

给定t-1时刻的N个采样点组成集合S_t-1，各个采样点的权重

设为1/N；

在集合S_t-1中，选择N个权重大的采样点组成一个新的集合S′_t-1；

预测集合S′_t-1中各个采样点在t时刻的状态，构成新的集合S_t；

获取集合S_t中每一个采样点所在位置处的颜色直方图，并与t-1时刻的目标区域颜色直方图进行比较，计算巴氏距离

确定各个采样点的权重值；

根据各个采样点的权重值，采用蒙特卡洛方法估算集合S_t的期望，将该期望为待定位目标的位置数据；

重复上述步骤，得到连续N帧待定位目标的位置数据。

3.根据权利要求2所述的四足机器人全局视觉定位方法，其特征是，还包括采用基于均匀采样的丢失重检算法对目标区域图像进行处理的步骤；

所述采用基于均匀采样的丢失重检算法对目标区域图像进行处理的步骤包括：

向目标区域图像中均匀撒播采样点，并实时计算每一个采样点处的颜色直方图；

将每一个采样点处的颜色直方图与目标图像模板直方图进行比较，计算巴氏距离，并更新每一个采样点的权重；

设置重检测阈值，当某个采样点的权重超过阈值，则判断采样点复现，根据所有采样点权重大小进行重采样，利用基于核函数加权的粒子滤波算法对所有目标点进行处理。

4.根据权利要求2所述的四足机器人全局视觉定位方法，其特征是，所述将待定位目标的位置数据经过变换得到待定位目标在鱼眼摄像头坐标系中的位置的步骤包括：

以标定板所在空间构建地面坐标系并确定标定板上任意三个非共线角点在地面坐标系中的三维空间坐标；

计算这三点在鱼眼摄像头坐标系中的三维空间坐标，确定地面坐标系O-X-Y平面在鱼眼摄像头坐标系中的方程；

计算鱼眼摄像头坐标系中待定位目标与其坐标系原点之间的向量的三维空间坐标；

将鱼眼摄像头坐标系中待定位目标与其坐标系原点之间的向量于地面坐标系O-X-Y平面方程相结合，得到待定位目标在鱼眼摄像头坐标系中的位置。

5.根据权利要求2所述的四足机器人全局视觉定位方法，其特征是，所述对待定位目标图像中的二维码进行检测的步骤包括：

对待定位目标图像进行预处理，包括滤波去噪、灰度化和二值化；

利用边缘检测算子对预处理后的待定位目标图像进行边缘检测；

采用OpenCV库函数对边缘提取处理后的图像进行轮廓提取；

利用链表对提前的轮廓信息进行检索，得到二维码的三个定位标志中心坐标；

根据二维码的三个定位标志中心坐标，计算二维码中心在待定位目标图像中的位置坐标与朝向角。

6.根据权利要求2所述的四足机器人全局视觉定位方法，其特征是，所述经过变换得到二维码中心点在地面坐标系中二维坐标的步骤包括：

计算云台转动之后长焦摄像头坐标系相对于鱼眼摄像头坐标系的变换矩阵

采用长焦摄像头的参数矩阵M计算得到二维码中心点P与长焦摄像头坐标系原点O之间的向量

将向量

变换至鱼眼摄像头坐标系中，求解向量

和原点O在鱼眼摄像头坐标系中三维空间坐标，确定向量

在鱼眼摄像头坐标系中的空间直线方程；

将向量

在鱼眼摄像头坐标系中的空间直线方程与地面坐标系O-X-Y平面方程联立，得到二维码中心点P在鱼眼摄像头坐标系中的空间位置；

根据地面坐标系W与鱼眼摄像头坐标系F之间的变换矩阵

将二维码中心点P从鱼眼摄像头坐标系中变换至地面坐标系，得到二维码中心点在地面坐标系中二维坐标。

7.根据权利要求2所述的四足机器人全局视觉定位方法，其特征是，所述对二维码中心点在地面坐标系中坐标进行校正的步骤包括：

根据待定位目标的高度，计算二维码中心点在鱼眼镜头坐标系中的三维空间坐标；

经过坐标变换，得到二维码中心点在地面坐标系中的三维空间坐标，即获得待定位目标在地面坐标系中的位置。

8.一种四足机器人全局视觉定位系统，其特征是，包括：

双摄像头系统，用于拍摄待定位目标所在的目标区域图像以及待定位目标图像；

处理器，用于标定由鱼眼摄像头和长焦摄像头组成的双摄像头系统，搭建双摄像头系统运动模型，计算双摄像头系统运动模型参数；获取标定好的鱼眼摄像头拍摄的目标区域图像；利用基于核函数加权的粒子滤波算法对所述目标区域图像进行处理，得到待定位目标在图像上的位置数据；将待定位目标的位置数据经过变换得到待定位目标在鱼眼摄像头坐标系中的位置，通过双摄像头系统运动学模型计算待定位目标在云台坐标系中的空间位置，并进行二自由度运动学反解，得到二自由度关节转角；获取根据二自由度关节转角控制标定好的长焦摄像头拍摄的待定位目标图像，并对待定位目标图像中的二维码进行检测，得到二维码中心点在待定位图像中的二维坐标；经过变换得到二维码中心点在地面坐标系中二维坐标；对二维码中心点在地面坐标系中二维坐标进行校正，得到待定位目标上的二维码中心在地面坐标系中的三维空间坐标。

Images