CN112985259B - 基于多目视觉的目标定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于多目视觉的目标定位方法及系统，该目标定位方法包括：在待测目标区域覆盖至少四个视觉测量单元，每个视觉测量单元包括至少一个相机和至少一个激光测距仪；确定视觉测量单元之间的相对位置关系；标定视觉测量单元中相机与激光测距仪之间的相对位姿关系；利用相机采集包括目标区域的图像，确定目标在图像中的位置和目标在相机坐标系下的方向；调整激光测距仪，使激光测距仪对准目标，测量目标到激光测距仪的距离；利用至少四个视觉测量单元获取的距离信息，计算目标的位置。本发明的基于多目视觉的目标定位方法及系统能够实现户外大范围场景下的目标定位，硬件要求低、定位精度高，适用场景更广泛。

Description

基于多目视觉的目标定位方法及系统

技术领域

本发明涉及视觉定位技术领域，具体涉及一种基于多目视觉的目标定位方法及系统。

背景技术

现有目标定位方法主要通过对极几何理论和多个相机之间极线约束来求解目标的位置信息。为了完成目标位置估计，既要对单个相机标定内参数，又要标定多个相机之间的外参数，整个求解过程繁琐耗时，通常用于解决室内或者户外小范围场景下的目标定位问题。当用于户外大范围场景下的目标定位时，现有目标定位方法存在如下问题：难以设计合适标定板，标定模式检测精度低；相机之间的基线太长，距离太远，场景中难以找到多个相机之间可视标定图像；若采用全站仪确定标志点，则过程非常复杂，相机运动时需要重新标定。因此，采用传统的方法难以解决户外大范围场景下目标定位问题。

发明内容

为解决上述现有技术中存在的技术问题，本发明提供一种基于多目视觉的目标定位方法及系统。

第一方面，本发明公开了一种基于多目视觉的目标定位方法，包括：

在待测目标区域覆盖至少四个视觉测量单元，每个视觉测量单元包括至少一个相机和至少一个激光测距仪；

确定视觉测量单元之间的相对位置关系；

标定视觉测量单元中相机与激光测距仪之间的相对位姿关系；

利用相机采集包括目标区域的图像，确定目标在图像中的位置和目标在相机坐标系下的方向；

调整激光测距仪，使激光测距仪对准目标，测量目标到激光测距仪的距离；

利用至少四个视觉测量单元获取的距离信息，计算目标的位置。

在一些可选的实施方式中，视觉测量单元安装在可调节云台上。

在一些可选的实施方式中，标定视觉测量单元中相机与激光测距仪之间的相对位姿关系，包括步骤S31-S32：

S31，标定相机内参；

S32，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

在一些可选的实施方式中，当激光测距仪的激光光斑可见时，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标，包括步骤S3201-S3210：

S3201，设定激光测距仪光轴的三维向量为^Cn＝[n_x,n_y,n_z]^T，

激光测距仪原点在相机坐标系下的坐标为^Ct＝[t_x,t_y,t_z]^T；

S3202，利用激光测距仪测量相机坐标系下任意一点^Cp_i＝[^Cx_i,^Cy_i,^Cz_i]^T，并得到反馈距离l_i；

S3203，确定激光测距仪的测量斑点的图像坐标值w_i＝[u_i,v_i,1]^T；

S3204，利用标定的相机内参数K和畸变参数D得到图像坐标到相机坐标的投影方程P(K,D)，将图像坐标值w_i＝[u_i,v_i,1]^T转换到相机坐标系下为：k_i[x_i,y_i,1]^T＝P(K,D)w_i，x_i和y_i能够通过以上方程求解，k_i为未知参数；

S3205，根据k_i[x_i,y_i,1]^T＝^Ct+^Cn*l_i＝[^Cx_i,^Cy_i,^Cz_i]^T，确定求解方程组：

f_q1＝t_x+n_xl_i-x_it_z-x_in_zl_i＝0

f_q2＝t_y+n_yl_i-y_it_z-y_in_zl_i＝0

S3206，移动视觉测量单元，获取Q个不同的图像，Q≥3，基于获取的图像得到Q个方程组，Q个方程组包括2Q个方程；

S3207，基于方程组，利用奇异值求解法求解光轴向量^Cn＝[n_x,n_y,n_z]^T和原点坐标^Ct＝[t_x,t_y,t_z]^T的初始值；

S3208，构造^Cn的约束条件为：

S3209，构造优化函数为：

ρ表示惩罚因子；

S3210，根据^Cn和^Ct的初始值、约束条件和优化函数，通过最小二乘优化算法获取^Cn和^Ct的精确值，并对获取的^Cn进行归一化，以转换为单位向量。

在一些可选的实施方式中，当激光测距仪的激光光斑不可见时，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标，包括步骤S3211-S3222：

