CN102682445A - 一种仿蜥蜴亚目避役科生物视觉坐标提取算法 - Google Patents

Abstract

本发明属于计算机视觉范畴，尤其是一种能够同时进行目标跟踪，定位与摄像头标定的计算机视觉算法。本发明提出了一套应用于仿照蜥蜴亚目避役科生物视觉***构架的主动、立体视觉***的摄像头标定方法，目标跟踪与定位方法。其最主要的特点为摄像头标定，兴趣目标的跟踪与定位是同步进行的，其摄像头的标定过程无需要求兴趣目标的持续静止，亦无需在目标跟踪与定位之前执行。本发明所采取的标定方法化连续的标定过程为一个离散的标定过程，利用目标在视觉算法运行过程中离散的静止状态进行摄像头的标定，降低了摄像头标定的要求且实现了视觉算法与标定过程的同步进行。其自标定的特性减少了人工的标定工作，大幅简化了算法的前期准备工作。

Description

一种仿蜥蜴亚目避役科生物视觉坐标提取算法

技术领域

本发明属于计算机视觉范畴，尤其是一种能够同时进行目标跟踪，定位与摄像头标定的计算机视觉算法。其仿照的生物***是蜥蜴亚目避役科生物的视觉***。

背景技术

计算机视觉是人工智能领域重要的研究方向，相较于其他信息检测手段而言，基于计算机视觉理念所构架的检测装置具有信息量大、成本低、对环境无干扰，便于控制等特点。视觉***的构架方式有很多，以摄像头的数量分类可以分为：单目视觉，双目视觉，多目视觉；以机械结构约束条件可以分为：主动视觉，被动视觉；以视觉信息之间的关联程度可以分为：普通视觉，立体视觉等。

以最简单的单目视觉来说，其具有结构简单，视觉和控制算法简单，成本低廉的优点。但由于基于普通单目视觉的视觉过程是一个由高维的现实世界向低维的图像空间映射的一个过程，此过程中不可避免的要损失掉很多信息，其中最重要的便是常规的单目视觉无法获取目标的深度信息。

基于双目视觉的立体视觉可以弥补普通单目视觉无法获取深度信息这一缺陷。通过保证视觉***中两台固定的摄像机之间具有适宜的共同视域，再辅以精确的摄像头标定与立体匹配算法，基于双目视觉的立体视觉***即可得到目标的深度信息。但此种视觉***构架方式本质上仍然是一种开环***，其在运行视觉算法时无法通过改变自身的参数以适应环境的变化。

解决这一缺陷，令视觉***变为一种稳定的闭环***的方法便是以主动视觉的架构方式来架构视觉***，即给摄像头增加一定的运动能力，使视觉***能够以其获得的图像信息为反馈量，驱动摄像头转动，闭环的控制整个视觉***。

由于基于主动视觉理念架构的视觉***与传统的基于被动视觉理念架构的视觉***在机械结构上已有所不同，因此其适用的深度信息获取方式，兴趣目标定位方式，摄像头标定方式也与被动视觉所采取的方法有所不同。对于以主动视觉理念架构的视觉***来说，如果需要进行后续的视觉算法处理，其首先需要对所使用的摄像头进行标定。而常规的标定方式要求在视觉算法运行前执行，且要求标定过程中兴趣目标静止不动。这两个要求使得主动视觉***的使用过程变得繁琐，特别是当视觉***中的摄像头经常更换，或是***使用变焦摄像头时会使视觉处理过程异常繁琐。

发明内容

本发明提出了一套应用于仿照蜥蜴亚目避役科生物视觉***构架的主动、立体视觉***的摄像头标定方法，目标跟踪与定位方法。其最主要的特点为摄像头标定，兴趣目标的跟踪与定位是同步进行的，其摄像头的标定过程无需要求兴趣目标的持续静止，亦无需在目标跟踪与定位之前执行。所应用的视觉***采用广角定焦摄像头构架，且两台广角定焦摄像头可以相互独立的运动。

本发明所应用的仿蜥蜴亚目避役科生物视觉***其总体结构可以概述如下：

整个视觉***仿照蜥蜴亚目避役科生物的视觉***架构：其主要构件为2台广角定焦摄像头，每台广角定焦摄像头配备了2台步进电机为其提供独立的水平方向与竖直方向的2自由度运动能力。相较于常规的视觉***，其具有的两个广角定焦摄像头之间的水平运动是相互独立的，俯仰运动也是独立的。其仿照的生物***是蜥蜴亚目避役科(俗称变色龙)生物的视觉***，其视觉***的特点为两眼球可互相独立的运动，而不是像灵长类一样相关运动。特别注意的是，每台广角定焦摄像头与其配套的2台步进电机三者各自的几何中心点是在同一条垂直于水平面的直线上的，这一特点保证了后文将要详细介绍的深度信息及世界坐标提取算法的顺利进行。

本发明主要内容为通过控制两个广角定焦摄像头协同工作，调用下文详述的角度信息标定学习算法、深度信息及世界坐标提取算法，并以Cam shift跟踪算法作为辅助算法，对仿蜥蜴亚目避役科生物视觉***进行运动控制和参数计算，最终实时地获取视觉***跟踪目标相对于视觉***两广角定焦摄像头各自的几何中心点连线中点的世界坐标。

本发明的技术方案为：

1、令两广角定焦摄像头分别搜索目标，当A摄像头发现目标后，启用基于Cam shift算法的单目视觉跟踪算法保持对目标的跟踪，并实时返回A摄像头此时像平面法向量相对于A摄像头初始位置时摄像头像平面法向量的水平夹角和竖直夹角；所述的A摄像头为首先搜索到目标的摄像头，B摄像头为另一台摄像头；所述的初始位置为：使广角定焦摄像头的像平面法向量与水平面相互平行，并且使两台广角定焦摄像头的像平面法向量垂直于两广角定焦摄像头几何中心点连线所构成的线段的位置；

2、B摄像头跟随A摄像头进行目标搜索，B摄像头搜索到目标后，亦启用基于Cam shift算法的单目视觉跟踪算法保持对目标的跟踪；

3、调用角度信息标定学***面法向量相对于各自初始位置摄像头像平面法向量的水平夹角和竖直夹角；

4、根据步骤3)得到的结果，使用深度信息及世界坐标提取算法，实时计算并输出目标深度信息及目标在世界坐标系中的世界坐标；所述的世界坐标系为：以两广角定焦摄像头几何中心点连线的中点为原点，以与水平面平行、垂直于两广角定焦摄像头几何中心点连线、且沿视觉追踪的方向为x轴正方向，以垂直于水平面向上的方向为z轴正方向的右手坐标系；

5、若目标未丢失，返回步骤3)，若果目标丢失，则返回步骤1)。

角度信息标定学习算法包括以下步骤：

2.1)初始化学习入口角度(Δθ₁、Δη₁)

所述的学***角度和竖直角度；

2.2)分别计算在摄像头图像中，摄像头图像中心点与目标的水平距离P和竖直距离Q相对于摄像头图像主对角线长度的百分比f(P)和g(Q)；

2.3)判断目标是否在摄像头图像中心点，

