CN102175222B - 基于立体视觉的吊车避障系统 - Google Patents

Abstract

一种基于立体视觉的吊车避障系统。包括：标定双目立体视觉系统，得到左右相机的内参数，以及左右相机之间的外参数；使用立体视觉系统拍摄吊车工作场景的图像，重建吊车工作空间中的静态障碍物；在负载运送的过程中，实时跟踪负载，同时检测突然进入吊车工作空间的运动物体（人、车辆、或者其它障碍物等），并估计负载和运动物体的运动信息、位置信息和尺寸信息；根据碰撞预测策略，预测负载与障碍物（静态障碍物和运动物体）是否发生碰撞，以决定是否采取紧急制动，避免负载与障碍物发生碰撞。

Description

基于立体视觉的吊车避障系统

技术领域

本发明属于非线性欠驱动系统自动控制的技术领域，特别涉及一种基于立体视觉的吊车避障系统。

背景技术

吊车系统运行过程中，监控人员发现(潜在)碰撞危险的情况，通过紧急制动按钮来实现吊车的紧急制动。但是，这种紧急制动策略取决于监控人员的判断时机，实时性达不到，极易发生碰撞，危险性高，极大的降低了吊车系统的效率。

随着计算机视觉技术的不断发展，视觉传感器在各类机电系统中得到了越来越广泛的应用。双目视觉传感器以其结构简单、非接触测量、准确度高等诸多优点被广泛地用于视场内环境的监控和测量。在吊车系统运行过程中，通过立体视觉系统完成运动物体检测、吊车空间的三维重建，在线计算出负载与障碍物的运动信息(位置信息、速度信息)和尺寸信息，来进行碰撞预测，决定是否采用紧急制动，实现吊车系统的安全运行。

发明内容

本发明目的是解决现有吊车系统的控制达不到实时性要求、极易发生碰撞，使危险性高等问题，提供一种基于立体视觉的吊车避障系统。

该系统的主要功能是：标定双目立体视觉系统，得到左右相机的内参数，以及左右相机之间的外参数；使用立体视觉系统拍摄吊车工作场景的图像，重建吊车工作空间中的静态障碍物；在吊车系统运送负载的工程中，实时检测突然进入吊车工作空间的运动物体(人、车辆、或者其它障碍物等)，同时预测出负载与工作空间中的静态障碍物和运动障碍物是否发生碰撞。如果发生碰撞，则采取紧急制动策略，避免负载与障碍物发生碰撞。

本发明提供的基于立体视觉的吊车避障系统共由四部分组成，包括：

第1、双目立体视觉系统的标定

第1.1、模型分析

相机模型采用通常的针孔模型描述三维欧式空间到二维图像平面的投影变换，双目立体视觉系统一般由左右两个相机组成，其成像模型均采用所述的针孔模型；在双目立体视觉系统中，需要标定的参数包括左相机和右相机的内参数A_l、A_r，A_l为左相机的内参数矩阵，A_r为右相机的内参数矩阵，以及左相机和右相机之间的外参数R和t，R和t分别表示左相机和右相机之间外参数的旋转矩阵和平移向量；

第1.2、基于全局代价函数优化的立体标定

立体标定包括左相机和右相机的内参数标定、左相机和右相机分别与标定板之间的外参数标定、镜头畸变系数的标定、以及左右相机之间外参数标定；

引入左右相机之间外参数保持不变这一约束条件，并选取全局代价函数作为优化目标进行非线性优化，所述基于全局代价函数优化的立体标定方法包括：角点提取、2维单应矩阵的估计、相机内外参数的估计、径向畸变的估计，以及基于全局代价函数的非线性优化五个步骤：

第1.2.1、左右相机同时各拍摄3幅以上的标定图像，利用Harris角点检测算法提取出角点坐标，精确至亚像素精度；

第1.2.2、对于每幅图像，计算左右相机图像平面与标定板之间的2维单应矩阵；

第1.2.3、估计得到左右相机的内参数，以及它们与标定板之间的外参数；

第1.2.4、采用最小二乘方法估计径向畸变系数；

第1.2.5、引入左右相机之间外参数不变这一约束条件，基于全局代价函数进行非线性优化；

第2、吊车工作空间中静态障碍物的重建

首先提取图像的边缘特征；然后利用极线约束和邻域灰度信息，沿特征点的主方向建立描述符；同时通过“冗余角阈值”策略来选择合适的候选匹配点；最后通过计算特征点与候选匹配点之间的相关度，得到正确的匹配点。

第2.1、具有旋转不变性的边缘点匹配算法

针对传统边缘匹配算法对图像旋转敏感，易发生误匹配的缺点，提出了一种具有旋转不变性的边缘点匹配算法，通过将SIFT算子的旋转不变性引入匹配中来提高对图像旋转的鲁棒性，具体包括五个步骤：

第2.1.1、图像预处理，包括灰度化处理，直方图均衡化，图像平滑滤波操作；

第2.1.2、特征点提取，特征点提取在图像空间中进行，利用Canny边缘检测算子得到样本图和搜索图的轮廓点信息，并保存搜索图的轮廓图像；

第2.1.3、特征点描述符的建立

对任意特征点，首先计算特征点主方向；在以该特征点为中心的邻域窗口内采样，使用梯度方向直方图统计邻域像素的梯度方向和幅值；直方图的峰值代表该特征点邻域内梯度的主方向，即特征点的主方向；获得特征点主方向后，利用邻域灰度信息，沿特征点主方向来建立描述符，从而保证特征点描述符具备旋转不变性；

第2.1.4、候选匹配点的选取

为了消除旋转对匹配结果的影响，采用SIFT算子的旋转不变性来建立特征点描述符，但匹配结果出现二义性，即一个样本点可能对应多个搜索点，为此，采用一种“冗余角阈值”策略对二义性问题进行处理，在匹配时，仅仅将搜索图中对应主方向附近的特征点作为候选，以剔除错误的匹配点；

第2.1.5、特征点描述符相关度的计算

特征点的匹配，实质是计算特征点描述符之间的相关度；对两个点p₁、p₂，设其描述符分别为v₁、v₂；v₁和v₂的相关度越高，则两个点的匹配度越高；反之，则匹配度越低；使用如下向量夹角余弦值来计算两个描述符之间的相关度，当余弦值等于1时，两特征点具有最佳匹配；当余弦值等于0时，则匹配度最低；使用该方法对吊车工作场景进行重建，最终得到吊车工作空间中静态障碍物的位置信息和尺寸大小；

第3、吊车工作空间中动态障碍物的检测与跟踪

使用背景差分法，实时检测出突然进入吊车工作空间的运动物体。再使用camshift算法，实时跟踪负载和运动障碍物，估计它们各自的位置信息和尺寸信息。最后对这些信息进行kalman滤波，得到准确的估计值；