S3211，设定激光测距仪光轴的三维向量为^Cn＝[n_x,n_y,n_z]^T，

激光测距仪原点在相机坐标系下的坐标为^Ct＝[t_x,t_y,t_z]^T；

S3212，构建同时满足相机和激光测距仪的测量要求的平面标定板；

S3213，确定标定板坐标系与相机坐标系的相对位姿关系；

S3214，确定标定板平面方程：π:AX+BY+CZ+D＝0；

S3215，确定相机坐标系下标定板平面方程：A'x+B'y+C'z+D'＝0；

S3216，利用激光测距仪测量相机坐标系标定板上任意一点^Cp_i＝[^Cx_i,^Cy_i,^Cz_i]^T，并得到反馈距离l_i；

S3217，确定求解方程组：

f_p1＝^Cx_i-t_x-n_xl_i＝0

f_p2＝^Cy_i-t_y-n_yl_i

f_p3＝^Cz_i-t_z-n_zl_i

f_p4＝(A',B',C',D')[^Cx_i,^Cy_i,^Cz_i,1]^T＝0

S3218，移动标定板，且保证移动前后的标定板不平行，获取P个不同的图像，P≥3，基于获取的图像得到P个方程组，P个方程组包括4P个方程；

S3219，基于方程组，利用奇异值求解法求解光轴向量^Cn＝[n_x,n_y,n_z]^T和原点坐标^Ct＝[t_x,t_y,t_z]^T的初始值；

S3220，构造^Cn的约束条件为：

S3221，构造优化函数为：

ρ表示惩罚因子；

S3222，根据^Cn和^Ct的初始值、约束条件和优化函数，通过最小二乘优化算法获取^Cn和^Ct的精确值，并对获取的^Cn进行归一化，以转换为单位向量。

在一些可选的实施方式中，标定视觉测量单元中相机与激光测距仪之间的相对位姿关系，还包括步骤S33-S34：

S33，基于步骤S32的结果，对激光测距仪和相机的位置进行校正，以使激光测距仪原点在相机坐标系下的坐标值在设定范围内，使激光测距仪的光轴与相机光轴平行；

S34，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

在一些可选的实施方式中，采用YOLO目标检测算法确定目标在图像中的位置和目标在相机坐标系下的方向。

在一些可选的实施方式中，调整激光测距仪以使激光测距仪对准目标时，激光测距仪的旋转轴和旋转角度利用以下公式确定；

rot_axis＝^Cn×^Cn_t

θ＝arccos(^Cn·^Cn_t)

rot_axis表示旋转轴，^Cn表示激光测距仪光轴向量，^Cn_t表示目标在相机坐标系下的方向向量，θ表示旋转角度。

在一些可选的实施方式中，设定：共有k个视觉测量单元观测到目标，k个视觉测量单元中激光测距仪测量得到的目标距离分别表示为l_i，k个视觉测量单元在全局坐标系下的坐标分别为p_i＝(x_i,y_i,z_i)，i＝1,2,...,k，目标在全局坐标系下的坐标为p_t＝(x_t,y_t,z_t)；

目标的位置通过对方程

进行消元求解确定，i＝1,2,...,k。

第二方面，本发明公开了一种基于多目视觉的目标定位系统，包括：

至少四个视觉测量单元，每个所述视觉测量单元包括至少一个相机和至少一个激光测距仪，用于采集包括目标区域的图像，以及测量目标到所述视觉测量单元的距离；

相对位置确定单元，用于确定所述视觉测量单元之间的相对位置关系；

相对位姿标定单元，用于标定所述视觉测量单元中相机与激光测距仪之间的相对位姿关系；

图像处理单元，用于确定目标在图像中的位置和目标在相机坐标系下的方向；

调整单元，用于调整激光测距仪，使激光测距仪对准目标；

目标位置计算单元，用于根据视觉测量单元获取的距离信息，计算目标的位置。

本发明技术方案的主要优点如下：

本发明的基于多目视觉的目标定位方法及系统能够实现户外大范围场景下的目标定位，硬件要求低、定位精度高，适用场景更广泛。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一实施例的基于多目视觉的目标定位方法的流程图；

图2为本发明一实施例的视觉测量单元的位置布置示意图；

图3为本发明一实施例的相机与激光测距仪的标定过程示意图；

图4为本发明一实施例的真实目标位置与激光测量位置的关系示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例及相应的附图对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

以下结合附图，详细说明本发明实施例提供的技术方案。

第一方面，参见图1，本发明一实施例提供了一种基于多目视觉的目标定位方法，该方法包括步骤S1-S6：

S1，在待测目标区域覆盖至少四个视觉测量单元，每个视觉测量单元包括至少一个相机和至少一个激光测距仪；