若(|f(P)|＜ε)∧(|g(Q)|＜ε)，阈值ε＞0，则目标在中心点，跳转至步骤2.7)；

若(|f(P)|＞ε)∨(|g(Q)|＞ε)，阈值ε＞0，则目标不在中心点，跳转至步骤2.4)；

2.4)查标定信息表，

若标定信息表中存在对应于|f(P)|和|g(Q)|的角度信息，则算法跳转至步骤2.8)；

若标定信息表中不存在对应于|f(P)|和|g(Q)|的角度信息，则跳转至步骤2.5)；

2.5)逐步转动摄像头，使目标在摄像头图像中与图像中心点重合；

2.6)当目标与摄像头图像中心点重合时，跳转至步骤2.7)；否则返回步骤2.5)；

2.7)当目标与图像中心点重合时，读出摄像头此时的像平面法向量相对于该摄像头初始位置摄像头像平面法向量的水平夹角θ和竖直夹角η并输出，转至步骤2.10)；

2.8)若标定信息表中存在对应于|f(P)|和|g(Q)|的角度信息，则摄像头转动该角度使目标几何中心点在摄像头图像中与图像中心点重合，转动方向根据标定信息换算示意表确定；

2.9)输出世界坐标系下，目标相对于初始位置摄像头法向量的水平夹角和竖直夹角；延时T时间，其中T为预先设置的延时量，返回步骤2.2)；

2.10)通过预设学习标志位判断是否学习完毕，

若学习完毕，则转至步骤2.2)；

若未学习完毕，则算法继续，开始标定；

2.11)判断是否是第一次标定，

如果学习入口角度等于初始值则为第一次标定，此时算法继续；

如果学习入口角度不等于初始值则不为第一次标定，此时跳转至步骤2.13)；

2.12)令摄像头在水平方向和竖直方向分别转动(θ_ε，η_ε)，θ_ε，η_ε为阈值角度，跳转至步骤2.14)

2.13)摄像头转过前一次标定中断时记录的学习入口角度，跳转至步骤2.14)；

2.14)若为初始标定，将摄像头在水平方向和竖直方向分别转动的阈值角度(θ_ε，η_ε)以及与其对应的摄像头图像内目标几何中心点的|f(P)|和|g(Q)|写入标定信息表；若不为初始标定，首先判断目标在进入此步骤前是否发生了运动；如果目标运动了，则更新学习入口角度，并跳转至步骤2.2)；如果目标没运动，则更新标定信息表；

2.15)保持摄像头竖直角度不变，进行行标定，每当摄像头在水平方向转过一个单位水平角度，判断此时目标几何中心点是否离开摄像头图像；若己离开，则转至步骤2.17)；若未离开，则继续算法；

2.16)判断摄像头在水平方向每转过一个单位水平角度后，目标是否移动，若没有发生移动，则更新标定信息表，并跳转至步骤2.15)；若发生移动，则更新学习入口角度，并跳转至步骤2.2)：

2.17)改变摄像头竖直角度，进行另一行的水平标定，如果目标几何中心点离开摄像头图像，则标定信息表建立完毕，转至步骤2.18)；若未离开，则转至步骤2.14)；

2.18)标定信息表建立完毕后，清除学习标志位，转至步骤2.2)；

所述的标定信息表为记录有摄像头成像标定信息的一张二维向量表；表中记录了目标位置信息，以及摄像头图像中，目标从原点移动到目标位置，摄像头像平面法向量对应转过的水平夹角和竖直夹角；所述的目标位置信息由摄像头图像中心点与目标的水平距离P和竖直距离Q相对于摄像头图像主对角线长度的百分比f(P)和g(Q)表示。

标定信息换算示意表可描述为：

首先将摄像头图像依照所在象限划分为4个区域：

其中imax为摄像头图像的行数；jmax为摄像头图像的列数；i指摄像头图像第i行；j指摄像头图像第j列；I_ij指摄像头图像中第i行第j列的像素点；

标定信息表只记录有I象限摄像头图像的标定信息，其他象限摄像头图像的标定信息可通过遵循标定信息换算示意表的规则计算得到；标定信息换算示意表如下所示：

I象限	IF：(f(P)＝|f(P)|)∧(g(Q)＝|g(Q)|)	|Δθ|	|Δη|
				II象限	IF：(-f(P)＝|f(P)|)∧(g(Q)＝|g(Q)|)	-|Δθ|	|Δη|
III象限	IF：(-f(P)＝|f(P)|)∧(-g(Q)＝|g(Q)|)	-|Δθ|	-|Δη|
				IV象限	IF：(f(P)＝|f(P)|)∧(-g(Q)＝|g(Q)|)	|Δθ|	|Δη|

深度信息及世界坐标提取算法包括以下步骤：

根据两广角定焦摄像头几何中心点之间的距离，以及根据角度信息标定学***面法向量相对于各自的初始位置摄像头法向量的水平夹角和竖直夹角，利用三角函数计算得到目标的世界坐标。

本发明的应用范围：

此套技术方案中的摄像头标定部分可以作为单目视觉的标定算法使用，亦可以化多目视觉为各自独立的单目视觉进行标定，降低摄像头的标定要求。配套的目标跟踪与定位方法可以应用于主动视觉***中。

本发明具有如下优点：

1)常规的主动视觉摄像头标定过程要求标定过程中目标静止不动且标定过程要先于图像处理过程。本发明所采取的标定方法化连续的标定过程为一个离散的标定过程，利用目标在视觉算法运行过程中离散的静止状态进行摄像头的标定，降低了摄像头标定的要求且实现了视觉算法与标定过程的同步进行。其自标定的特性减少了人工的标定工作，大幅简化了算法的前期准备工作。其自标定功能具有自动中断恢复功能，即使自标定过程中受到外界干扰，其亦能保存标定断点，待之后再次满足标定条件后继续完成自标定。

2)由于本发明所采用的视觉***架构方式为基于主动视觉的立体视觉架构方式，因此视觉***可以准确的获得目标的深度信息，并计算出相应的世界坐标。相较于常规的立体视觉***，其配套的深度信息及世界坐标提取算法对摄像头镜头的图像畸变具有很强的容错性，因此该***可以采用广角定焦摄像头来扩大其可视范围，且不必担心广角镜头引起的畸变会对深度信息及世界坐标的计算造成影响。

3)本发明所应用的视觉***至少拥有4个自由度，超过了常规视觉***的自由度数量。其主动视觉的特性使***构成了基于图像反馈的闭环控制***，使配套的视觉算法具有更强的环境适应能力。

附图说明

图1仿蜥蜴亚目避役科生物视觉***基础结构抽象示意图

图2仿蜥蜴亚目避役科生物视觉***扩展结构总体结构图

图3角度信息标定学***面示意图

图4X，Y坐标提取示意图

图5Z坐标提取示意图

图6总步骤流程图

图7角度信息标定学习算法流程图

图8角度信息标定学习算法学习过程示意图

图9角度信息标定学习算法学习过程流程图

图10标定信息表示意图

图11深度信息及世界坐标提取算法流程图