第3.1、基于背景差分法的运动检测

第3.1.1、一般情况下，前景的运动目标的灰度值与背景的灰度值之间有很大的差异，而运动物体本身的灰度值一般不会有很大差异，因此通过图像差分就能很好地反映出两帧图像之间的变化，如能检测出这种变化，就可以将运动目标提取出来并分析其运动特征；

第3.1.2、由于噪声及背景提取存在的误差，需要对得到的差分图像进行二值化处理，即将属于背景的静止像素和属于目标的运动像素分开，得到二值化图像；

第3.1.3、得到二值化图像后，对图像进行腐蚀和膨胀操作，消除孤立点或较小的孤立区域，连通碎化的区域，然后统计连通区域个数，判断是否有运动障碍物进入，对运动物体进行编号；同时统计连通区域的外接矩形，可以估计运动物体的尺寸信息，还可以用于camshift跟踪算法的初始化；

第3.2、尺寸信息估计

根据第3.1.3步得到的图像上连通区域的外接矩形，估计运动物体的实际尺寸信息；

第3.2.1、匹配左图像和右图像中的连通区域，根据图像上连通区域的中心位置，来计算物体中心的实际坐标，得到物体的深度值Z_C；

第3.2.2、已知连通区域的外接矩形的长轴a，短轴b，以及外接矩形与水平方向的偏转角度为θ，可以得到长边在u，v方向上的长度Δu，Δv；根据投影原理，可以得到物体在X_C，Y_C方向上的长度ΔX_C和ΔY_C，进一步得到物体的包围球半径r；

第3.3、camshift跟踪算法

Camshift算法可以分为两大部分，第一部分meanshift查找目标，第二部分自适应调整搜索窗口的长、宽、方向；对于每一幅图像，先采用meanshift算法计算出目标包围窗口，在通过公式自适应的计算出下一幅图像的初始搜索框的长和宽，这样就完成了对搜索框的自适应更新；

第3.4、kalman滤波

以上第3.3步跟踪算法得到的只是物体每时刻位置的估计，这个估计是不准确，可以将这些不准确简单地看作跟踪过程中的噪声；为了最大限度地估计物体的运动，使用多个测量的累积可以检测出不受噪声影响的部分观测轨迹；应用在吊车系统中，我们将物体的状态用三个位置变量x、y和z，以及三个速度变量v_x，v_y和v_z表示，这六个变量组成状态向量x_k的元素；我们用立体视觉来测量物体的位置，作为观测值来进行更新，就可以得到最优的估计值；

第4、碰撞预测

使用包围球来表示负载和障碍物的碰撞模型，以静止包围球的碰撞相交算法为基础，得到运动的包围球碰撞算法；负载在场景中运动，根据运动包围球碰撞算法，实时预测负载与静态或者动态障碍物是否发生碰撞，以启动紧急制动控制；

第4.1、碰撞模型

包围球碰撞模型，是指包含物体的最小球体，包括球体中心坐标C和半径R；

第4.1.1、静态障碍物的包围球创建

根据立体匹配重建得到的物体轮廓的点集X_j(0≤j≤n)，首先，累积所有点的坐标向量获取平均向量C，即为包围球的中心点；然后再根据所有点到中心点的最大距离确定包围球的半径R；

第4.1.2、动态障碍物的包围球创建

根据第3步实时跟踪并经过kalman滤波后得到的中心点位置作为包围球的中心点，实时估计的最大半径作为包围球的半径，并可以在线更新；

第4.2、运动碰撞预测算法

第4.2.1、计算负载包围球中心O_l与障碍物包围球中心O_o之间的距离d₁，判断障碍物是否在安全半径范围之内，如果在，进行下一步；其中，安全半径R_safe由吊车负载的最大安全制动距离d_safe、负载包围球半径R_l和障碍物包围球半径R_o组成；

第4.2.2、计算负载与障碍物的相对速度是否为零，如果相对速度为零，则两个物体不会发生碰撞；若不为零，则进行下一步；其中，相对速度V_rel由负载速度V_l和障碍物速度V_o生成；

第4.2.3、计算直线与平面的交点P_intre；其中，直线过负载中心点且方向向量为相对速度方向，平面过障碍物中心点且法向量为相对速度方向；

第4.2.4、然后计算交点P_inter与障碍物中心点之间距离d₂；如果d₂小于R_l和R_o之和，则负载与障碍物发生碰撞，反之，则不发生碰撞；

第4.2.5、当预测负载与障碍物将发生碰撞时，及时采取制动措施。

本发明的优点和积极效果：

本发明基于工程实际需要，将立体视觉应用于吊车系统的运行过程中。使用立体视觉系统重建吊车工作空间中静态障碍物。同时在负载运送过程中，实时检测突然进入吊车工作空间的运动物体(人、车辆、或者其它障碍物等)，同时预测出负载与工作空间中的静态障碍物和运动障碍物是否发生碰撞。如果预测出碰撞，则采取紧急制动策略，避免负载与障碍物发生碰撞。

考虑到各国现有吊车的控制基本均由人工进行操纵，由监控人员发现潜在的碰撞危险，通过紧急制动按钮来实现吊车的紧急制动。但是，这种紧急制动策略取决于监控人员的判断时机，实时性达不到，极易发生碰撞，危险性高，极大的降低了吊车系统的效率。研究该方向的自动控制方法无疑对安全、效率、精确性的提升有较大帮助具有明显的优点与积极效果。

附图说明

图1是系统硬件逻辑示意图。

图2是三维桥式吊车实验平台机械部分。

图3是相机固定支架示意图。

图4是系统软件逻辑示意图。

图5是基于全局代价函数优化的立体标定方法流程图。

图6是标定实验平台。

图7是重投影残差。

图8是左摄像机的内参数和畸变系数。

图9是右摄像机的内参数和畸变系数。

图10是左右摄像机之间的外参数。

图11是具有旋转不变性的边缘点匹配算法流程图。

图12是冗余角阈值策略示意图。

图13是左相机拍摄的图像。

图14是右相机拍摄的图像。

图15是重建场景中的静态障碍物。

图16是动态障碍物检测与跟踪流程图。

图17是背景差分法检测运动物体流程图。

图18是物体投影图。

图19是尺寸信息估计示意图。

图20是运动碰撞检测算法流程图。

图21是碰撞包围球模型。

图22是运动碰撞检测示意图。

图23是负载与运动障碍物的尺寸估计。

图24是负载与障碍物之间的距离。

图25是负载的速度估计。

图26是运动障碍物的速度估计。

图中，1相机固定支架，2相机，3固定板，4旋转支架，5导轨，6标定板。

具体实施方式

实施例1：