S2，确定视觉测量单元之间的相对位置关系；

S3，标定视觉测量单元中相机与激光测距仪之间的相对位姿关系；

S4，利用相机采集包括目标区域的图像，确定目标在图像中的位置和目标在相机坐标系下的方向；

S5，调整激光测距仪，使激光测距仪对准目标，测量目标到激光测距仪的距离；

S6，利用至少四个视觉测量单元获取的距离信息，计算目标的位置。

以下对本发明一实施例提供的基于多目视觉的目标定位方法的步骤S1-S6进行具体说明。

S1，在待测目标区域覆盖至少四个视觉测量单元，每个视觉测量单元包括至少一个相机和至少一个激光测距仪；

具体地，参见图2，在待测目标区域覆盖n个测量单元，n≥4，每个视觉测量单元包括至少一个相机和至少一个激光测距仪，每个视觉测量单元可以分别安装在一个可调节云台上，以便于后续利用可调节云台对相机和激光测距仪的位置进行调整。

S2，确定视觉测量单元之间的相对位置关系；

具体地，在完成视觉测量单元的布置安装后，可以利用差分卫星定位系统、全站仪或者激光测距仪等辅助设备确定n个视觉测量单元之间的相对位置关系。

S3，标定视觉测量单元中相机与激光测距仪之间的相对位姿关系；

由于激光测距仪仅仅提供了单点信息，故无法通过传统的方法标定相机与激光测距仪的外参数。本发明一实施例中，由于仅利用相机与激光测距仪的外参关系来确定目标的位置，因此仅需确定在相机坐标系下的激光测距仪的光轴向量(单位向量形式)，以及激光测距仪原点在相机坐标系下的坐标，即相机与激光测距仪之间的相对位姿关系包括：相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

基于上述待确定的相机与激光测距仪之间的相对位姿关系的具体内容，本发明一实施例中，标定视觉测量单元中相机与激光测距仪之间的相对位姿关系包括以下步骤S31-S32：

S31，标定相机内参；

S32，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

相机标定就是要得到相机模型的内参数、外参数和畸变参数，已知这些参数和相机模型就能够建立三维空间点与二维图像点之间的映射关系，相机只有经过标定后才能够应用于三维环境感知、重建、视觉测量、摄像机自身运动估计等任务，而标定结果的精度也直接关系到其应用效果的好坏。

本发明一实施例中，可以利用张正友标定法标定相机内参。

张正友标定法是一种基于单平面棋盘格的相机标定方法，通过利用棋盘格标定板即可标定出相机的内外参数和畸变参数，标定过程简单，精度高。由于棋盘格几何信息已知，根据每一帧图像检测到的棋盘格角点可以得到一组二维图像坐标和三维世界坐标相对应的角点集合(m_pij,m_wij)，i为图像索引，j为i图像的第j个角点特征，m_pij表示特征点的二维图像坐标，m_wij表示特征点在世界坐标系的坐标，利用优化方法使得投影误差最小化，得到相机内参数和畸变参数。具体地，构建优化函数如下：

其中，x为相机映射模型Φ中的参数，参数x包括相机内参数K、外参数R和畸变参数D，M是采集到的平面模板图像数，N是图像上提取的特征点数。

进一步地，基于标定的相机内参和畸变参数，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

由于激光测距仪存在激光光斑可见和激光光斑不可见的情况，为此，本发明一实施例中，分以下两种情况确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

具体地，当激光光斑可见时，采用下述步骤S3201-S3210所述的过程来确定相机与激光测距仪之间的相对位姿关系，即确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

S3201，设定激光测距仪光轴的三维向量为^Cn＝[n_x,n_y,n_z]^T，

激光测距仪原点在相机坐标系下的坐标为^Ct＝[t_x,t_y,t_z]^T。

S3202，利用激光测距仪测量相机坐标系下任意一点^Cp_i＝[^Cx_i,^Cy_i,^Cz_i]^T，并得到反馈距离l_i。

S3203，确定激光测距仪的测量斑点的图像坐标值w_i＝[u_i,v_i,1]^T。

S3204，利用标定的相机内参数K和畸变参数D得到图像坐标到相机坐标的投影方程P(K,D)，将图像坐标值w_i＝[u_i,v_i,1]^T转换到相机坐标系下为：k_i[x_i,y_i,1]^T＝P(K,D)w_i，x_i和y_i能够通过以上方程求解，k_i为未知参数。

转换后的坐标值可以表述为k_i[x_i,y_i,1]^T＝[^Cx_i,^Cy_i,^Cz_i]^T＝^Cp_i，可以得到方程：^Cp_i＝[^Cx_i,^Cy_i,^Cz_i]^T＝^Ct+^Cn*l_i＝k_i[x_i,y_i,1]^T。