图中：1-广角定焦摄像头，2-竖直方向步进电机，3-水平方向步进电机，4-图像采集处理器，5-电机控制器。

具体实施方式

下面结合图1～图11，详细说明本实例。

1.实现仿蜥蜴亚目避役科生物视觉坐标提取算法的硬件平台及基本工作原理。

本实施例中，实现仿蜥蜴亚目避役科生物视觉坐标提取算法的硬件平台描述如下：

如图1所示，该硬件平台设有两台广角定焦摄像头(1)，竖直方向运动的步进电机(2)、水平方向运动的步进电机(3)、图像采集处理器(4)和电机控制器(5)。每台广角定焦摄像头(1)配有一个竖直方向运动的步进电机(2)和一台水平方向运动的步进电机(3)，其目的在于模仿蜥蜴亚目避役科生物的视觉***，尤其是模拟生物的眼球运动机制，具有竖直与水平方向的自由度。相对于常规视觉***来说，这种结构具有鲜明的主动视觉特点，有助于降低算法的复杂度。选择步进电机的原因是它相对于舵机可返回更精确的角度信息。

广角定焦摄像头(1)取得外界图像信息后发送给与之相连接的图像采集处理器(4)；图像采集处理器(4)经过相应预置算法处理后，将控制量发送至电机控制器(5)，通过电机控制器(5)驱动竖直方向运动的步进电机(2)、水平方向运动的步进电机(3)转动，进而驱动广角定焦摄像头(1)转动。

特别注意的是，广角定焦摄像头(1)的几何中心点与竖直方向运动的步进电机(2)和水平方向运动的步进电机(3)的几何中心点是在同一条垂直于水平面的直线上的，这一特点保证了后文将要详细介绍的深度信息及世界坐标提取算法的顺利进行。

如图2所示，在此基础视觉***上，在两台广角定焦摄像头连线中点处上方可加入一台具有水平和竖直方向2自由度运动能力的长焦变焦摄像头，且长焦变焦摄像头要高于广角定焦摄像头，使扩展的视觉***的识别能力得到提升。所加入的长焦变焦摄像头需在水平和竖直方向分别具有180度角度的运动能力来配合之后的识别步骤。其长焦变焦摄像头的长焦距，变焦段功能使得其可以保证待识别目标图像的清晰度与所占长焦变焦摄像头图像的百分比，充分利用摄像头的图像获取能力。其主要模仿人体视觉结构中的视网膜中心窝生理结构。

为有效解决目标易离开视觉***共同视域范围的问题，在前述硬件平台的底部可增加一台竖直方向运动的步进电机和一台水平方向运动的步进电机。新增的电机功率要大于前述步进电机。当目标即将离开视觉***图像边缘时，通过调整新增结构的转动角度，避免目标离开视觉***共同视域范围。这一结构主要模仿自蜥蜴亚目避役科生物的颈部结构，可辅助整个视觉***工作。新增加的长焦变焦摄像头和两台电机使整个***具有了更明显的主动视觉特征，更有利于防止算法运行过程中目标的丢失；具有了更清晰的目标图像细节，更有利于实现识别与导航类算法。

2、仿蜥蜴亚目避役科生物视觉坐标提取算法实现，如图6所示：

1)广角定焦摄像头位置初始化

首先令两台广角定焦摄像头转动至预先设定的初始化位置：即使广角定焦摄像头的像平面法向量与水平面相互平行，并且使两台广角定焦摄像头的像平面法向量垂直于两广角定焦摄像头几何中心点连线的位置；

2)仿照蜥蜴亚目避役科生物的视觉***，令两台广角定焦摄像头分别向预先设定的不同方向搜索目标，其中一台搜索到目标后，启用基于Cam shift算法的单目视觉跟踪算法保持对目标的跟踪，并实时返回该摄像头此时像平面法向量相对于该摄像头初始位置时摄像头像平面法向量的水平夹角和竖直夹角；

Cam shift算法能够提供目标中心点在当前图像中的位置以及目标在当前图像中所占的大小。

此仿生搜索方法结合广角定焦摄像头可以获得较大的可视搜索范围。此时两台广角定焦摄像头所采集的图像有可能是不具有共同视域的，处理时可视作2组相互独立的单目视觉视频序列。

为算法说明需要，假设首先搜索到目标的摄像头为广角定焦摄像头A。

令广角定焦摄像头B跟随广角定焦摄像头A进行目标搜索，摄像头B搜索到目标后，亦启用基于Cam shift算法的单目视觉跟踪算法保持对目标的跟踪；

本实施例中将广角定焦摄像头B跟随广角定焦摄像头A进行目标搜索的过程分解为竖直方向和水平方向的搜索。首先，令广角定焦摄像头B的竖直角度保持与广角定焦摄像头A相同；然后，广角定焦摄像头B开始水平方向搜索目标。水平方向搜索过程如下：

本实施例中水平方向运动的步进电机(3)为摄像头提供水平180度的转动角度。在世界坐标系中，定义广角定焦摄像头A像平面法向量与Y轴正方向夹角为α，广角定焦摄像头B像平面法向量与Y轴正方向夹角为β。当0°＜α≤90°时，广角定焦摄像头B从β＝0°处开始沿夹角增大方向扫描；当90°＜α＜180°时，广角定焦摄像头B从β＝180°处开始沿夹角减小方向扫描；

所述的世界坐标系为：以两广角定焦摄像头几何中心点连线的中点为原点，以与水平面平行、垂直于两广角定焦摄像头几何中心点连线、且沿视觉追踪的方向为X轴正方向，以垂直于水平面向上的方向为Z轴正方向的右手坐标系；

当广角定焦摄像头B搜索到目标后，亦启用基于Cam shift算法的单目视觉跟踪算法保持对目标的跟踪。

当两台广角定焦摄像头均启用跟踪算法后，若目标持续运动，则跟踪可能存在一定的跟踪余差和跟踪滞后。为消除算法的跟踪余差与跟踪滞后，可考虑扩展算法，通过应用PID算法消除跟踪余差与跟踪滞后。具体做法为：在摄像头当前图像中，把目标相对于摄像头当前图像中心点的水平距离和竖直距离作为反馈量，应用PID控制算法，使广角定焦摄像头跟踪目标更加精确。

3)调用角度信息标定学***面法向量相对于各自初始位置摄像头像平面法向量的水平夹角和竖直夹角；

4)使用角度信息标定学***面平行、垂直于两广角定焦摄像头几何中心点连线、且沿视觉追踪的方向为X轴正方向，以垂直于水平面向上的方向为Z轴正方向的右手坐标系；

5)输出目标世界坐标。若目标未丢失，则返回步骤3)；若目标丢失，则返回步骤2)。

若仿蜥蜴亚目避役科生物视觉坐标提取算法所应用的视觉***在底部增加了上文所描述的步进电机，则在两台广角定焦摄像头均启用基于Cam shift算法的单目视觉跟踪算法这一步骤后，若目标即将运动到两台广角定焦摄像头的共同视域范围之外时，可以通过调整新增电机在水平和竖直方向上的转动角度以保持目标在两台广角定焦摄像头的共同视域范围之内。