本发明所涉及系统的硬件结构如图1所示。在吊车(如图2)的工作空间中，通过相机固定支架1(如图3)将左右相机固定在吊车上，调整相机角度使其观测范围尽可能最大。根据同步信号，左右相机同时拍摄吊车工作场景，将图像通过图像采集卡和1394总线与主机相连。主机进行立体视觉系统标定，图像处理，吊车工作空间中静态障碍物的重建、动态障碍物的检测与跟踪，负载的跟踪，估计运动物体的速度信息、位置信息和尺寸信息，生成碰撞模型，进行碰撞预测。检测出发生碰撞，则通过设备USB4711A发出触发信号，再接入光电开关电路，切换到紧急制动控制。

表1硬件配置

该系统的软件结构示意图如图4所示。左右相机拍摄标定图像，完成立体视觉系统的标定。以左相机为主相机，右相机为辅相机，通过同步信号，同时拍摄场景的图像，依据特征匹配算法，离线完成吊车工作场景中静态障碍物的重建。然后，实时监控吊车工作场景，对负载进行跟踪和运动信息(位置信息和速度信息)估计、尺寸信息估计，同时检测突然进入工作场景中的动态障碍物，估计其运动信息与尺寸信息，并进行跟踪。根据碰撞模型和碰撞预测算法，检测负载是否与障碍物(静态和动态)发生碰撞。

本发明提供的基于立体视觉的吊车避障系统共由四部分组成，包括：

第1、双目立体视觉系统的标定

立体标定包括左右相机的内参数标定、它们与标定板之间的外参数标定、镜头畸变系数的标定，以及左右相机之间的外参数标定。考虑到张正友的平面标定法在单目相机标定时具有的诸多优点，本发明以张正友的标定算法为基础，选用了一种有利于从二维图像信息恢复三维位姿信息的镜头畸变模型，将左右相机的内参数、它们与标定板之间的外参数、镜头畸变系数，以及两个相机之间的外参数标定相互融合。同时引入左右相机外参数保持不变这一约束条件，并选取全局代价函数作为优化目标进行非线性优化，最终得到立体标定所需的参数。本发明提到的基于全局代价函数优化的立体标定方法流程如图5所示，它包括角点提取、2维单应矩阵的估计、相机内外参数的估计、径向畸变的估计，以及基于全局代价函数的非线性优化等五个步骤；

摄像机模型采用通常的针孔模型描述三维欧式空间到二维图像平面的投影变换，双目立体视觉系统一般由左摄像机和右摄像机两个摄像机组成，其成像模型均采用所述的针孔模型；在双目立体视觉系统中，需要标定的参数包括左摄像机和右摄像机的内参数A_l、A_r，A_l为左摄像机的内参数矩阵，A_r为右摄像机的内参数矩阵，以及左摄像机和右摄像机之间的外参数R和t，R和t分别表示左摄像机和右摄像机之间外参数的旋转矩阵和平移向量；相机模型采用通常的针孔模型来描述三维欧式空间到二维图像平面的投影变换；

左右相机同时各拍摄3幅以上的标定图像，利用Harris角点检测算法提取出角点坐标，精确至亚像素精度；

对于每幅图像，计算左右相机图像平面与标定板之间的2维单应矩阵；在标定过程中，将世界坐标系的原点选择在标定板平面上，且Z轴的方向与此平面垂直，则标定板上特征点的世界坐标简化为M＝[X，Y，0，1]^T，因此得到：

$\mathrm{sm}=H\tilde{M}---\left(1\right)$

其中，

表示特征点在标定平面上的齐次坐标。H为2维单应矩阵。在相差一个比例因子s的意义下，各给定四对以上的匹配点，便可以线性地估计出左右相机的2维单应矩阵H_il和H_ir(其中，下标i表示第i幅图像)；

估计得到左右相机的内参数，以及它们与标定板之间的外参数；求解出2维单应矩阵以后，我们就可以对它们进行分解来得到相机的内、外参数。使用至少3对图像(左右相机各3幅)信息求解后，便可以得到左右相机的内参数。在获得相机内参数后，进而来确定左右相机相对于第i幅图像的外参数。经过上述步骤，可以得到左右相机的内参数A_l和A_r，以及相对于第i幅图像的外参数R_il，t_il，R_ir和t_ir(其中，下标i表示第i幅图像)。

采用最小二乘方法估计径向畸变系数。常用的相机模型忽略了透镜的厚度和畸变的存在，因此不能很好地反映实际情况。实际上，镜头并不是理想的透视成像，而是带有不同程度的畸变，这使得空间点所成的像并不是在线性模型所描述的位置。一般而言，对于普通的相机，通常只需要考虑径向畸变的前两项即可满足测量精度的要求。为了提高标定算法的精度和便于在测量中使用，我们选用了一种有利于从二维图像信息恢复三维位姿信息的镜头畸变模型。以下仅以左相机为例来描述镜头的畸变模型，而右相机与其完全类似。在本文的研究中，我们选取如下的畸变模型：