方程^Cp_i＝[^Cx_i,^Cy_i,^Cz_i]^T＝^Ct+^Cn*l_i＝k_i[x_i,y_i,1]^T可以分解为三个方程：

^Cx_i＝t_x+n_xl_i＝k_ix_i

^Cy_i＝t_y+n_yl_i＝k_iy_i

^Cz_i＝t_z+n_zl_i＝k_i。

S3205，将上述三个方程转化为求解方程组：

f_q1＝t_x+n_xl_i-x_it_z-x_in_zl_i＝0

f_q2＝t_y+n_yl_i-y_it_z-y_in_zl_i＝0。

S3206，移动视觉测量单元，获取Q个不同的图像，Q≥3，基于获取的图像得到Q个方程组，Q个方程组包括2Q个方程。

由于每个方程组包括两个方程，两个方程包含光轴向量^Cn＝[n_x,n_y,n_z]^T和原点坐标^Ct＝[t_x,t_y,t_z]^T的六个未知参数，利用一个图像可以获得一个方程组，利用至少三个不同的图像可以获得至少三个不同的方程组，利用三个以上不同的方程组可以求解得到光轴向量和原点坐标的六个未知参数。

S3207，基于方程组，利用奇异值求解法求解光轴向量^Cn＝[n_x,n_y,n_z]^T和原点坐标^Ct＝[t_x,t_y,t_z]^T的初始值。

S3208，构造^Cn的约束条件为：

S3209，构造优化函数为：

其中，ρ表示惩罚因子。

S3210，根据^Cn和^Ct的初始值、约束条件和优化函数，通过最小二乘优化算法获取^Cn和^Ct的精确值，并对获取的^Cn进行归一化，以转换为单位向量。

为了提高求解精度，将约束条件和优化函数引入方程组求解过程。由于^Cn和^Ct的初始值已经通过方程组求解得到，基于上述构造的约束条件和优化函数，可以进一步通过优化算法以得到^Cn和^Ct的精确值，从而提高计算精度，优化算法例如为最小二乘优化算法。同时，若求解出来的^Cn不是标准单位矩阵，则可对^Cn进行归一化处理，以转换为单位向量。

具体地，当激光光斑不可见时，参见图3，采用下述步骤S3211-S3222所述的过程来确定相机与激光测距仪之间的相对位姿关系，即确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

S3211，设定激光测距仪光轴的三维向量为^Cn＝[n_x,n_y,n_z]^T，

激光测距仪原点在相机坐标系下的坐标为^Ct＝[t_x,t_y,t_z]^T。

S3212，构建同时满足相机和激光测距仪的测量要求的平面标定板。

平面标定板例如为棋盘格标定板，棋盘格标定板具有良好的激光反射特性，相机和激光测距仪可以同时对标定板进行测量。

S3213，确定标定板坐标系与相机坐标系的相对位姿关系。

由于标定板的几何信息已知，该步骤过程与相机内参标定过程类似。

具体地，在图像上检测视觉标志点或角点，最终得到一组二维图像坐标和三维世界坐标相对应视觉标志点或角点的集合(m_pj,m_wj)，j表示图像的第j个特征，m_pj表示特征点在二维图像上坐标，m_wj表示特征点在世界坐标系的坐标，利用直接线性变换(DLT)算法或PNP算法求解相机坐标系与世界坐标系初始外参

和^Ct_OW。

由于已标定出相机内参和畸变参数，此处仅仅需要计算外参。其中，

和^Ct_OW分别表示标定板坐标系F(W)相对于相机坐标系F(C)旋转变换以及标定板坐标系F(W)的原点在相机坐标系F(C)下的表示，

和^Ct_OW即为待确定的标定板坐标系与相机坐标系的相对位姿关系。

本发明一实施例中，求解相机坐标系与世界坐标系初始外参

和^Ct_OW时，也可以利用非线性优化方法进行求解，以得到更精确的解。

具体地，在利用非线性优化方法进行求解时，构建优化函数如下：

其中，x为相机映射模型Φ中的参数，参数x包括相机内参数K、外参数R和畸变参数D，N是图像上提取的特征点数，外参数R由

和^Ct_OW构成，通过优化算法可得到更加准确的外参。

S3214，确定标定板平面方程：π:AX+BY+CZ+D＝0。

设定棋盘格标定板世界坐标系为F(W)，标定板平面表示为：π:AX+BY+CZ+D＝0，通过棋盘格角点坐标可以求解平面方程参数A,B,C,D，也可利用全站仪确定标定板关键点位置，最终确定标定板平面方程。

S3215，确定相机坐标系下标定板平面方程：A'x+B'y+C'z+D'＝0。

标定板平面的法向量为

设定o^w为标定板坐标系F(W)的原点，则标定板平面法向量n^w和原点坐标o^w在相机坐标系下可以表示为：