若仿蜥蜴亚目避役科生物视觉坐标提取算法所应用的视觉***扩展了上文所述的长焦变焦摄像头，则在步骤4)得到的目标世界坐标的基础上，进行坐标变换，将世界坐标系的原点移动到长焦变焦摄像头的几何中心点，并计算目标在新坐标系中的坐标。根据此坐标转动摄像头可使目标中心点与长焦变焦摄像头图像中心点近似重合，启用基于Cam shift算法的单目视觉跟踪算法保持长焦变焦摄像头对目标的跟踪，改变长焦变焦摄像头的焦段使目标占整个长焦变焦摄像头图像的百分比为预设的百分比，即可开始识别。

2.角度信息标定学习算法

仿蜥蜴亚目避役科生物视觉坐标提取算法中所述的角度信息标定学习算法可以描述如下：

图3所示为任意一台广角定焦摄像头在角度信息标定学习算法开始时采集到的初始图像。图中两条虚线交叉点处为摄像头成像中心点，以该中心点为原点，横向虚线为LX轴，纵向虚线为LY轴建立摄像头图像坐标系。上方球体为兴趣目标，其坐标为(P、Q)，整个图像横向长度记为X，纵向长度记为Y。

规定标志位：设Marker_learning为学习模式标志位。当Marker_learning值非0时，算法运行于学习模式。当Marker_learning值为0时，算法不运行学习模式。

算法目的：实时输出摄像头跟踪目标过程中，摄像头像平面法向量相对于自身初始位置摄像头像平面法向量的水平夹角和竖直夹角。

算法的流程图如图7所示。

角度信息标定学习算法是针对于单摄像头自标定行为的算法。在进行算法的步骤说明时，假设讨论的为广角定焦摄像头A以及它所对应的两台步进电机。但算法也同样适用于广角定焦摄像头B及它所对应的两台步进电机。

算法步骤如下：

1)初始化学***角度和竖直角度。学习入口角度用于记录角度信息标定学习算法中算法自标定的进度。当(Δθ₁≠θ_ε)ˇ(Δη₁≠η_ε)时，说明算法存储了一组自标定断点。当Δθ₁＝θ_ε，Δη₁＝η_ε时，说明算法并未存储自标定断点。

2)计算广角定焦摄像头图像中(图3)图像中心点与目标的水平距离P和竖直距离Q相对于图像主对角线长度

的百分比，即

其中X、Y分别表示摄像头图像的长和宽；P为正时表示目标位于图像中心点右方，P为负时表示目标位于目标中心点左方。Q为正时表示目标位于图像中心点上方，Q为负时表示目标位于目标中心点下方。

3)判断目标是否在摄像头图像中心点，即判断|f(P)|和|g(Q)|是否均小于一个大于0的预先设定的阈值ε。

若(|f(P)|＜ε)∧(|g(Q)|＜ε)，则目标与图像中心点近似重合，转至步骤7)，

若(|f(P)|＞ε)∨(|g(Q)|＞ε)，则目标与图像中心点不重合，算法继续。

其中，阈值ε用于判定目标是否已经与图像中心点近似重合。较大的阈值ε将会使算法具有更快的运行速度，但也会降低算法的精度。较小的阈值ε将会使算法具有更高的精度，但也会降低算法的运行速度，其具体大小需要根据实际角度精度需要和视觉***实时性要求调试得到，参考值：1/50。

4)若目标与图像中心点不重合，查标定信息表，判断标定信息表中相对于|f(P)|和|g(Q)|是否已经记录有对应的|Δθ|值和|Δη|值，若存在对应的|Δθ|值与|Δη|值，则算法跳转至步骤8)；若不存在对应的|Δθ|值和|Δη|值，则算法继续。

其中Δθ值是指在世界坐标系下，以摄像头几何中心点为起点，以目标几何中心点为终点的向量

在水平面上的投影与此时摄像机像平面法向量在水平面上的投影之间的水平夹角值。Δη值是指在世界坐标系下，以摄像头中心点为起点，以兴趣目标中心点为终点的向量

在垂直于水平面及两摄像头中心点连线的平面上的投影与此时摄像头法向量在垂直于水平面及两摄像头中心点连线的平面上的投影之间的竖直夹角值。

需要注意的是，此步骤中判定的依据为是否存在对应的|Δθ|和|Δη|值而非是否存在标定信息表的原因是：角度信息标定学习算法的自标定过程是可以中断的，其原理类似于计算机的中断处理。如果目标移动了，将标定断点的学习入口角度进行保存；跳出标定学习过程，回到主算法；当目标中心点与图像中心点再次重合，且目标静止时，调用学习入口角度返回学习过程。这也就意味着在此步骤判定时标定信息表中的标定信息可能是不完整的，而非完全存在或完全不存在。因此判定标定信息时需要具体判定每一个|f(P)|和|g(Q)|所对应的标定信息是否存在。

自标定过程加入断点返回机制的好处是显而易见的：1.它增强了算法的适应性，使标定过程由一个连续过程变为一个离散的标定过程，即不再要求目标在算法开始后首先保持静止以标定，而是利用算法运行过程中，目标离散的静止状态进行标定。2.它使得算法无需等待自标定过程完成即可正常运行，表现为算法运行过程中的角度信息输出频率将会随着自标定过程的进行而越来越实时化。3.它增强了算法的智能性。

5)逐步转动摄像头使在摄像头图像中目标与图像中心点重合

具体实现：逐步变化发送给步进电机(2)(3)的PWM波占空比，使广角定焦摄像头(1)向使图像中心点和目标中心点距离|P|和|Q|减小的方向转动。其中摄像头转动控制量PWM波占空比的变化步长分为水平方向的变化步长μ₁和竖直方向的变化步长μ₂，变化步长并不是一个固定的值，而是一个根据f(P)或g(Q)计算得出的值，其计算公式分别为μ₁＝k*μ₀*f(P)和μ₂＝k*μ₀*g(Q)。其中k为预先设定的倍率常数，μ₀为单位变化步长，f(P)或g(Q)有正有负，因此变化步长μ亦有正有负。

在递增或递减PWM波占空比时，若硬件***中增加了前文所描述的控制整体水平和竖直运动的步进电机，则计算摄像头此时的像平面法向量相对于初始位置摄像头法向量的水平夹角θ和竖直夹角η。计算|θ|是否接近了步进电机(3)的临界运动范围，以及计算|η|是否接近了步进电机(2)的临界运动范围。如果|θ|＞|θ_max|-|θ_ε2|，其中|θ_max|指步进电机(3)的最大运动角度，θ_ε2为阈值角度，即指一个较小的步进电机(3)的运动角度，如3到5倍PWM波单位变化步长μ0所对应的步进电机的运动角度，则在算法进一步继续前先调整新增的水平运动步进电机的输出角度，使整个视觉***在水平方向上运动以缩小|θ|的大小，扩大仿蜥蜴亚目避役科生物视觉坐标提取算法所应用的视觉***的水平运动范围。如果|η|＞|η_max|-|η_ε2|，其中|η_max|指步进电机(2)的最大运动角度，η_ε2为阈值角度，即指一个较小的步进电机(2)的运动角度，如3到5倍PWM波单位变化步长μ₀所对应的步进电机的运动角度。则在算法进一步继续前先调整新增的竖直运动步进电机的输出角度，使整个视觉***在竖直方向上运动以缩小|η|的大小，扩大仿蜥蜴亚目避役科生物视觉坐标提取算法所应用的视觉***的竖直运动范围。当|θ|＜|θ_max|-|θ_ε2|且|η|＜|η_max|-|η_ε2|，则算法继续。