$\left\{\begin{array}{c}\hat{u}=u+(u-u_0)[k_{l1}({x_d}^2+{y_d}^2)+k_{l2}{({x_d}^2+{y_d}^2)}^2]\\ \hat{v}=v+(v-v_0)[k_{l1}({x_d}^2+{y_d}^2)+k_{l2}{({x_d}^2+{y_d}^2)}^2]\end{array}\right.---\left(2\right)$

其中，

为左相机下的理想像素坐标，(u，v)是左相机下的实际像素坐标。k_l1和k_l2是左相机的径向畸变系数。则每个点可以得到下面两个等式：

$\left[\begin{array}{cc}(u-u_0)({x_d}^2+{y_d}^2)& (u-u_0){({x_d}^2+{y_d}^2)}^2\\ (v-v_0)({x_d}^2+{y_d}^2)& (v-v_0){({x_d}^2+{y_d}^2)}^2\end{array}\right]\left[\begin{array}{c}{k}_{l1}\\ k_{l2}\end{array}\right]=\left[\begin{array}{c}\hat{u}-u\\ \hat{v}-v\end{array}\right]---\left(3\right)$

对于n幅图像，如果每幅图像可以提取出m个对应点时，就可以得到2mn个等式，进行数学整理之后，然后通过线性最小二乘方法即可求解出左相机的径向畸变系数k_l1和k_l2。同理，得到右相机的径向畸变系数k_r1和k_r2；

引入左右相机之间外参数不变这一约束条件，基于全局代价函数进行非线性优化；经过上面的初步标定过程，可以得到全局优化所需的初始参数：

(1)左相机的内参数，与标定板的外参数，径向畸变系数为A_l，R_il，t_il，k_1l和k_2l。右相机的内参数，与标定板的外参数，径向畸变系数为A_r，R_ir，t_ir，k_1r和k_2r。

(2)对于第i对图像，利用下式可以计算出左右相机之间的外参数：

$\left\{\begin{array}{c}{R}_i=R_{\mathrm{il}}{R}_{\mathrm{ir}}^{-1}\\ t_i=t_{\mathrm{il}}-R_{\mathrm{il}}{R}_{\mathrm{ir}}^{-1}{t}_{\mathrm{ir}}\end{array}\right.---\left(4\right)$

使用Rodrigues旋转公式，将旋转矩阵R_i转换成旋转向量，具体形式如下：

R_i＝[θ_xi θ_yi θ_zi]^T    (5)

其中θ_xi，θ_yi，θ_zi表示绕单位向量方向的旋转角。则左右相机之间的外参数R和t的初始值可以通过求R_i的θ_xi，θ_yi，θ_zi分量的均值和t_i的均值得到。

应该注意到：通常的立体标定方法是通过左右相机与标定板之间的外参数，设计一个局部代价优化函数来得到左右相机之间的外参数，在优化过程中没有充分考虑相机之间的外参数保持不变这个约束条件。基于这种情况，本文所选取的优化代价函数中引入了相机之间的外参数保持不变这一约束条件，这样可以在一定程度上降低标定方法的计算量，而实验测试结果表明这种处理方法可以提高立体视觉系统的标定精度。

由于左右相机的相对位姿固定，因此，它们之间的外参数R和t对于所有的标定图像对都相同。对于第i对图像，右相机与标定板之间的外参数R_ir和t_ir，可以利用左相机与标定板之间的外参数R_il和t_il，以及左右相机之间的外参数R和t，计算得到：

$\left\{\begin{array}{c}{r}_{\mathrm{ir}}=R^{-1}{R}_{\mathrm{il}}\\ t_{\mathrm{ir}}=R^{-1}(t_{\mathrm{il}}-t)\end{array}\right.---\left(6\right)$

基于这个关系，在优化过程中，我们将外参数R和t保持不变，而对于不同的图像对，左相机与标定板之间的外参数R_il和t_il则不一样，这样就保证了左右相机之间的外参数保持不变这一约束条件。

为了进一步得到立体视觉系统中的相机参数，需要构造一个合适的全局代价函数，来非线性优化左右相机的内参数A_l和A_r，径向畸变系数k_l1，k_l2，k_r1和k_r2，左相机与标定板的外参数R_il和t_il，以及左右相机之间的外参数R和t。考虑立体标定所需的所有内外参数，以及上述约束条件，建立了如下反映左右图像平面重投影误差这一全局代价函数：

$\min \{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\left|\right|({\hat{m}}_{\mathrm{ijl}}(A_l,R_{\mathrm{il}},t_{\mathrm{il}},M_{\mathrm{ij}})-{\tilde{m}}_{\mathrm{ijl}}(A_l,k_{1l},k_{2l}))\left|\right|}^2+$

$\left(7\right)$

$\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\left|\right|({\hat{m}}_{\mathrm{ijr}}(A_r,R_{\mathrm{il}},t_{\mathrm{il}},{R,t,M}_{\mathrm{ij}})-{\tilde{m}}_{\mathrm{ijr}}(A_r,k_{1r},k_{2r}))\left|\right|}^2\}$

其中，下标l表示左相机，下标r表示右相机。下标i表示第i幅标定图像，下标j表示标定图像中的第j个点。

和

为理想的像素坐标，

和

为实际的像素坐标经过径向畸变校正后得到的像素坐标。可以通过下式计算得出：

$\left\{\begin{array}{c}{\hat{m}}_l=A_l\left[\begin{array}{cc}{R}_l& t_l\end{array}\right]M\\ {\hat{m}}_r=A_r\left[\begin{array}{cc}{R}^{-1}{R}_l& R^{-1}(t_l-t)\end{array}\right]M\end{array}\right.---\left(8\right)$

$\left\{\begin{array}{c}{\tilde{m}}_l=m_l+{\mathrm{\δ}}_l\left(m_l\right)\\ {\tilde{m}}_r=m_r+{\mathrm{\δ}}_r\left(m_r\right)\end{array}\right.---\left(9\right)$

其中，m_l和m_r为实际的像素坐标，δ_l(m_l)和δ_r(m_r)为畸变量。式(9)可以通过式(2)计算得到。

在本文中，我们采用Levenberg-Marquardt迭代算法来对式(7)进行求解。在非线性优化过程中，充分考虑了左右相机之间的外参数保持不变这个约束条件，因此可以得到全局最优的左右相机内参数A_l和A_r，镜头径向畸变系数k_l1，k_l2，k_r1和k_r2，以及左右相机之间的外参数R和t；

第2、吊车工作空间中静态障碍物的重建

使用一种具有旋转不变性的边缘点匹配算法，通过将SIFT算子的旋转不变性引入匹配中来提高对图像旋转的鲁棒性，使用立体视觉系统对吊车工作空间进行拍摄，然后进行匹配，最终得到吊车工作空间中静态障碍物的轮廓点集。这种边缘匹配算法的流程如图11所示；

对立体视觉系统拍摄的吊车工作空间的图像进行预处理，包括灰度化处理，直方图均衡化，图像平滑滤波操作；

在图像空间中进行特征点提取，利用Canny边缘检测算子得到样本图和搜索图的轮廓点信息，并保存搜索图的轮廓图像；

对任意特征点，首先计算特征点主方向；在以该特征点为中心的邻域窗口内采样，使用梯度方向直方图统计邻域像素的梯度方向和幅值；直方图的峰值代表该特征点邻域内梯度的主方向，即特征点的主方向；像素点I(x，y)的梯度幅值m(x，y)和梯度方向θ(x，y)计算公式如下：

$m(x,y)=\sqrt{{(I(x+1,y)-I(x-1,y))}^2+{(I(x,y+1)-I(x,y-1))}^2}---\left(10\right)$

θ(x，y)＝atan2((I(x，y+1)-I(x，y-1))/(I(x+1，y)-I(x-1，y)))

获得特征点主方向后，利用邻域灰度信息，沿特征点主方向来建立描述符，从而保证特征点描述符具备旋转不变性；

为了提高匹配速度，算法利用两幅图像间极线约束(相机内、外参数已标定)，进行特征点搜索：任一空间点在两个像平面上的投影点，一定落在一对极线上。因此，在搜索图中寻找特征点时，并不需要在整幅图像中进行，而只需要沿着对应极线进行搜索就能够找到对应的特征点。这种处理可以将匹配过程从2维空间搜索降为1维线性搜索，大大提高了匹配效率。

为了消除旋转对匹配结果的影响，采用SIFT算子的旋转不变性来建立特征点描述符，但匹配结果出现二义性，即一个样本点可能对应多个搜索点，为此，采用一种“冗余角阈值”策略对二义性问题进行处理，在匹配时，仅仅将搜索图中对应主方向附近的特征点作为候选，以剔除错误的匹配点；

本文提出了一种“冗余角阈值”策略对二义性问题进行处理，以消除错误的候选匹配点。为此，在匹配时，仅仅将搜索图中对应主方向附近的特征点作为候选匹配点。

如图12所示，对两个特征点A和B(B点在A点对应极线上)，设其主方向分别为α_A和α_B。如果B是A的候选匹配点，则B点的主方向是A点主方向与对应的旋转角、冗余角之和，即：

α_B＝α_A+θ+ε    (11)

其中，ε的阈值为-δ≤ε≤δ，δ＞0。基于式(57)来排除错误的匹配点，提高匹配的正确率。因此在搜索图中，如果B点的主方向在相应的阈值范围内，即：

α_A+θ-δ≤α_B≤α_A+θ+δ    (12)

则其为A点的候选匹配点，继续计算两者描述符之间的相似度；反之，则排除该特征点。

随后的实验结果表明：上述方法可以很好地消除匹配的二义性问题。其中，冗余角ε的阈值是消除二义性问题的关键。选取合适的阈值范围，既可以保留正确的匹配点，又可以排除无效的匹配点。

特征点的匹配，其实质是计算描述符之间的相关度。对两个点p₁、p₂，设其描述符分别为v₁、v₂。v₁和v₂的相关度越高，则两个点的匹配度越高；反之，则匹配度越低。我们使用如下向量夹角来计算两个描述符之间的相关度：

$\cos \mathrm{\φ}=\frac{v_1^T{v}_2}{\left|\right|v_1\left|\right|\left|\right|v_2\left|\right|}---\left(13\right)$

其中，φ为描述符v₁和v₂的夹角。当cosφ＝1时，两特征点具有最佳匹配；而当cosφ＝0时，则匹配度最低。对于样本索引图中特征点描述符与候选匹配点，根据式(13)，计算两者描述符之间的相关度，判断候选点是否为正确的匹配点；

第3、吊车工作空间中动态障碍物的检测与跟踪

使用背景差分法，实时检测出突然进入吊车工作空间的运动物体。再使用camshift算法，实时跟踪负载和运动障碍物，估计它们各自的位置信息和尺寸信息。最后对这些信息进行kalman滤波，得到准确的估计值，其流程图如图16所示。

一般情况下，前景的运动目标的灰度值与背景的灰度值之间有很大的差异，而运动物体本身的灰度值一般不会有很大差异，因此通过图像差分就能很好地反映出两帧图像之间的变化，如能检测出这种变化，就可以将运动目标提取出来并分析其运动特征；

背景图像差分用数学公式表示变化检测为：

I_k(i，j)＝b_k(i，j)+m_k(i，j)+n_k(i，j)

                                        (14)

d_k(i，j)＝m_k(i，j)+n_k(i，j)

其中，I_k(i，j)是当前帧图像，b_k(i，j)表示当前帧的背景图像，m_k(i，j)表示运动部分图像，包括真正的运动、遮挡及显露部分，n_k(i，j)是各种系统和传感器固有噪声、目标周围背景引起的干扰噪声，d_k(i，j)表示当前帧图像减去背景图像所得到的差分图像，由运动部分和干扰噪声组成；

由于噪声及背景提取存在的误差，需要对得到的差分图像进行二值化处理，即将属于背景的静止像素和属于目标的运动像素分开，差分图像二值化公示为：

$B_k(i,j)=\left\{\begin{array}{cc}=0,& d_k(i,j)<T\\ =255,& d_k(i,j)\&GreaterEqual;T\end{array}\right.---\left(15\right)$

其中，B_k(i，j)是二值化图像，T代表阈值；

得到二值化图像后，对图像进行腐蚀和膨胀操作，消除孤立点或较小的孤立区域，连通碎化的区域，然后统计连通区域个数，判断是否有运动障碍物进入，对运动物体进行编号；同时统计连通区域的外接矩形，可以估计运动物体的尺寸信息，还可以用于camshift跟踪算法的初始化；

根据上面得到的图像上连通区域的外接矩形，估计运动物体的实际尺寸信息。空间中物体在相机中的投影如图18所示。其中，L为物体在垂直光轴的平面上的长度，l为物体在成像平面上的投影长度。匹配左右图像中的连通区域，根据图像上连通区域的中心位置，来计算物体中心的实际坐标，得到其深度值Z_C，连通区域的外接矩形如图19所示。

匹配左图像和右图像中的连通区域，根据图像上连通区域的中心位置，来计算物体中心的实际坐标，得到物体的深度值Z_C；

设连通区域的外接矩形的长轴为a，短轴为b，外接矩形与水平方向的偏转角度为θ；m₁(u₁，v₁，1)为外接矩形长边的一个端点，m₂(u₂，v₂，1)为外接矩形长边的另一个端点，Δu，Δv为长边在u，v方向上的长度，则有

Δu＝u₂-u₁＝asinθ

                          (16)

Δv＝v₂-v₁＝acosθ

根据投影公式，得到

$m_1=\frac{1}{Z_C}{\mathrm{AM}}_1$ $\left(17\right)$

$m_2=\frac{1}{Z_C}{\mathrm{AM}}_2$

其中，

为相机的内参数，a₁₁、a₂₂、a₁₂、a₁₃和a₂₃分别为内参数矩阵相应的元素。M₁(X_C1，Y_C1，Z_C，1)为空间中物体在垂直光轴的平面上长边的一个点，M₂(X_C2，Y_C2，Z_C，1)为长边的另一个点，则

$m_2-m_1=\frac{1}{Z_C}A(M_2-M_1)---\left(18\right)$

即

$\left[\begin{array}{c}\mathrm{\&Delta;u}\\ \mathrm{\&Delta;v}\\ 0\end{array}\right]=\frac{1}{Z_C}A\left[\begin{array}{c}\mathrm{\&Delta;}{X}_C\\ \mathrm{\&Delta;}{Y}_C\\ 0\\ 0\end{array}\right]---\left(19\right)$

其中，ΔX_C和ΔY_C为长边在在X，Y方向上的长度，则有：

ΔX_C＝X_C2-X_C1

                 (20)

ΔY_C＝Y_C2-Y_C1

则可以得到

$\mathrm{\&Delta;}{Y}_C=\frac{Z_C\mathrm{\&Delta;v}}{a_{22}}$ $\left(21\right)$

$\mathrm{\&Delta;}{X}_C=\frac{Z_C\mathrm{\&Delta;u}-a_{12}\mathrm{\&Delta;}{Y}_C}{a_{11}}$

进一步可以得到，物体的包围球半径

$r=\sqrt{{\left(\mathrm{\&Delta;}{X}_C\right)}^2+{\left(\mathrm{\&Delta;}{Y}_C\right)}^2}---\left(22\right)$

然后进行Camshift跟踪。Camshift算法可以分为两大部分，第一部分meanshift查找目标，第二部分自适应调整搜索窗口的长、宽、方向；对于每一幅图像，先采用meanshift算法计算出目标包围窗口，在通过公式自适应的计算出下一幅图像的初始搜索框的长和宽，这样就完成了对搜索框的自适应更新；

以上跟踪算法得到的只是物体每时刻位置的估计，这个估计是不准确，可以将这些不准确简单地看作跟踪过程中的噪声。为了最大限度地估计物体的运动，使用多个测量的累积可以检测出不受噪声影响的部分观测轨迹。

将时间k的状态描述推广为时间k-1的状态的函数。

x_k＝Fx_k-1+Bu_k+w_k    (23)

其中，x_k是一个状态元素的n维向量，F是一个与x_k想成的nxn矩阵，其被称为传递矩阵。向量u_k是先添加的。它的作用是允许外部控制施加于系统，由表示输入控制的c为向量组成。B是一个联系输入控制和状态改变的nxc矩阵。变量w_k时一个关联直接影响系统状态的随机时间或外力的随机变量(通常称为过程噪声)。假设w_k的元素具有高斯分布N(0，Q_k)，nxn协方差矩阵Q_k(Q可以随时间变化，但通常不这样做)。

一般来说，测量值z_k表示为：

z_k＝H_kx_k+v_k    (24)

这里，H_k是mxn矩阵，v_k是测量误差，也被假设为具有高斯分布N(0，R_k)和mxm协方差矩阵R_k。

应用在吊车系统中，我们将物体的状态用三个位置变量x、y和z，以及三个速度变量v_x，v_y和v_z表示。这六个变量组成状态向量x_k的元素。这就是说F的正确形式为：

$x_k={\left[\begin{array}{c}x\\ y\\ z\\ v_x\\ v_y\\ v_z\end{array}\right]}_k,F=\left[\begin{array}{cccccc}1& 0& 0& \mathrm{dt}& 0& 0\\ 0& 1& 0& 0& \mathrm{dt}& 0\\ 0& 0& 1& 0& 0& \mathrm{dt}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]---\left(25\right)$

但是，当使用立体视觉系统去测量物体的状态是，可能只能测量到位置变量：

$z_k={\left[\begin{array}{c}x\\ y\\ z\end{array}\right]}_k---\left(26\right)$

这就暗示了H结果类似于下面这样：

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]---\left(27\right)$

我们可能并不真正认为物体的速度是不变的，所以要设置一个值Q_k来反映这个问题。我们用立体视觉来测量物体的位置，再根据对测量精确程度的估计来选择R_k。

接下来，就是将这些表达式嵌入推广形式的更新等式中。首先要计算状态的先验估计

$x_k^{-}={\mathrm{Fx}}_{k-1}+{\mathrm{Bu}}_{k-1}+w_k---\left(28\right)$

其中，

表示“在新的测量之前”。

用

表示误差协方差，此协方差在实践k的鲜艳估计由其在时间k-1的值获得：

$P_k^{-}={\mathrm{FP}}_{k-1}{F}^T+Q_{k-1}---\left(29\right)$

这个等式就构成了预估器预测部分的基础。由此我们可以给出所谓的kalman更新率，它告诉我们如何给定新信息相对已经知道的信息的权重：

$K_k=P_k^{-}{H}_k^T{(H_k{P}_k^{-}{H}_k^T+R_k)}^{-1}---\left(30\right)$

根据我们已经获得的新的测量值，可以计算x_k和P_k最优的更新值：

$x_k=x_k^{-}+K_k(z_k-H_k{x}_k^{-})---\left(31\right)$

$P_k=(I-K_k{H}_k)R_k^{-}$

这样我们就可以得到最优的估计值；

第4、碰撞预测

使用包围球来产生物体(负载和障碍物)的碰撞模型，以静止包围球的碰撞相交算法为基础，得到运动的包围球碰撞算法。负载在场景中运动，根据运动包围球碰撞算法，实时预测负载与障碍物(静态或者动态)是否发生碰撞，以启动紧急制动控制。与静止的碰撞算法相比，运动碰撞预测算法考虑了物体之间的运动，即在物体发生实际碰撞之前，就可以预测出两个物体在将来是否会发生碰撞。其流程如图20所示。

第4.1、碰撞模型

包围球碰撞模型，是指包含物体的最小球体，包括球体中心坐标C和半径R；

根据立体匹配重建得到的物体轮廓的点集X_j(0≤j≤n)，首先，累积所有点的坐标向量获取平均向量C，即为包围球的中心点：

$C=\frac{1}{n}\underset{j=0}{\overset{n}{\mathrm{\&Sigma;}}}{X}_j---\left(32\right)$

然后，再根据所有点到中心点的最大距离确定包围球的半径R：

$R={\max }_j\sqrt{{\left|\right|X_j-C\left|\right|}^2}---\left(33\right)$

根据第3步实时跟踪并经过kalman滤波后得到的中心点位置作为包围球的中心点，实时估计的最大半径作为包围球的半径，并可以在线更新；

计算负载包围球中心Q_l与障碍物包围球中心Q_o之间的距离d₁：

$d_1=\sqrt{{\left|\right|O_l-O_o\left|\right|}^2}---\left(34\right)$

判断障碍物是否在安全半径范围之内，如果在，进行下一步；

安全半径R_safe由吊车负载的最大安全制动距离d_safe、负载包围球半径R_l和障碍物包围球半径R_o组成：

R_safe＝d_safe+R_l+R_o    (35)

计算负载与障碍物的相对速度是否为零，如果相对速度为零，则两个物体不会发生碰撞；若不为零，则进行下一步；

其中，相对速度V_rel由负载速度V_l和障碍物速度V_o生成：

V_rel＝V_l-V_o    (36)

计算直线与平面的交点P_inter。其中，直线过负载中心点且方向向量为相对速度方向，平面过障碍物中心点且法向量为相对速度方向；

空间直线方程参数形式为：

$\left\{\begin{array}{c}x=x_0+k_{x}t\\ y=y_0+k_{y}t\\ z=z_0+k_{z}t\end{array}\right.---\left(37\right)$

其中(x₀，y₀，z₀)为负载中心点坐标，(k_x，k_y，k_z)为相对速度向量且为直线的方向向量；空间平面的点法式形式，为：

k_x(x-x₁)+k_y(y-y₁)+k_z(z-z₁)＝0    (38)

其中，(x₁，y₁，z₁)为障碍物中心点坐标，(k_x，k_y，k_z)为相对速度向量且为平面的法向量；

联立两式，即可得到交点P_inter的坐标；

然后计算交点P_inter与障碍物中心点之间距离d₂：

$d_2=\sqrt{{\left|\right|P_{\mathrm{inter}}-O_o\left|\right|}^2}---\left(39\right)$

如果d₂小于R_l和R_o之和，则负载与障碍物发生碰撞，反之，则不发生碰撞；

当预测负载与障碍物将发生碰撞时，及时采取制动措施。

实验效果描述

第1、双目立体视觉系统的标定

为了验证本文标定方法的可靠性和标定精度，设计了视觉标定和立体视觉测量实验，并将本文提出的标定方法与通常的立体标定方法进行了对比分析。

采用本文的立体标定方法，对图6所示的立体视觉系统进行标定。该系统由两套MV-VS045FC/FM(分辨率为780×582)工业相机和焦距5mm镜头组成。实验所用的标定板4是一个5行7列的棋盘格，棋盘格大小为39.5mm×39.5mm。实验中，左右相机平行安装，右相机相对左相机沿X方向偏移150mm。左右相机由相机固定支架1固定，相对位置不变。

为了验证本文标定方法的可靠性和稳定性，共进行了7次独立标定实验，每次左右相机各拍摄6幅图像。为了便于比较，我们同时利用本文提出的方法和通常的方法对立体视觉系统进行了标定，得到的标定结果分析如下：

图7描述了使用不同标定算法得到的重投影残差。如图所示，本文标定方法得到的重投影残差比通常立体标定方法得到的重投影残差更小，且波动更小。进一步分析实验数据可知：本文标定方法得到的重投影残差的均值为7.0836(pixel)，方差为0.0595(pixel)。而通常立体标定方法得到的重投影残差的均值为15.2569(pixel)，方差为35.9509(pixel)。

图8和图9分别描述了使用不同标定算法得到的左右相机的内参数和畸变系数。如图所示，本文方法得到的左右相机的内参数和径向畸变系数，与通常立体标定方法得到的参数相比，均值相差不大。但本文标定方法得到的参数波动更小。进一步分析实验数据可知：本文标定方法得到的左相机的主点坐标均值为(356.5846，289.4626)，方差为(0.9706，0.3999)。而通常立体标定方法得到的左相机的主点坐标均值为(356.4123，288.1458)，方差为(2.7240，3.3397)。

图10描述了使用不同标定算法得到的左右相机之间的外参数。如图所示，本文标定方法得到的t_y和t_z的均值与通常立体标定方法得到的参数均值相比更小，更接近真实测量值，且波动更小。进一步分析实验数据可知到：本文标定方法得到的t_z的均值为4.4705mm，方差为0.9531mm。通常立体标定方法得到的tz的均值为8.9545mm，方差为21.4919mm。

综上所述，与通常的立体标定方法相比较，本文标定方法得到的结果，可靠性和稳定性更好。

第2、吊车工作空间中静态障碍物的重建

使用该方法对吊车工作场景进行重建，图13是左相机拍摄的图像，图14是右相机拍摄的图像，图15是计算静态障碍物的轮廓点集，重建效果如下：

第3、吊车工作空间中动态障碍物的检测与跟踪

使用背景差分法，实时检测出突然进入吊车工作空间的运动物体。再使用camshift算法，实时跟踪负载和运动障碍物，估计它们各自的位置信息和尺寸信息。最后对这些信息进行kalman滤波，得到准确的估计值，其流程图如图16所示。

第3、吊车工作空间中动态障碍物的检测与跟踪

使用该方法跟踪负载，同时实时检测出突然进入吊车工作空间的运动物体。再使用camshift算法，实时跟踪负载和运动障碍物，估计它们各自的位置信息和尺寸信息。最后对这些信息进行kalman滤波，得到准确的估计值。

图23描述了避障系统对负载和动态障碍物的尺寸的估计，在线生成和更新负载和动态障碍物的包围球半径。

图24描述了负载与障碍物之间的距离。其中，包括与动态障碍物的距离，以及与两个静态障碍物的距离。可以看出，在碰撞预测的时刻，负载与障碍物都没有发生实际的碰撞。

图25描述了对负载速度的估计。包括两个过程：检测负载和跟踪负载。

图26描述了对运动障碍物速度的估计。

第4、碰撞预测

这里假设安全距离为d_safe＝600mm。系统成功检测出运动障碍物进入吊车工作场景，并且能很好的跟踪负载和运动障碍物，准确地估计它们的速度信息和尺寸信息，同时能准确预测出发生碰撞。负载在运行到第217时刻时，预测出负载与运动障碍物发生碰撞，如图23、24、25和26中的竖直直线所在的时刻。

Claims (1)

1.一种基于立体视觉的吊车避障系统，其特征在于该系统由四部分组成，包括：

第1、双目立体视觉系统的标定

第1.1、模型分析

相机模型采用通常的针孔模型描述三维欧式空间到二维图像平面的投影变换，双目立体视觉系统一般由左右两个相机组成，其成像模型均采用所述的针孔模型；在双目立体视觉系统中，需要标定的参数包括左相机和右相机的内参数A_l、A_r，A_l为左相机的内参数矩阵，Ar为右相机的内参数矩阵，以及左相机和右相机之间的外参数R和t，R和t分别表示左相机和右相机之间外参数的旋转矩阵和平移向量；

第1.2、基于全局代价函数优化的立体标定

立体标定包括左相机和右相机的内参数标定、左相机和右相机分别与标定板之间的外参数标定、镜头畸变系数的标定、以及左右相机之间外参数标定；

引入左右相机之间外参数保持不变这一约束条件，并选取全局代价函数作为优化目标进行非线性优化，所述基于全局代价函数优化的立体标定方法包括：角点提取、2维单应矩阵的估计、相机内外参数的估计、径向畸变的估计，以及基于全局代价函数的非线性优化五个步骤：

第1.2.1、左右相机同时各拍摄3幅以上的标定图像，利用Harris角点检测算法提取出角点坐标，精确至亚像素精度；

第1.2.2、对于每幅图像，计算左右相机图像平面与标定板之间的2维单应矩阵；

第1.2.3、估计得到左右相机的内参数，以及它们与标定板之间的外参数；

第1.2.4、采用最小二乘方法估计径向畸变系数；

第1.2.5、引入左右相机之间外参数不变这一约束条件，基于全局代价函数进行非线性优化；

第2、吊车工作空间中静态障碍物的重建

首先提取图像的边缘特征；然后利用极线约束和邻域灰度信息，沿特征点的主方向建立描述符；同时通过“冗余角阈值”策略来选择合适的候选匹配点；最后通过计算特征点与候选匹配点之间的相关度，得到正确的匹配点,

第2.1、具有旋转不变性的边缘点匹配算法

针对传统边缘匹配算法对图像旋转敏感，易发生误匹配的缺点，提出了一种具有旋转不变性的边缘点匹配算法，通过将SIFT算子的旋转不变性引入匹配中来提高对图像旋转的鲁棒性，具体包括五个步骤：

第2.1.1、图像预处理，包括灰度化处理，直方图均衡化，图像平滑滤波操作；

第2.1.2、特征点提取，特征点提取在图像空间中进行，利用Canny边缘检测算子得到样本图和搜索图的轮廓点信息，并保存搜索图的轮廓图像；

第2.1.3、特征点描述符的建立

对任意特征点，首先计算特征点主方向；在以该特征点为中心的邻域窗口内采样，使用 梯度方向直方图统计邻域像素的梯度方向和幅值；直方图的峰值代表该特征点邻域内梯度的主方向，即特征点的主方向；获得特征点主方向后，利用邻域灰度信息，沿特征点主方向来建立描述符，从而保证特征点描述符具备旋转不变性；

第2.1.4、候选匹配点的选取

为了消除旋转对匹配结果的影响，采用SIFT算子的旋转不变性来建立特征点描述符，但匹配结果出现二义性，即一个样本点可能对应多个搜索点，为此，采用一种“冗余角阈值”策略对二义性问题进行处理，在匹配时，仅仅将搜索图中对应主方向附近的特征点作为候选，以剔除错误的匹配点；

第2.1.5、特征点描述符相关度的计算

特征点的匹配，实质是计算特征点描述符之间的相关度；对两个点p₁、p₂，设其描述符分别为v₁、v₂；v₁和v₂的相关度越高，则两个点的匹配度越高；反之，则匹配度越低；使用如下向量夹角余弦值来计算两个描述符之间的相关度，当余弦值等于1时，两特征点具有最佳匹配；当余弦值等于0时，则匹配度最低；

使用以上“吊车工作空间中静态障碍物的重建”方法对吊车工作场景进行重建，最终得到吊车工作空间中静态障碍物的位置信息和尺寸大小；

第3、吊车工作空间中动态障碍物的检测与跟踪

使用背景差分法，实时检测出突然进入吊车工作空间的运动物体， 再使用camshift算法，实时跟踪负载和运动障碍物，估计它们各自的位置信息和尺寸信息， 最后对这些信息进行kalman滤波，得到准确的估计值；

第3.1、基于背景差分法的运动检测

第3.1.1、一般情况下，前景的运动目标的灰度值与背景的灰度值之间有很大的差异，而运动物体本身的灰度值一般不会有很大差异，因此通过图像差分就能很好地反映出两帧图像之间的变化，如能检测出这种变化，就可以将运动目标提取出来并分析其运动特征；

第3.1.2、由于噪声及背景提取存在的误差，需要对得到的差分图像进行二值化处理，即将属于背景的静止像素和属于目标的运动像素分开，得到二值化图像；

第3.1.3、得到二值化图像后，对图像进行腐蚀和膨胀操作，消除孤立点或较小的孤立区域，连通碎化的区域，然后统计连通区域个数，判断是否有运动障碍物进入，对运动物体进行编号；同时统计连通区域的外接矩形，可以估计运动物体的尺寸信息，还可以用于camshift跟踪算法的初始化；

第3.2、尺寸信息估计

根据第3.1.3步得到的图像上连通区域的外接矩形，估计运动物体的实际尺寸信息；

第3.2.1、匹配左图像和右图像中的连通区域，根据图像上连通区域的中心位置，来计算物体中心的实际坐标，得到物体的深度值Z_C； 

第3.2.2、已知连通区域的外接矩形的长轴a，短轴b，以及外接矩形与水平方向的偏转角度为θ，可以得到长边在u，v方向上的长度Δu，Δv；根据投影原理，可以得到物体在X_C，Y_C方向上的长度ΔX_C和ΔY_C，进一步得到物体的包围球半径r；

第3.3、camshift跟踪算法

Camshift算法可以分为两大部分，第一部分meanshift查找目标，第二部分自适应调整搜索窗口的长、宽、方向；对于每一幅图像，先采用meanshift算法计算出目标包围窗口，在通过公式自适应的计算出下一幅图像的初始搜索框的长和宽，这样就完成了对搜索框的自适应更新；

第3.4、kalman滤波

以上第3.3步跟踪算法得到的只是物体每时刻位置的估计，这个估计是不准确，可以将这些不准确简单地看作跟踪过程中的噪声；为了最大限度地估计物体的运动，使用多个测量的累积可以检测出不受噪声影响的部分观测轨迹；应用在吊车系统中，我们将物体的状态用三个位置变量x、y和z，以及三个速度变量v_x，v_y和v_z表示，这六个变量组成状态向量x_k的元素；我们用立体视觉来测量物体的位置，作为观测值来进行更新，就可以得到最优的估计值；

第4、碰撞预测

使用包围球来表示负载和障碍物的碰撞模型，以静止包围球的碰撞相交算法为基础，得到运动的包围球碰撞算法；负载在场景中运动，根据运动包围球碰撞算法，实时预测负载与静态或者动态障碍物是否发生碰撞，以启动紧急制动控制；

第4.1、碰撞模型

包围球碰撞模型，是指包含物体的最小球体，包括球体中心坐标C和半径R；

第4.1.1、静态障碍物的包围球创建

根据立体匹配重建得到的物体轮廓的点集X_j(0≤j≤n)，首先，累积所有点的坐标向量获取平均向量C，即为包围球的中心点；然后再根据所有点到中心点的最大距离确定包围球的半径R；

第4.1.2、动态障碍物的包围球创建

根据第3步实时跟踪并经过kalman滤波后得到的中心点位置作为包围球的中心点，实时估计的最大半径作为包围球的半径，并可以在线更新；

第4.2、运动碰撞预测算法

第4.2.1、计算负载包围球中心O_l与障碍物包围球中心O_o之间的距离d₁，判断障碍物是否在安全半径范围之内，如果在，进行下一步；其中，安全半径R_safe由吊车负载的最大安全制动距离d_safe、负载包围球半径R_l和障碍物包围球半径R_o组成；

第4.2.2、计算负载与障碍物的相对速度是否为零，如果相对速度为零，则两个物体不会发生碰撞；若不为零，则进行下一步；其中，相对速度V_rel由负载速度V_l和障碍物速 度V_o生成；

第4.2.3、计算直线与平面的交点P_inter；其中，直线过负载中心点且方向向量为相对速度方向，平面过障碍物中心点且法向量为相对速度方向；

第4.2.4、然后计算交点P_inter与障碍物中心点之间距离d₂；如果d₂小于R_l和R_o之和，则负载与障碍物发生碰撞，反之，则不发生碰撞；

第4.2.5、当预测负载与障碍物将发生碰撞时，及时采取制动措施。 

Images