^Co_w＝^Ct_OW

n^C和^Co_w为标定板平面法向量和标定板坐标系F(W)的原点在相机坐标系下的表示。

标定板平面方程Ax+By+Cz+D＝0在相机坐标系下可以表示为：

(n^C)^T[X,Y,Z]^T+D'＝0

将^Ct_OW＝[X,Y,Z]^T代入上述方程可以求解D'，得到相机坐标系下标定板平面方程：A'x+B'y+C'z+D'＝0。

S3216，利用激光测距仪测量相机坐标系标定板上任意一点^Cp_i＝[^Cx_i,^Cy_i,^Cz_i]^T，并得到反馈距离l_i。

S3217，确定求解方程组：

f_p1＝^Cx_i-t_x-n_xl_i＝0

f_p2＝^Cy_i-t_y-n_yl_i

f_p3＝^Cz_i-t_z-n_zl_i

f_p4＝(A',B',C',D')[^Cx_i,^Cy_i,^Cz_i,1]^T＝0

由于，激光测距仪光轴的三维向量为^Cn＝[n_x,n_y,n_z]^T，激光测距仪原点在相机坐标系下的坐标为^Ct＝[t_x,t_y,t_z]^T，故点^Cp_i的坐标可以表示为：

^Cx_i＝t_x+n_xl_i

^Cy_i＝t_y+n_yl_i

^Cz_i＝t_z+n_zl_i

同时点^Cp_i在标定板平面上，则满足平面方程：(A,B,C,D)[^Cx_i,^Cy_i,^Cz_i,1]^T＝0。

S3218，移动标定板，且保证移动前后的标定板不平行，获取P个不同的图像，P≥3，基于获取的图像得到P个方程组，P个方程组包括4P个方程。

S3219，基于方程组，利用奇异值求解法求解光轴向量^Cn＝[n_x,n_y,n_z]^T和原点坐标^Ct＝[t_x,t_y,t_z]^T的初始值。

S3220，构造^Cn的约束条件为：

S3221，构造优化函数为：

其中，ρ表示惩罚因子。

S3222，根据^Cn和^Ct的初始值、约束条件和优化函数，通过最小二乘优化算法获取^Cn和^Ct的精确值，并对获取的^Cn进行归一化，以转换为单位向量。

为了提高求解精度，将约束条件和优化函数引入方程组求解过程。由于^Cn和^Ct的初始值已经通过方程组求解得到，基于上述构造的约束条件和优化函数，可以进一步通过优化算法以得到^Cn和^Ct的精确值，从而提高计算精度，优化算法例如为最小二乘优化算法。同时，若求解出来的^Cn不是标准单位矩阵，则可对^Cn进行归一化处理，以转换为单位向量。

利用上述的求解步骤分别标定每个视觉测量单元中相机与激光测距仪之间的相对位姿关系。

进一步地，参见图4，激光测距仪原点坐标为^Ct，光轴方向为^Cn，相机坐标系下的目标点为p_t，^Cn_t表示目标在相机坐标系下的方向，旋转激光光轴^Cn与^Cn_t重合后，可以发现激光测距仪测量的点实际为p′_t点，并不是真实的目标点位置。p′_t与p_t之间并不一致，存在一定误差，该误差是由相机坐标系原点与激光测距仪坐标系原点不一致(实质是^Ct)引入的。^Ct点到^Cn_t方向的垂直距离体现了横向误差，^Ct点与^Cn_t直线的焦点与相机坐标系原点的距离为纵向误差。在实际测量过程中，为了减小误差，需要使^Ct尽可能小，如此横向误差和纵向误差都会减小。

因此，在步骤S32的求解结果的基础上，可以进一步调整激光测距仪的位置，使得^Ct尽可能的小。此外，可以调整激光测距仪的光轴^Cn使得其与相机光轴(相机光轴向量为[0,0,1]^T)尽可能平行，如此，当目标处于相机光轴上时，激光测距仪可以直接测量到目标。

在实际测量过程中，当^Ct足够小，且测量的目标距离较远时，由^Ct引起的误差可以忽略不计。由激光测距仪测量得到的目标点位置为p′_t，当^Ct较小时，可以认为测量的为同一点，则：p_t＝^Ct+p′_t，^Ct值比较小忽略不计，默认为0。

为此，本发明一实施例中，标定视觉测量单元中相机与激光测距仪之间的相对位姿关系还可以包括以下步骤S33-S34：

S33，基于步骤S32的结果，对激光测距仪和相机的位置进行校正，以使激光测距仪原点在相机坐标系下的坐标值在设定范围内，使激光测距仪的光轴与相机光轴平行；

S34，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

本发明一实施例中，在步骤S33-S34中，在完成激光测距仪和相机的位置校正后，可以采用步骤S32所述的过程确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