6)当f(P)值和g(Q)值的绝对值均小于某预设的阈值ε，即(|f(P)|＜ε)∧(|g(Q)|＜ε)时，判定为目标与摄像头图像中心点近似重合，跳转至步骤7)；否则返回步骤5)。

7)当目标与图像中心点近似重合时，读出摄像头此时的像平面法向量相对于该摄像头初始位置摄像头法向量的水平夹角θ和竖直夹角η并输出。转至步骤10)。

8)若标定信息表中存在对应的标定信息，则根据|f(P)|值和|g(Q)|值查标定信息表得到对应的|Δθ|值和|Δη|值。依照标定信息换算示意表，根据目标所在摄像头图像象限确定|Δθ|与|Δη|前附加的符号，确定摄像头转动的水平角度Δθ与竖直角度Δη。

此处所述的标定信息表将在下文标定原理部分之后详细描述，标定信息换算表将在下文标定信息表部分之后详细描述。

若硬件***中增加了前文所描述的控制整个***进行水平和竖直运动的步进电机，则计算摄像头此时的像平面法向量相对于初始位置摄像头法向量的水平夹角θ和竖直夹角η。根据目标目前所在广角定焦摄像头图像的象限，依据标定信息换算示意表确定Δθ和Δη的符号，计算|θ±Δθ|是否接近了步进电机(3)的临界运动范围，以及计算|η±Δη|是否接近了步进电机(2)的临界运动范围。

如果|θ±Δθ|＞|θ_max|-|θ_ε2|，其中|θ_max|指步进电机(3)的最大运动角度，θ_ε2为阈值角度，即指一个较小的步进电机(3)的运动角度，如3到5倍PWM波单位变化步长μ₀所对应的步进电机的运动角度，则在算法进一步继续前先调整新增的水平运动步进电机的输出角度，使整个视觉***在水平方向上运动以缩小|θ±Δθ|，从而扩大仿蜥蜴亚目避役科生物视觉坐标提取算法所应用的视觉***的水平运动范围。由于这种扩大运动范围的方式改变了目标在图像上的位置，因此不能直接使用标定信息表中的标定信息，需重新转至步骤4查找标定信息。

如果|η±Δη|＞|η_max|-|η_ε2|，其中|η_max|指步进电机(2)的最大运动角度，η_ε2为阈值角度，即指一个较小的步进电机(2)的运动角度，如3到5倍PWM波单位变化步长μ₀所对应的步进电机的运动角度，则在算法进一步继续前先调整新增的竖直运动步进电机的输出角度，使整个视觉***在竖直方向上运动以缩小|η±Δη|的大小，扩大仿蜥蜴亚目避役科生物视觉坐标提取算法所应用的视觉***的竖直运动范围。由于这种扩大运动范围的方式改变了目标在图像上的位置，因此不能直接使用标定信息表中的标定信息，需重新转至步骤4查找标定信息。

如果|θ±Δθ|＜|θ_max|-|θ_ε2|且|η±Δη|＜|η_max|-|η_ε2|，则算法继续。

查完标定信息表及标定信息换算表后，给步进电机发送对应于摄像头转动水平角度Δθ与竖直角度Δη的占空比的PWM波以控制广角定焦摄像头使|f(P)|和|g(Q)|减小。

9)输出世界坐标系下，此时的目标低精度实际水平夹角θ+Δθ和低精度实际竖直夹角η+Δη。其中，θ、η分别为摄像头此时的像平面法向量相对于其初始位置摄像头法向量的水平夹角和竖直夹角。延时T时间，其中T为预先设置的延时量，返回步骤2)。

由于在算法运行过程中目标有可能是实时运动的，导致***的步进电机控制量也是需要实时更新的。因此在此处计算出的PWM波发送后，延迟一段确定的时间T，即可进入下一步骤继续更新新的电机控制量。这里的延迟时间T需要根据实际需要调试设定：较长的延迟时间T使算法计算量小但***具有滞后性，较短的延迟时间T使***比较灵敏但算法计算量大。

10)判断Marker_learning标志位是否为0，此标志位的值是预先设定的。若Marker_learning标志位值为0，则说明标定信息表已经建立完毕，转至步骤2)。若Marker_learning标志位值非0，则继续算法，开始或继续建立标定信息表。

标定原理

如图8所示为角度信息标定学***方向的标定，接下来改变摄像头的竖直角度，对应于新的竖直角度进行水平方向的标定，不断循环，直到目标在竖直方向上离开摄像头图像。在标定第一象限同一行的像素点时，摄像头是水平移动的，这也就意味着目标的g(Q)值的浮动应该是小于一个阈值范围的，该阈值需要在实际调试时确定。

进行标定需要满足两个条件，第一，目标位于摄像头图像的中心点，第二，目标静止。第一个条件已经在前面的步骤中得到了保证，因此在接下来的步骤中只需要判断目标是否静止。判断的方法分为两步：

第一步，在摄像头以固定竖直角度进行水平方向标定的过程中，判断目标是否静止。判断的方法为：在开始进行水平方向标定前，首先计算g(Q)，并将其赋值给g(Q₀)，即g(Q₀)＝g(Q)，然后开始进行水平方向标定，实时计算g(Q)，并判断|g(Q)-g(Q₀)|＜ε₂是否成立，如果成立，则说明目标没有移动，如果不成立，则说明目标发生了移动。ε₂为参照阈值，为一个较小的正值，可取摄像头图像主对角线长度1/50。阈值的设置与摄像头性能以及实验要求有关。

第二步，在摄像头完成前一行水平方向标定且连续转动至下一行的水平方向起始标定位置过程后，判断目标是否发生运动。判断的方法为：将前一行对应的g(Q₀)赋值给g(Q₁)，然后在下一行水平方向标定开始前计算g(Q)，并赋值给g(Q₀)，即g(Q₀)＝g(Q)，判断|g(Q₁)-g(Q₀)|＜ε₃。若不等式成立，则说明在目标没有发生运动，若不等式不成立，则说明目标发生了运动。ε₃为参照阈值，为一个比ε2略大的正值，可取主对角线长度1/30，视实际要求精度而定。

一旦目标在标定过程发生了运动，标定过程立即停止，并记录目标运动时对应的学习入口角度。当触发再次标定的条件成立时，依照学习入口角度继续未完成的标定工作。

步骤11至步骤18的程序详细流程图如图9所示。

11)判断是否是第一次标定

定义变量并赋值：

标准水平角度θ₀：摄像头开始标定时像平面法向量相对于其初始位置摄像头法向量的水平夹角；

