[发明专利]一种小型四足机器人全局视觉定位方法和系统

权利要求书

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所述的四足机器人全局视觉定位方法，其特征是，还包括采用基于均匀采样的丢失重检算法对目标区域图像进行处理的步骤；

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

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

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

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