S4，利用相机采集包括目标区域的图像，确定目标在图像中的位置和目标在相机坐标系下的方向；

本发明一实施例中，可以采用检测算法确定目标在图像中的位置和目标在相机坐标系下的方向。

检测算法可以为YOLO目标检测算法，采用YOLO检测算法确定目标在图像中的位置和目标在相机坐标系下的方向包括以下步骤S41-S43：

S41，离线采集目标数据，并进行标注；

S42，采用YOLO提供的预训练模型，利用新的数据对模型参数进一步优化；

S43，用训练好的模型进行目标检测，确定目标在图像中的位置和目标在相机坐标系下的方向。

S5，调整激光测距仪，使激光测距仪对准目标，测量目标到激光测距仪的距离；

在确定目标在图像中的位置后，根据目标位置调整激光测距仪，使激光测距仪的光束照射到目标，以测量目标到激光测距仪的距离。

具体地，在采用可调节云台安装视觉测量单元的情况下，可以采用视觉伺服控制方法旋转云台以调整激光测距仪。

本发明一实施例中，在调整过程中，可调节云台的旋转轴和旋转角度，即激光测距仪的旋转轴和旋转角度可以采用下述方式进行确定：

对于测量单元，假设通过检测算法得到目标位置的图像坐标值为w_i＝[u_i,v_i,1]^T，在相机内参数和扭曲参数标定后，可将其转换到相机坐标系下表示为k_i[^cx_i,^cy_i,1]^T，因此可以确定目标在相机坐标系下的方向为

由于激光测距仪光轴^Cn已标定，则可以确定旋转轴为：rot_axis＝^Cn×^Cn_t。

^Cn_t和^Cn均为单位向量，旋转角度为目标在相机坐标系下的方向与激光测距仪光轴的夹角，即可以确定旋转角度为：θ＝arccos(^Cn·^Cn_t)。

在已知旋转轴和旋转角度的情况下，可以旋转云台使得激光测距仪的激光光轴指向目标。同时，在旋转过程中，若目标一直在相机视野中移动，则可以通过持续监测，更新旋转轴和旋转角，使得激光光轴最终调整到位。

S6，利用至少四个视觉测量单元获取的距离信息，计算目标的位置。

假设共有k个视觉测量单元观测到目标，k个视觉测量单元中激光测距仪测量得到的目标距离分别表示为l_i(i＝1,2,...,k)，由于所有视觉测量单元在全局坐标系下的全局位姿已知，假设k个视觉测量单元在全局坐标系F(G)下的坐标分别为p_i＝(x_i,y_i,z_i)(i＝1,2,...,k)，假设目标在全局坐标F(G)下的坐标为p_t＝(x_t,y_t,z_t)。理论上在三维空间中四点即可确定目标位置，基于上述设定可以得到下列方程：

假设有四个视觉测量单元观测到目标，即k＝4。则对上列方程进行消元，可以得到下列方程组：

通过求解上列方程组，可以得到目标位置p_t＝(x_t,y_t,z_t)。

进一步地，若观测到目标的视觉测量单元大于四个，则可以采用优化算法求解目标位置。

在采用优化方法求解目标位置时，构造如下的优化函数。

具体地，上述方程组可以表示为：

为此，可以构造优化函数为：

通过优化算法可以进一步地提高目标位置的求解精度。

进一步地，本发明一实施例中，还可以通过RANSAC算法(随机采样算法)去除掉视觉测量单元中的外点，保留测量一致性的点，以提高算法的准确性。

随机采样算法的具体实现过程如步骤S61-S64所示；

S61，考虑一个最小抽样集的势为n＝4的模型和视觉测量单元样本集M＝{(l_i,p_i)}(i＝1,2,...,k)，集合M的数量k大于n，从集合M中随机抽取包含n个样本的子集S，通过构建方程组求解方法确定目标的位置p_t＝(x_t,y_t,z_t)；

S62，集合M的余集集合SC＝M-S中视觉测量单元与目标位置距离和真实测量之间的误差为：|p_i-p_t|-l_i，误差值小于设定阈值t的样本集以及S构成S*，S*认为是内点集，它们构成S的一致集，S*大于n时，通过上述的优化算法确定更加精确的目标位置p_t＝(x_t,y_t,z_t)；

S63，重新随机抽取新的子集S，重复以上过程；

S64，在完成N次抽样后，选择内点集最大的势对应的目标位置p_t＝(x_t,y_t,z_t)作为最终结果。

进一步地，本发明一实施例中，为了消除噪声，估计结果的平滑性，可以采用卡尔曼滤波算法提高算法的精度。