标准竖直角度η₀：摄像头开始标定时像平面法向量相对于初始位置摄像头法向量的竖直夹角；

Δθ、Δη、θ、η定义同前；

θ，Δθ，θ₀三者的关系可以表述为：θ＝θ₀+Δθ

η，Δη，η₀三者的关系可以表述为：η＝η₀+Δη

判断方法：

判断此时学习入口角度(Δθ₁、Δη₁)是否为(θ_ε，η_ε)，即(Δθ₁＝θ_ε)^(Δη₁＝η_ε)是否成立。

判断结果：

如果成立，算法继续下一步。判断成立说明学习入口角度的初始化值没有被重新赋值，即之前并不存在自标定过程中断的情况，算法并未存储标定中断时的学习入口角度，接下来需要开始建立标定信息表；

如果不成立，跳转至步骤13)。判断不成立说明存在标定过程中断的情况，并存储有上次标定中断时对应的学习入口角度。

12)在摄像头坐标系中，令摄像头在水平方向和竖直方向分别转动(θ_ε，η_ε)，θ_ε，η_ε为阈值角度，并计算转动后的g(Q)；定义目标静止参照值g(Q₀)和备选目标静止参照值g(Q₁)，并赋值g(Q₀)＝g(Q)，g(Q₁)＝0，然后跳转至步骤14)。g(Q₀)用于判断目标在进行任意一行水平方向标定过程中，目标是否静止，备选目标静止参照值g(Q₁)用于判断摄像头在完成一行标定，转动准备进行下一行标定的过程中，目标是否静止。

具体方法：逐渐变化发送给竖直运动的步进电机的PWM波占空比，令摄像头持续向下转动。当摄像头此时的像平面法向量相对于其初始位置摄像头法向量的竖直夹角η与标准竖直角度η₀的差值等于预先设置的竖直学***运动的步进电机的PWM波占空比，令摄像头持续向左转动。当摄像头此时的像平面法向量相对于其初始位置摄像头法向量的水平夹角θ与标准水平角度θ₀的差值等于预先设置的水平学习阈值角度值|θ_ε|时，开始建立对应于此时g(Q)值的标定信息表，跳转至步骤14)。

在建立标定信息表之前要先转动阈值角度的原因是：由于通过标定信息表输出的角度信息是粗精度的角度信息，而在目标与图像中心点近似重合时算法需要输出的角度是高精度的角度信息。因此当目标即将与图像中心点重合时，不再使用标定信息表指导电机转动进而获得高精度的角度信息。所以，算法并不需要阈值角度范围内的摄像头标定信息，即算法不标定图8中阴影区域。

13)算法此时将跳过学习入口角度前的自标定过程，直接恢复上一次自标定过程的断点进度；

逐渐变化发送给竖直运动的步进电机的PWM坡占空比，在摄像头坐标系内，首先令摄像头持续向下转动，直到摄像头向下转过的角度等于竖直方向的学习入口角度Δη₁时，摄像头停止。计算此时的g(Q)，令备选目标静止参照值g(Q₁)＝g(Q₀)，并设目标静止参照值g(Q₀)＝g(Q)

逐渐变化发送给水平运动的步进电机的PWM波占空比，在摄像头坐标系内，令摄像头持续向左转动，直到摄像头向左转过的角度等于水平方向的学习入口角度Δθ₁为止。继续下一步。

14)如果g(Q₁)等于零，计算此时的f(P)值，将此时的Δθ＝θ_ε与Δη＝η_ε值记录入标定信息表中对应于此时f(P)值与g(Q)值的位置中，继续下一步。

如果g(Q1)不等于零，则判断备选目标静止参照值g(Q₁)与目标静止参照值g(Q₀)的差值绝对值是否小于阈值，即|g(Q₁)-g(Q₀)|＜ε₃。若不等式成立，计算此时的f(P)值，将此时的Δθ与Δη值记录入标定信息表中对应于此时f(P)值与g(Q)值的位置中。算法继续下一步；若不等式不成立，记录下此时的Δθ和Δη，令学习入口角度Δθ₁＝Δθ，Δη₁＝Δη；跳转步骤2)。

15)在摄像头坐标系中，摄像头向左转动一个对应于PWM波占空比单位变化步长μ0的水平角度，判断此时目标几何中心点是否离开摄像头图像右方；若己离开，则转至步骤17)；若未离开，则继续算法；

16)计算此时g(Q)值，判断此时|g(Q)-g(Q0)|＜ε₂是否成立，若成立，计算此时的f(P)值，并将该f(P)，g(Q)对应的Δθ与Δη值记录入标定信息表中，返回步骤15)；否则，记录下此时的Δθ和Δη，令学习入口角度Δθ₁＝Δθ，Δη₁＝Δη；跳转步骤2)；

步骤16)中，只有在确认新获得的标定信息在竖直方向并未产生过大的跳变，即|g(Q)-g(Q₀)|＜ε₂时，才会将其记录入标定信息表并返回继续获取新的标定信息。否则可判定目标已经移动，自标定过程需要在保存断点后终止。

17)在摄像头坐标系中，令摄像头向右转动至Δθ＝|θ_ε|，再令摄像头向下转动一个对应于PWM波占空比单位变化步长μ₀的竖直角度；判断此时目标几何中心点是否离开摄像头图像上方；若己离开，则说明标定信息表建立完毕，转至步骤18)；若未离开，则计算此时的g(Q)，并更新备选目标静止参照值g(Q₁)＝g(Q₀)，然后更新目标静止参照值g(Q₀)＝g(Q)，转至步骤14)；

角度信息标定学习算法中所辅助使用的基于Cam shift算法的跟踪算法可以为角度信息标定学习算法提供目标的中心点在摄像头图像中的位置和目标在摄像头图像中所占的大小。借助于此两项信息，即可判别出目标的几何中心点是否已经离开摄像头图像，具体实现为：当f(P)＞ε₄或g(Q)＞ε₅时，即可判断目标已经离开摄像头图像。此时的真实情况为目标的中心点接近了图像的边缘。ε₄、ε₅为阈值，本实施例中ε₄的取值为

略小，ε₅的取值为比

略小。

18)标定信息表建立完毕后，清Marker_learning标志位为0，转至步骤2)；

只有在确认角度信息标定学习算法的自标定过程已经完成的情况下，算法才会转至步骤18，清除学习标志位Marker_learning为0。在清除了学习标志位Marker_learning之后，不论算法是否存储有自标定过程断点，都不再进入自标定状态。

标定信息表

如图10所示，在角度信息标定学***角度和竖直角度组成。即表中每一个位置记录了一组对应于一组|f(P)|，|g(Q)|值的|Δθ|，|Δη|值。其中每一行的数据对应于相同的|f(P)|值，自行最左侧至行最右侧对应于数值逐渐加大的|g(Q)|值。每一列的数据对应于相同的|g(Q)|值，自列最上方至列最下方对应于数值逐渐加大的|f(P)|值。例如：图10中对应于|f(P)|＝0.25，|g(Q)|＝0.33，表中记录有一组Δθ，Δη的数据：Δθ＝15°，Δη＝24.3°。