卡尔曼滤波是一种基于概率模型的状态和参数的估计，根据前一时刻的系统状态，估计出当前的系统状态，然后以当前时刻的实际观测值作为反馈，来修正估计的状态。卡尔曼滤波算法可分为两个步骤，包括状态预测和测量更新。

假设系统离散状态空间模型为：

式中x(k)为k时刻系统的状态变量(此处可以为目标位置p_t和速度

)，u(k-1)为k-1时刻系统的控制变量，z(k)为k时刻传感器的测量变量(通过RANSAC等算法估计的目标位置)，w(k)为系统的过程噪声，v(k)为k时刻传感器的观测噪声，A和H分别为系统的状态转移矩阵和测量矩阵，B为系统的输入控制矩阵。

第二方面，本发明一实施例还提供了一种基于多目视觉的目标定位系统，该系统包括：

至少四个视觉测量单元，每个视觉测量单元包括至少一个相机和至少一个激光测距仪，用于采集包括目标区域的图像，以及测量目标到视觉测量单元的距离；

相对位置确定单元，用于确定视觉测量单元之间的相对位置关系；

相对位姿标定单元，用于标定视觉测量单元中相机与激光测距仪之间的相对位姿关系；

图像处理单元，用于确定目标在图像中的位置和目标在相机坐标系下的方向；

调整单元，用于调整激光测距仪，使激光测距仪对准目标；

目标位置计算单元，用于根据视觉测量单元获取的距离信息，计算目标的位置。

本发明一实施例中，相对位置确定单元可以为差分卫星定位系统或全站仪；相对位姿标定单元可以采用上述方法中步骤S31-34，步骤S3201-3210，以及步骤S3211-3222所述的过程来标定视觉测量单元中相机与激光测距仪之间的相对位姿关系；图像处理单元可以采用检测算法确定目标在图像中的位置和目标在相机坐标系下的方向；调整单元能够根据上述计算的激光测距仪的旋转轴和旋转角度调整激光测距仪，以使激光测距仪对准目标；目标位置计算单元能够采用上述方法中所述的求解方法计算目标的位置。

本发明一实施例提供的基于多目视觉的目标定位方法及系统能够实现户外大范围场景下的目标定位，硬件要求低、定位精度高，适用场景更广泛。

需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。此外，本文中“前”、“后”、“左”、“右”、“上”、“下”均以附图中表示的放置状态为参照。

最后应说明的是：以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

1.一种基于多目视觉的目标定位方法，其特征在于，包括：

在待测目标区域覆盖至少四个视觉测量单元，每个视觉测量单元包括至少一个相机和至少一个激光测距仪；

确定视觉测量单元之间的相对位置关系；

标定视觉测量单元中相机与激光测距仪之间的相对位姿关系；

利用相机采集包括目标区域的图像，确定目标在图像中的位置和目标在相机坐标系下的方向；

调整激光测距仪，使激光测距仪对准目标，测量目标到激光测距仪的距离；

利用至少四个视觉测量单元获取的距离信息，计算目标的位置；

标定视觉测量单元中相机与激光测距仪之间的相对位姿关系，包括步骤S31-S32：

S31，标定相机内参；

S32，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标；

当激光测距仪的激光光斑可见时，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标，包括步骤S3201-S3210：

S3201，设定激光测距仪光轴的三维向量为^Cn＝[n_x,n_y,n_z]^T，

激光测距仪原点在相机坐标系下的坐标为^Ct＝[t_x,t_y,t_z]^T；

S3202，利用激光测距仪测量相机坐标系下任意一点^Cp_i＝[^Cx_i,^Cy_i,^Cz_i]^T，并得到反馈距离l_i；

S3203，确定激光测距仪的测量斑点的图像坐标值w_i＝[u_i,v_i,1]^T；

S3204，利用标定的相机内参数K和畸变参数D得到图像坐标到相机坐标的投影方程P(K,D)，将图像坐标值w_i＝[u_i,v_i,1]^T转换到相机坐标系下为：k_i[x_i,y_i,1]^T＝P(K,D)w_i，x_i和y_i能够通过以上方程求解，k_i为未知参数；

S3205，根据k_i[x_i,y_i,1]^T＝^Ct+^Cn*l_i＝[^Cx_i,^Cy_i,^Cz_i]^T，确定求解方程组：

f_q1＝t_x+n_xl_i-x_it_z-x_in_zl_i＝0

f_q2＝t_y+n_yl_i-y_it_z-y_in_zl_i＝0

S3206，移动视觉测量单元，获取Q个不同的图像，Q≥3，基于获取的图像得到Q个方程组，Q个方程组包括2Q个方程；

S3207，基于方程组，利用奇异值求解法求解光轴向量^Cn＝[n_x,n_y,n_z]^T和原点坐标^Ct＝[t_x,t_y,t_z]^T的初始值；

S3208，构造^Cn的约束条件为：