由于所使用的摄像头镜头畸变是近似对称的，因此标定信息表中只记录有摄像头图像中I象限图像的标定信息。其他象限图像的标定信息可通过标定信息换算示意表中示意的运算得到。

标定信息换算表

标定信息换算示意表：标定信息换算示意表辅助标定信息表工作。

首先将摄像头图像依照所在象限划分为4个区域：

其中imax为摄像头图像的行数。jmax为摄像头图像的列数。i指摄像头图像第i行。j指摄像头图像第j列。I_ij指摄像头图像中第i行第j列的像素点。

标定信息表只记录有I象限摄像头图像的标定信息，其他象限摄像头图像的标定信息可通过遵循标定信息换算示意表的规则计算得到。标定信息换算示意表如下所示：表中第二列表示判断不同象限的判断方法，第三、四列表示位于不同象限时，在摄像头坐标系中摄像头应该转动的角度及方向。

I象限	IF：(f(P)＝|f(P)|)∧(g(Q)＝|g(Q)|)	|Δθ|	|Δη|
				II象限	IF：(-f(P)＝|f(P)|)∧(g(Q)＝|g(Q)|)	-|Δθ|	|Δη|
III象限	IF：(-f(P)＝|f(P)|)∧(-g(Q)＝|g(Q)|)	-|Δθ|	-|Δη|
				IV象限	IF：(f(P)＝|f(P)|)∧(-g(Q)＝|g(Q)|)	|Δθ|	|Δη|

角度信息标定学习算法需要进行自标定的必要性：1.角度信息标定学习算法若运行过程中不进行自标定，其只有在目标与图像中心点近似重合的情况下才可以输出角度信息，这将导致角度信息的输出不具有实时性，不利于后续算法的使用。2.角度信息标定学习算法若运行过程中不进行自标定，其跟踪目标的能力将会下降，使目标更易丢失。

3.深度信息及世界坐标提取算法

深度信息及世界坐标提取算法步骤如下：

1)对仿蜥蜴亚目避役科生物视觉坐标提取算法所应用的视觉***进行标定。经手工标定后，两广角定焦摄像头各自的几何中心点之间的距离d为己知量。如图4所示，为详细说明算法的计算过程，设两广角定焦摄像头像平面法向量所在的射线与两广角定焦摄像头各自的几何中心点之间的连线的夹角均为锐角。设左侧广角定焦摄像头几何中心点与目标在两广角定焦摄像头各自的几何中心点连线上的投影点之间的距离为d₁，右广角定焦摄像头几何中心点与目标在两广角定焦摄像头几何中心点连线上的投影点之间的距离为d₂，则d₁+d₂＝d。

2)接收来自角度信息标定学***面法向量相对于各自的初始位置摄像头法向量的水平夹角θ₁，θ₂和竖直夹角η₁，η₂。在来自角度信息标定学习算法的角度信息中，有通过查询标定信息表得到的角度信息，也有通过读取控制量反向计算得到的角度信息。前者精度低，实时性高，后者精度高，实时性低。根据具体情况需要，深度信息及世界坐标提取算法可选择屏蔽其中一类角度信息或全部接收。

3)根据得到的标定量d，以及角度信息θ₁，θ₂，计算目标相对于两广角定焦摄像头各自的几何中心点之间的连线中点的深度，即目标的X坐标。

相对于图4而言，目标的深度信息可以表示为：

推导得： $d_1=\frac{\tan {\mathrm{\θ}}_1}{\tan {\mathrm{\θ}}_2}{d}_2$

代入：d₁+d₂＝d，可得： $(\frac{\tan {\mathrm{\θ}}_1}{\tan {\mathrm{\θ}}_2}+1)d_2=d$

整理得： $d_2=\frac{d}{(\frac{\tan {\mathrm{\θ}}_1}{\tan {\mathrm{\θ}}_2}+1)},$ $d_1=d-d_2=d-\frac{d}{(\frac{\tan {\mathrm{\θ}}_1}{\tan {\mathrm{\θ}}_2}+1)}$

相应的： $X=\frac{d_2}{\tan {\mathrm{\θ}}_2}=\frac{d_1}{\tan {\mathrm{\θ}}_1}=\frac{d}{(\frac{\tan {\mathrm{\θ}}_1}{\tan {\mathrm{\θ}}_2}+1)\tan {\mathrm{\θ}}_2}=\frac{d}{\tan {\mathrm{\θ}}_1+\tan {\mathrm{\θ}}_2}$

即深度信息、X坐标为： $X=\frac{d}{\tan {\mathrm{\θ}}_1+\tan {\mathrm{\θ}}_2}.$

4)进一步计算得

即为***世界坐标系的Y坐标。在步骤1)中，只考虑了如图4所示的两广角定焦摄像头像平面法向量所在的射线与两广角定焦摄像头各自的几何中心点之间的连线的夹角均为锐角的情况。若为钝角则计算方法与上述情况类似。

世界坐标系的Z坐标亦可用类似于求取Y坐标的方法求出。如图5所示，得到深度信息X后，结合角度信息标定学***面法向量相对于初始位置摄像头法向量的竖直夹角η₁，通过公式：Z＝tan(η₁)×X，即可得到Z坐标。至此，目标相对于视觉***坐标系的世界坐标建立完毕。

5)输出目标相对于两广角定焦摄像头各自的几何中心点之间的连线中点的世界坐标，退出算法。

其算法流程图如图11所示。

将深度信息及世界坐标提取算法与角度信息标定学习算法分离开来的原因为：1.角度信息标定学习算法需不停的更新目标的角度信息，其实时性要求较高。故算法的简短十分重要。2.角度信息标定学习算法所输出的角度信息并不止提供给深度信息及世界坐标提取算法，还可以提供给其他算法。

4.基于Camshifi算法的单目跟踪算法

角度信息标定学习算法使用了基于Cam shift算法的单目跟踪算法，其主要用于实时向角度信息标定学习算法提供目标在摄像头图像中的大小以及目标几何中心点在摄像头图像中的位置。

常规的多目视觉***跟踪算法需要综合考虑多个摄像头间跟踪时的配合问题，计算量大，算法复杂，且由于不具备主动视觉的功能，跟踪范围窄，易发生目标丢失。基于主动视觉的仿蜥蜴亚目避役科生物视觉坐标提取算法所应用的视觉***将各个摄像头的跟踪算法独立开来，有效的降低了算法的复杂程度，降低了算法的计算量。

基于Cam shift算法的单目视觉跟踪算法属现有技术，可以概述如下：

对摄像头回传的图像进行HSV颜色空间变化，提取hue分量，构建颜色直方图，颜色概率分布图。根据颜色概率分布图，使用Cam_shift算法进行目标的跟踪。建立适当的图像搜索窗，提取搜索窗中心点坐标。计算控制量发送给步进电机，控制摄像头使跟踪目标始终处于图像的中心点处，其具体控制方法为角度信息标定学习算法中所述的控制方法。

Claims (4)

1.一种仿蜥蜴亚目避役科生物视觉坐标提取算法，基于由两台广角定焦摄像头、步进电机构成的物理平台，其特征在于包括以下步骤：