S3209，构造优化函数为：

ρ表示惩罚因子；

S3210，根据^Cn和^Ct的初始值、约束条件和优化函数，通过最小二乘优化算法获取^Cn和^Ct的精确值，并对获取的^Cn进行归一化，以转换为单位向量；

当激光测距仪的激光光斑不可见时，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标，包括步骤S3211-S3222：

S3211，设定激光测距仪光轴的三维向量为^Cn＝[n_x,n_y,n_z]^T，

激光测距仪原点在相机坐标系下的坐标为^Ct＝[t_x,t_y,t_z]^T；

S3212，构建同时满足相机和激光测距仪的测量要求的平面标定板；

S3213，确定标定板坐标系与相机坐标系的相对位姿关系；

S3214，确定标定板平面方程：π:AX+BY+CZ+D＝0；

S3215，确定相机坐标系下标定板平面方程：A'x+B'y+C'z+D'＝0；

S3216，利用激光测距仪测量相机坐标系标定板上任意一点^Cp_i＝[^Cx_i,^Cy_i,^Cz_i]^T，并得到反馈距离l_i；

S3217，确定求解方程组：

f_p1＝^Cx_i-t_x-n_xl_i＝0

f_p2＝^Cy_i-t_y-n_yl_i

f_p3＝^Cz_i-t_z-n_zl_i

f_p4＝(A',B',C',D')[^Cx_i,^Cy_i,^Cz_i,1]^T＝0

S3218，移动标定板，且保证移动前后的标定板不平行，获取P个不同的图像，P≥3，基于获取的图像得到P个方程组，P个方程组包括4P个方程；

S3219，基于方程组，利用奇异值求解法求解光轴向量^Cn＝[n_x,n_y,n_z]^T和原点坐标^Ct＝[t_x,t_y,t_z]^T的初始值；

S3220，构造^Cn的约束条件为：

S3221，构造优化函数为：

ρ表示惩罚因子；

S3222，根据^Cn和^Ct的初始值、约束条件和优化函数，通过最小二乘优化算法获取^Cn和^Ct的精确值，并对获取的^Cn进行归一化，以转换为单位向量。

2.根据权利要求1所述的基于多目视觉的目标定位方法，其特征在于，视觉测量单元安装在可调节云台上。

3.根据权利要求1所述的基于多目视觉的目标定位方法，其特征在于，标定视觉测量单元中相机与激光测距仪之间的相对位姿关系，还包括步骤S33-S34：

S33，基于步骤S32的结果，对激光测距仪和相机的位置进行校正，以使激光测距仪原点在相机坐标系下的坐标值在设定范围内，使激光测距仪的光轴与相机光轴平行；

S34，确定相机坐标系下的激光测距仪的光轴向量和激光测距仪原点在相机坐标系下的坐标。

4.根据权利要求1所述的基于多目视觉的目标定位方法，其特征在于，采用YOLO目标检测算法确定目标在图像中的位置和目标在相机坐标系下的方向。

5.根据权利要求1所述的基于多目视觉的目标定位方法，其特征在于，调整激光测距仪以使激光测距仪对准目标时，激光测距仪的旋转轴和旋转角度利用以下公式确定；

rot_axis＝^Cn×^Cn_t

θ＝arccos(^Cn·^Cn_t)

rot_axis表示旋转轴，^Cn表示激光测距仪光轴向量，^Cn_t表示目标在相机坐标系下的方向向量，θ表示旋转角度。

6.根据权利要求1所述的基于多目视觉的目标定位方法，其特征在于，设定：共有k个视觉测量单元观测到目标，k个视觉测量单元中激光测距仪测量得到的目标距离分别表示为l_i，k个视觉测量单元在全局坐标系下的坐标分别为p_i＝(x_i,y_i,z_i)，i＝1,2,...,k，目标在全局坐标系下的坐标为p_t＝(x_t,y_t,z_t)；

目标的位置通过对方程

进行消元求解确定，i＝1,2,...,k。

7.一种使用如权利要求1-6中任一项所述的基于多目视觉的目标定位方法的基于多目视觉的目标定位系统，其特征在于，包括：

至少四个视觉测量单元，每个所述视觉测量单元包括至少一个相机和至少一个激光测距仪，用于采集包括目标区域的图像，以及测量目标到所述视觉测量单元的距离；

相对位置确定单元，用于确定所述视觉测量单元之间的相对位置关系；

相对位姿标定单元，用于标定所述视觉测量单元中相机与激光测距仪之间的相对位姿关系；

图像处理单元，用于确定目标在图像中的位置和目标在相机坐标系下的方向；

调整单元，用于调整激光测距仪，使激光测距仪对准目标；

目标位置计算单元，用于根据视觉测量单元获取的距离信息，计算目标的位置。

Images