1).令两广角定焦摄像头分别搜索目标，当A摄像头发现目标后，启用基于Cam shift算法的单目视觉跟踪算法保持对目标的跟踪，并实时返回A摄像头此时像平面法向量相对于A摄像头初始位置时摄像头像平面法向量的水平夹角和竖直夹角；所述的A摄像头为首先搜索到目标的摄像头，B摄像头为另一台摄像头；所述的初始位置为：使广角定焦摄像头的像平面法向量与水平面相互平行，并且使两台广角定焦摄像头的像平面法向量垂直于两广角定焦摄像头几何中心点连线所构成的线段的位置；

2).B摄像头跟随A摄像头进行目标搜索，B摄像头搜索到目标后，亦启用基于Camshift算法的单目视觉跟踪算法保持对目标的跟踪；

3).调用角度信息标定学***面法向量相对于各自初始位置摄像头像平面法向量的水平夹角和竖直夹角；

4).根据步骤3)得到的结果，使用深度信息及世界坐标提取算法，实时计算并输出目标深度信息及目标在世界坐标系中的世界坐标；所述的世界坐标系为：以两广角定焦摄像头几何中心点连线的中点为原点，以与水平面平行、垂直于两广角定焦摄像头几何中心点连线、且沿视觉追踪的方向为x轴正方向，以垂直于水平面向上的方向为z轴正方向的右手坐标系；

5).若目标未丢失，返回步骤3)，若果目标丢失，则返回步骤1)。

2.根据权利要求1所述的一种仿蜥蜴亚目避役科生物视觉坐标提取算法，其特征在于所述的角度信息标定学习算法包括以下步骤：

2.1)初始化学习入口角度(Δθ₁、Δη₁)

所述的学***角度和竖直角度；

2.2)分别计算在摄像头图像中，摄像头图像中心点与目标的水平距离P和竖直距离Q相对于摄像头图像主对角线长度的百分比f(P)和g(Q)；

2.3)判断目标是否在摄像头图像中心点，

若(|f(P)|＜ε)∧(|g(Q)|＜ε)，阈值ε＞0，则目标在中心点，跳转至步骤2.7)；

若(|f(P)|＞ε)∨(|g(Q)|＞ε)，阈值ε＞0，则目标不在中心点，跳转至步骤2.4)；

2.4)查标定信息表，

若标定信息表中存在对应于|f(P)|和|g(Q)|的角度信息，则算法跳转至步骤2.8)；

若标定信息表中不存在对应于|f(P)|和|g(Q)|的角度信息，则跳转至步骤2.5)；

2.5)逐步转动摄像头，使目标在摄像头图像中与图像中心点重合；

2.6)当目标与摄像头图像中心点重合时，跳转至步骤2.7)；否则返回步骤2.5)；

2.7)当目标与图像中心点重合时，读出摄像头此时的像平面法向量相对于该摄像头初始位置摄像头像平面法向量的水平夹角θ和竖直夹角η并输出，转至步骤2.10)；

2.8)若标定信息表中存在对应于|f(P)|和|g(Q)|的角度信息，则摄像头转动该角度使目标几何中心点在摄像头图像中与图像中心点重合，转动方向根据标定信息换算示意表确定；

2.9)输出世界坐标系下，目标相对于初始位置摄像头法向量的水平夹角和竖直夹角；延时T时间，其中T为预先设置的延时量，返回步骤2.2)；

2.10)通过预设学习标志位判断是否学习完毕，

若学习完毕，则转至步骤2.2)；

若未学习完毕，则算法继续，开始标定；

2.11)判断是否是第一次标定，

如果学习入口角度等于初始值则为第一次标定，此时算法继续；

如果学习入口角度不等于初始值则不为第一次标定，此时跳转至步骤2.13)；

2.12)令摄像头在水平方向和竖直方向分别转动(θ_ε，η_ε)，θ_ε，η_ε为阈值角度，跳转至步骤2.14)

2.13)摄像头转过前一次标定中断时记录的学习入口角度，跳转至步骤2.14)；

2.14)若为初始标定，将摄像头在水平方向和竖直方向分别转动的阈值角度(θ_ε，η_ε)以及与其对应的摄像头图像内目标几何中心点的|f(P)|和|g(Q)|写入标定信息表；若不为初始标定，首先判断目标在进入此步骤前是否发生了运动；如果目标运动了，则更新学习入口角度，并跳转至步骤2.2)；如果目标没运动，则更新标定信息表；

2.15)保持摄像头竖直角度不变，进行行标定，每当摄像头在水平方向转过一个单位水平角度，判断此时目标几何中心点是否离开摄像头图像；若己离开，则转至步骤2.17)；若未离开，则继续算法；

2.16)判断摄像头在水平方向每转过一个单位水平角度后，目标是否移动，若没有发生移动，则更新标定信息表，并跳转至步骤2.15)；若发生移动，则更新学习入口角度，并跳转至步骤2.2)：

2.17)改变摄像头竖直角度，进行另一行的水平标定，如果目标几何中心点离开摄像头图像，则标定信息表建立完毕，转至步骤2.18)；若未离开，则转至步骤2.14)；

2.18)标定信息表建立完毕后，清除学习标志位，转至步骤2.2)；

所述的标定信息表为记录有摄像头成像标定信息的一张二维向量表；表中记录了目标位置信息，以及摄像头图像中，目标从原点移动到目标位置，摄像头像平面法向量对应转过的水平夹角和竖直夹角；所述的目标位置信息由摄像头图像中心点与目标的水平距离P和竖直距离Q相对于摄像头图像主对角线长度的百分比f(P)和g(Q)表示。

3.根据权利要求2所述的角度信息标定学习算法，其特征在于所述的标定信息换算示意表可描述为：

首先将摄像头图像依照所在象限划分为4个区域：

其中imax为摄像头图像的行数；jmax为摄像头图像的列数；i指摄像头图像第i行；j指摄像头图像第j列；I_ij指摄像头图像中第i行第j列的像素点；

标定信息表只记录有I象限摄像头图像的标定信息，其他象限摄像头图像的标定信息可通过遵循标定信息换算示意表的规则计算得到；标定信息换算示意表如下所示：

  I象限   IF：(f(P)＝|f(P)|)∧(g(Q)＝|g(Q)|)   |Δθ|   |Δη|   II象限   I F：(-f(P)＝|f(P)|)∧(g(Q)＝|g(Q)|)   -|Δθ|   |Δη|   III象限   I F：(-f(P)＝|f(P)|)∧(-g(Q)＝|g(Q)|)   -|Δθ|   -|Δη|   IV象限   IF：(f(P)＝|f(P)|)∧(-g(Q)＝|g(Q)|)   |Δθ|   |Δη|

4.根据权利要求1所述的一种仿蜥蜴亚目避役科生物视觉坐标提取算法，其特征在于所述的深度信息及世界坐标提取算法包括以下步骤：

根据两广角定焦摄像头几何中心点之间的距离，以及根据角度信息标定学***面法向量相对于各自的初始位置摄像头法向量的水平夹角和竖直夹角，利用三角函数计算得到目标的世界坐标。

Images