CN111220143B - 一种成像设备的位置和姿态确定方法及装置 - Google Patents

Abstract

本发明提供了一种成像设备的位置和姿态确定方法及装置，涉及传感器的位姿测量技术领域。其中，成像设备固定设置于车辆的一侧；该方法包括：控制成像设备采集图像，确定图像中的成像设备的光心点坐标；识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据至少两条参数化线确定一消失点坐标；根据光心点坐标、消失点坐标和成像设备的焦距，确定成像设备的俯仰角。本发明可以实现一种成像设备的位置和姿态的便捷定位和姿态确定的方式。

Description

一种成像设备的位置和姿态确定方法及装置

技术领域

本发明涉及传感器的位姿测量技术领域，尤其涉及一种成像设备的位置和姿态确定方法及装置。

背景技术

目前，定位技术作为无人驾驶领域中的重要技术，得到了研究人员的普遍关注。在车辆行驶中，需要通过定位技术来确定每时每刻车辆所在的位置。当前通常的解决方案是使用全球卫星导航系统(Global Navigation Satellite System，简称GNSS)作为自身定位的依据。然而GNSS由于信号干扰等情况，在某些高精度场景中(例如港口等电磁环境复杂的场景，GNSS信号会受到较强干扰)，难以达到定位精度要求，故而需要其他传感器的辅助，例如摄像头，激光雷达等。当前，依靠摄像头和激光雷达等传感器来进行车辆定位的方式已经成为研究重点。

发明内容

本发明的实施例提供一种成像设备的位置和姿态确定方法及装置，以实现一种成像设备的位置和姿态的便捷定位和姿态确定的方式。

为达到上述目的，本发明采用如下技术方案：

一方面，本发明实施例提供一种成像设备的位置和姿态确定方法，所述成像设备固定设置于车辆的一侧；

所述方法包括：

控制所述成像设备采集图像，确定图像中的成像设备的光心点坐标；

识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据所述至少两条参数化线确定一消失点坐标；

根据所述光心点坐标、消失点坐标和成像设备的焦距，确定所述成像设备的俯仰角。

另一方面，本发明实施例还提供一种成像设备的位置和姿态确定装置，所述成像设备固定设置于车辆的一侧；

所述装置包括：

成像设备控制单元，用于控制所述成像设备采集图像；

图像识别单元，用于确定图像中的成像设备的光心点坐标，并识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据所述至少两条参数化线确定一消失点坐标；

偏离角度计算单元，用于根据所述光心点坐标、消失点坐标和成像设备的焦距，确定所述成像设备的俯仰角。

另一方面，本发明实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的成像设备的位置和姿态确定方法。

另一方面，本发明实施例还提供一种计算机设备，包括存储器、处理器及存储在存储上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述的成像设备的位置和姿态确定方法。

本发明实施例提供一种成像设备的位置和姿态确定方法及装置，首先控制成像设备采集图像，确定图像中的成像设备的光心点坐标；然后识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据所述至少两条参数化线确定一消失点坐标；最后可以根据光心点坐标、消失点坐标和成像设备的焦距，确定所述成像设备的俯仰角，从而能够便捷的确定成像设备的姿态。

附图说明

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

图1为本发明实施例提供的一种成像设备的位置和姿态确定方法的流程图一；

图2为本发明实施例提供的一种成像设备的位置和姿态确定方法的流程图二；

图3为本发明实施例中参数化线为曲线情况下的消失点偏差示意图；

图4为本发明实施例中的消失点坐标的确定方法的流程图；

图5为本发明实施例提供的一种成像设备的位置和姿态确定方法的流程图三；

图6为本发明实施例中参数化线为直线情况下的消失点偏差示意图；

图7为本发明实施例中绝对坐标系下的车道标志物的延伸线以及各标志点的位置示意图一；

图8为本发明实施例中绝对坐标系下的车道标志物的延伸线以及各标志点的位置示意图二；

图9为本发明实施例中图像平面上的车道标志物的延伸线以及各投影点的位置示意图一；

图10为本发明实施例中图像平面上的车道标志物的延伸线以及各投影点的位置示意图二；

图11为本发明实施例提供的一种成像设备的位置和姿态确定装置的结构示意图。

具体实施方式

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

为了便于理解本发明，下面对本发明所涉及的技术术语进行解释：

相机几何模型：将三维空间中的点(单位为米)，根据相机内参外参，映射到二维图像平面(单位为像素)的过程。

定位：根据传感器测量的信息和事先已知的信息，计算车辆或移动机器人在当前环境中的位置。

绝对坐标系：是指所有坐标全部基于一个固定的坐标系原点的位置的描述的坐标系。

消失点：三维空间中的平行线，投影的图像的二维平面上，会在无穷远处汇聚于一点，这个点被称为消失点。

高精度地图：高精度地图是用于自动驾驶的专题地图。高精度地图不同于传统的导航地图，其比导航地图具有更多的图层，用来表示更多的地图信息。例如车道模型(包括车道线、行驶方向等信息)、道路部件(包括可行驶区域、路肩、绿化带等信息)、交通设施(包括红绿灯、限行标志等信息)、行车限制(包括可变车道、车辆限行限号等信息)、以及动态特征(包括临时管控、道路维护保养等信息)等的矢量信息。由于高精度地图具有更多的地图信息，因此同样地理面积的高精度地图，数据量比传统的导航地图高出数倍。

位姿：Pose，位置和姿态的总称，包含6个自由度，其中包括3个位置自由度和3个朝向自由度。3个朝向自由度通常用俯仰角(pitch)、翻滚角(roll)、偏航角(yaw)来表示。

在实现本发明实施例的过程中，发明人发现当前的成像设备的定位方式如下：

将成像设备(此处以摄像头为例)安装于车辆上，并通过摄像头和高精度地图配合，从而完成定位，具体方式为在车体上安装向前拍摄的摄像头。在进行定位时，通过摄像头将高精度地图上的一些标志物(如车道线、广告牌等)，通过相机几何模型投影到摄像头的成像平面中，与预先识别的标志物结果做匹配优化，进而可以确定相机的位置。在进行优化的过程中，需要进行最小化重投影误差，进而需要构建优化问题，通过梯度下降等方法，来寻找使得损失函数最小的位姿。

而该当前方法存在如下问题：首先，当前方法需要高精度地图的配合，容易将高精度地图的误差引入到定位过程中，容易造成误差对定位产生影响。在进行优化问题求解时，需要占用较长时间，而对于需要高实时性的自动驾驶场景下，优化过程所占用的时间，会成为一个很明显的延迟。并且，在进行优化问题求解时，优化问题本身不一定是凸优化，难以找到最优解。

为了克服上述问题，如图1所示，本发明实施例提供一种成像设备的位置和姿态确定方法，其中，该成像设备固定设置于车辆的一侧(例如设置于车辆的车头的左侧或右侧，或者左右两侧均设置有成像设备，但不仅局限于此)，该成像设备的位置和姿态确定方法包括：

步骤101、控制成像设备采集图像，确定图像中的成像设备的光心点坐标。

一般情况下，图像中的成像设备的光心点是光轴与成像平面的交点。

步骤102、识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据至少两条参数化线确定一消失点坐标。

步骤103、根据光心点坐标、消失点坐标和成像设备的焦距，确定成像设备的俯仰角。

通过上述步骤101至步骤103可以采用几何关系来实现成像设备的俯仰角的确定，能够便捷地确定成像设备姿态中的俯仰角。

值得说明的是，本发明实施例中所涉及的车道标志物可以包括车道线、车道路肩、车道凸起路标(例如在车道线上，或者不在车道线上的一些线状分布的反光车道凸起路标)、车道两侧集装箱下沿或者车道两侧建筑物墙体下沿。

此处，需要说明的是，该步骤102中的参数化线可以为直线，也可以为曲线，下面分别以参数化线为曲线和参数化线为直线两种情况进行展开，以详细说明本发明的具体实施例。

(一)、参数化线为曲线：

如图2所示，本发明实施例提供的成像设备的位置和姿态确定方法，可以包括如下步骤：

步骤201、控制成像设备采集图像，确定图像中的成像设备的光心点坐标。

步骤202、识别图像中的两侧的车道标志物，提取车道标志物边缘，拟合得到图像中车道标志物边缘的至少两条参数化线。

其中，如图3所示，该参数化线为n次函数曲线：D_(n+2)ixⁿ+......+D_3ix+D_2iy+D_1i＝0,(n≥2,n∈N^*)，其中D_1i至D_(n+2)i为第i条参数化线的n次函数曲线参数。

步骤203、根据至少两条参数化线中的任两条参数化线的交点构成的集合，确定图像中的一消失点坐标。

例如，如图3所示，以两条参数化线为例，两条参数化线在图像中最终的交点X即为消失点，O为光心点。

此处，该步骤203可以采用如图4所示的步骤来确定消失点坐标：

步骤1、在交点构成的集合中确定各交点的坐标。

例如车道线存在一定宽度，则两条车道线就对应了4条车道线的边缘的参数化线，这4条车道线的边缘的参数化线两两相交，就存在6个交点。当然，若图像中存在更多的参数化线，则可以得到更多的交点。

步骤2、计算得到各交点的当前均值点坐标，并根据各交点的坐标和当前均值点坐标，计算当前各交点对应的标准偏差。

步骤3、保留满足标准偏差条件的交点。

其中，标准偏差条件为交点的坐标与当前均值点坐标的距离小于等于标准偏差的n倍；其中，n为预设倍数阈值。

重复执行步骤2至步骤3，判断当前均值点坐标与上一次均值点坐标的距离是否小于等于预先设置的距离阈值。

若当前均值点坐标与上一次均值点坐标的距离小于等于预先设置的距离阈值，则将当前均值点坐标作为图像中的消失点坐标。

若当前均值点坐标与上一次均值点坐标的距离大于预先设置的距离阈值，则返回执行步骤2。

步骤204、根据光心点坐标、消失点坐标和成像设备的焦距，确定成像设备的俯仰角。

此处，该步骤204可以采用如下方式：

如图3所示，根据光心点坐标(图3中的O点)和消失点坐标(图3中的X点)，确定图像中消失点坐标与光心点坐标的纵坐标偏差Δy。

根据成像设备的焦距、纵坐标偏差Δy，确定成像设备的俯仰角。

其中，俯仰角为

f为成像设备的焦距，即光心O′到图像上的光心点O的距离。

(二)、参数化线为直线：

如图5所示，本发明实施例提供的成像设备的位置和姿态确定方法，可以包括如下步骤：

步骤301、控制成像设备采集图像，确定图像中的成像设备的光心点坐标。

步骤302、识别图像中的两侧的车道标志物，提取车道标志物边缘，拟合得到图像中车道标志物边缘的至少两条参数化线。

其中，如图6所示，该参数化线为直线：A_ix+B_iy+C_i＝0，i∈N₊，其中，A_i、C_i和B_i为第i条参数化线的直线参数。

步骤303、根据至少两条参数化线中的任两条参数化线的交点构成的集合，确定图像中的一消失点坐标。

例如，如图6所示，以两条参数化线为例，两条参数化线在图像中最终的交点X即为消失点，O为光心点。

此处，该步骤303也可以采用如图4所示的步骤来确定消失点坐标：

步骤1、在交点构成的集合中确定各交点的坐标。

例如车道线存在一定宽度，则两条车道线就对应了4条车道线的边缘的参数化线，这4条车道线的边缘的参数化线两两相交，就存在6个交点。当然，若图像中存在更多的参数化线，则可以得到更多的交点。

步骤2、计算得到各交点的当前均值点坐标，并根据各交点的坐标和当前均值点坐标，计算当前各交点对应的标准偏差。

步骤3、保留满足标准偏差条件的交点。

其中，标准偏差条件为交点的坐标与当前均值点坐标的距离小于等于标准偏差的n倍；其中，n为预设倍数阈值。

重复执行步骤2至步骤3，判断当前均值点坐标与上一次均值点坐标的距离是否小于等于预先设置的距离阈值。

若当前均值点坐标与上一次均值点坐标的距离小于等于预先设置的距离阈值，则将当前均值点坐标作为图像中的消失点坐标。

若当前均值点坐标与上一次均值点坐标的距离大于预先设置的距离阈值，则返回执行步骤2。

步骤304、根据光心点坐标、消失点坐标和成像设备的焦距，确定成像设备的俯仰角和偏航角。

此处，该步骤304可以采用如下方式：

如图6所示，根据光心点坐标(图6中的O点)和消失点坐标(图6中的X点)，确定图像中消失点坐标与光心点坐标的纵坐标偏差Δy和横坐标偏差Δx。

根据成像设备的焦距、纵坐标偏差Δy，确定成像设备的俯仰角，以及根据成像设备的焦距、横坐标偏差Δx，确定成像设备的偏航角。

其中，俯仰角为

f为成像设备的焦距，即光心O′到图像上的光心点O的距离。偏航角为

当然，在步骤304中，若光轴和车道标志物的延伸线平行，则光心点和消失点在图像上重合，则俯仰角和偏航角均为0。

步骤305、在成像设备所在的绝对坐标系下确定在车辆行驶方向的成像设备前方的一第一标志点，及车道标志物的延伸线上的第二标志点和第三标志点。

其中，如图7和图8所示，在绝对坐标系下，第一标志点(以C表示)与成像设备所在位置点(以D表示)构成的直线与车道标志物的延伸线(即图7和图8的两侧的实线)平行；第一标志点C与第二标志点(以B表示)构成的直线与车道标志物的延伸线垂直。

此外，第二标志点和第三标志点(以A表示)位于车辆行驶方向的左侧车道标志物的延伸线上(如图7所示)或者位于车辆行驶方向的右侧车道标志物的延伸线上(如图8所示)；在绝对坐标系下，左侧车道标志物的延伸线和右侧车道标志物的延伸线平行。

该步骤305可以采用如下方式实现：

如图7和图8所示，在成像设备所在的绝对坐标系下确定在车辆行驶方向的成像设备前方的一第一标志点C，及车道标志物的延伸线上的第二标志点B和第三标志点A，并根据待求解的相对距离d、预先获知的两侧的车道标志物的延伸线的中线距离一侧车道标志物的延伸线的距离w，以及预先设置的坐标值表示第一标志点、第二标志点和第三标志点在绝对坐标系的坐标。例如如图7所示，以D为(0,0,0)，C为D前方10米的点，则C为(0,0,10)，则B为(-w-d，0,10)，A为B前方10米的点，则A为(-w-d，0，20)，此处仅为一种坐标的实例，不仅仅局限于此，其他坐标的实例在此处不一一进行列举。

步骤306、根据偏航角和俯仰角，确定第一标志点、第二标志点和第三标志点在图像中分别对应的第一投影点、第二投影点和第三投影点；第一投影点、第二投影点和第三投影点构成一三角形；获得第二投影点处该三角形的角的三角函数向量关系。

该步骤306可以采用如下方式实现：

根据偏航角、俯仰角以及预先确定的稳定翻滚角确定由绝对坐标系投影到相机坐标系的旋转矩阵R。一般情况下，车辆在水平路面上行驶，其翻滚角不会发生变化，因此此处翻滚角可以认为是稳定的。而旋转矩阵R是通过偏航角、俯仰角以及横滚角确定的，由于旋转矩阵R的表示形式为现有技术，此处不再赘述。

根据旋转矩阵R将第一标志点、第二标志点和第三标志点进行转化到相机坐标系下，分别得到第一相机坐标系标志点、第二相机坐标系标志点、第三相机坐标系标志点。其中，A_cam＝RA,B_cam＝RB,C_cam＝RC；A、B、C分别表示第三标志点在绝对坐标系的坐标、第二标志点在绝对坐标系的坐标和第一标志点在绝对坐标系的坐标；A_cam、B_cam、C_cam分别表示第三相机坐标系标志点的坐标、第二相机坐标系标志点的坐标和第一相机坐标系标志点的坐标；A_cam＝(A_x，A_y，A_z)，其中A_x、A_y、A_z为相机坐标系下A_cam在三个轴的坐标值，B_cam＝(B_x，B_y，B_z)，其中B_x、B_y、B_z为相机坐标系下B_cam在三个轴的坐标值，C_cam＝(C_x，C_y，C_z)，其中C_x、C_y、C_z为相机坐标系下C_cam在三个轴的坐标值。

将第一相机坐标系标志点、第二相机坐标系标志点、第三相机坐标系标志点分别投影到归一化成像平面上，得到第一归一化标志点、第二归一化标志点、第三归一化标志点；其中，

A_c′_am、B_c′_am、C_c′_am分别表示第三归一化标志点的坐标、第二归一化标志点的坐标、第一归一化标志点的坐标。

根据预先确定的相机内参矩阵K(该相机内参矩阵K可在架设成像设备后预先标定得到，此处不再赘述)将第一归一化标志点、第二归一化标志点、第三归一化标志点投影到图像平面上，分别得到第一投影点、第二投影点和第三投影点；其中，A′＝KA_c′_am，B′＝KB_c′_am，C′＝KC_c′_am；A′、B′和C′分别为第三投影点、第二投影点和第一投影点在图像平面上的坐标，如图9所示的A′、B′和C′分别为第三投影点、第二投影点和第一投影点在图像平面上的坐标，对应于图7中的A、B、C三个点；而如图10所示的A′、B′和C′分别为第三投影点、第二投影点和第一投影点在图像平面上的坐标，对应于图8中的A、B、C三个点。图9和图10中最外侧的矩形表示图像平面的边缘。

如图9和图10所示，进一步可以根据向量

和

确定第二投影点B′处三角形的角γ的余弦函数：

步骤307、根据三角函数向量关系，求解得到绝对坐标系下成像设备所在位置点到两侧的车道标志物的延伸线的中线的相对距离。

此处，该步骤307可以采用如下方式实现：

在图像平面上确定角γ的大小，并计算得到角γ的余弦值cosγ。

根据余弦函数：

确定cosγ与待求解的相对距离d的单调函数关系；其中，待求解的相对距离d的取值范围为[-w，w]。此处，由于

可以表示为cosγ与R、K、w和d的函数关系，而R、K和w为已知量，因此能够确定cosγ与待求解的相对距离d的函数关系。

根据单调函数关系，采用二分法不断在待求解的相对距离d的取值范围内取中值，将中值代入单调函数关系得到余弦结果，与cosγ进行比较，并根据比较结果更新待求解的相对距离d的取值范围，直至比较结果小于等于预先设置的阈值，得到相对距离d。

以单调递增函数为例，即若cosγ的值随着相对距离d的增大而增大，则不断在待求解的相对距离d的取值范围内取中值，将中值代入单调函数关系得到余弦结果，若余弦结果小于cosγ，则在当前取值范围内选择取值范围较大的一半，若余弦结果大于cosγ，则在当前取值范围内选择取值范围较小的一半，根据选择后的取值范围继续进行取中值，直至余弦结果与cosγ比较结果小于等于预先设置的阈值，从而得到相对距离d。

以单调递减函数为例，即若cosγ的值随着相对距离d的增大而减小，则不断在待求解的相对距离d的取值范围内取中值，将中值代入单调函数关系得到余弦结果，若余弦结果小于cosγ，则在当前取值范围内选择取值范围较小的一半，若余弦结果大于cosγ，则在当前取值范围内选择取值范围较大的一半，根据选择后的取值范围继续进行取中值，直至余弦结果与cosγ比较结果小于等于预先设置的阈值，从而得到相对距离d。

另一方面，如图11所示，本发明实施例还提供一种成像设备的位置和姿态确定装置，成像设备固定设置于车辆的一侧。

该成像设备的位置和姿态确定装置包括：

成像设备控制单元41，用于控制成像设备采集图像。

图像识别单元42，用于确定图像中的成像设备的光心点坐标，并识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据至少两条参数化线确定一消失点坐标。

偏离角度计算单元43，用于根据光心点坐标、消失点坐标和成像设备的焦距，确定成像设备的俯仰角。

另一方面，本发明实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的成像设备的位置和姿态确定方法。

另一方面，本发明实施例还提供一种计算机设备，包括存储器、处理器及存储在存储上并可在处理器上运行的计算机程序，处理器执行程序时实现上述的成像设备的位置和姿态确定方法。

本发明可以应用于无人驾驶领域中的内集卡和外集卡中，以便于对内集卡和外集卡的成像设备的位置和姿态进行确定。当然，本发明还可以应用于乘用车以及机器人等成像设备的位置和姿态的确定。

可见，本发明实施例提供一种成像设备的位置和姿态确定方法及装置，首先控制成像设备采集图像，确定图像中的成像设备的光心点坐标；然后识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据至少两条参数化线确定一消失点坐标；最后可以根据光心点坐标、消失点坐标和成像设备的焦距，确定成像设备的俯仰角，从而能够便捷的确定成像设备的姿态。并且，进一步的，在参数化线为直线时，还可以继续确定偏航角和成像设备的位置，而整个方案仅仅是通过简单的几何关系计算，不需要高精度地图的配合，也不存在复杂的优化问题求解。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (15)

1.一种成像设备的位置和姿态确定方法，其特征在于，所述成像设备固定设置于车辆的一侧；

所述方法包括：

控制所述成像设备采集图像，确定图像中的成像设备的光心点坐标；

识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据所述至少两条参数化线确定一消失点坐标；

根据所述光心点坐标、消失点坐标和成像设备的焦距，确定所述成像设备的俯仰角；

其中，根据所述至少两条参数化线确定一消失点坐标包括：

步骤1、确定所述至少两条参数化线中的任两条参数化线的交点的坐标；

步骤2、计算得到各交点的当前均值点坐标，根据各交点的坐标和当前均值点坐标，计算当前各交点对应的标准偏差，并保留满足标准偏差条件的交点；所述标准偏差条件为交点的坐标与当前均值点坐标的距离小于等于所述标准偏差的n倍；其中，n为预设倍数阈值；

重复执行步骤2，并当当前均值点坐标与上一次均值点坐标的距离小于等于预先设置的距离阈值时，确定当前均值点坐标为图像中的消失点坐标。

2.根据权利要求1所述的方法，其特征在于，所述车道标志物包括车道线、车道路肩、车道凸起路标、车道两侧集装箱下沿或者车道两侧建筑物墙体下沿。

3.根据权利要求1所述的方法，其特征在于，所述识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据所述至少两条参数化线确定一消失点坐标，包括：

识别图像中的两侧的车道标志物，提取车道标志物边缘，拟合得到图像中车道标志物边缘的至少两条参数化线；所述参数化线为n次函数曲线：D_(n+2)ixⁿ+......+D_3ix+D_2iy+D_1i＝0,(n≥2,n∈N^*)，其中D_1i至D_(n+2)i为第i条参数化线的n次函数曲线参数；

根据所述至少两条参数化线中的任两条参数化线的交点构成的集合，确定图像中的一消失点坐标。

4.根据权利要求1所述的方法，其特征在于，所述识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据所述至少两条参数化线确定一消失点坐标，包括：

识别图像中的两侧的车道标志物，提取车道标志物边缘，拟合得到图像中车道标志物边缘的至少两条参数化线；所述参数化线为直线：A_ix+B_iy+C_i＝0，i∈N₊，其中，A_i、C_i和B_i为第i条参数化线的直线参数；

根据所述至少两条参数化线中的任两条参数化线的交点构成的集合，确定图像中的一消失点坐标。

5.根据权利要求3或4所述的方法，其特征在于，

所述步骤1包括：在所述交点构成的集合中确定各交点的坐标；

所述重复执行步骤2，并当当前均值点坐标与上一次均值点坐标的距离小于等于预先设置的距离阈值时，确定当前均值点坐标为图像中的消失点坐标，包括：

重复执行步骤2，判断当前均值点坐标与上一次均值点坐标的距离是否小于等于预先设置的距离阈值；

若当前均值点坐标与上一次均值点坐标的距离小于等于预先设置的距离阈值，则将当前均值点坐标作为图像中的消失点坐标；

若当前均值点坐标与上一次均值点坐标的距离大于预先设置的距离阈值，则返回执行步骤2。

6.根据权利要求1所述的方法，其特征在于，所述根据所述光心点坐标、消失点坐标和成像设备的焦距，确定所述成像设备的俯仰角，包括：

根据所述光心点坐标和消失点坐标，确定图像中消失点坐标与光心点坐标的纵坐标偏差Δy；

根据成像设备的焦距、所述纵坐标偏差Δy，确定所述成像设备的俯仰角；其中，所述俯仰角为

f为所述成像设备的焦距。

7.根据权利要求4所述的方法，其特征在于，还包括：

根据所述光心点坐标、消失点坐标和成像设备的焦距，确定所述成像设备的偏航角；

在成像设备所在的绝对坐标系下确定在车辆行驶方向的成像设备前方的一第一标志点，及车道标志物的延伸线上的第二标志点和第三标志点；所述第一标志点与成像设备所在位置点构成的直线与所述车道标志物的延伸线平行；所述第一标志点与第二标志点构成的直线与所述车道标志物的延伸线垂直；

根据所述偏航角和俯仰角，确定第一标志点、第二标志点和第三标志点在图像中分别对应的第一投影点、第二投影点和第三投影点；所述第一投影点、第二投影点和第三投影点构成一三角形；获得第二投影点处该三角形的角的三角函数向量关系；

根据所述三角函数向量关系，求解得到绝对坐标系下成像设备所在位置点到两侧的车道标志物的延伸线的中线的相对距离。

8.根据权利要求7所述的方法，其特征在于，所述根据所述光心点坐标、消失点坐标和成像设备的焦距，确定所述成像设备的偏航角，包括：

根据所述光心点坐标和消失点坐标，确定图像中消失点坐标与光心点坐标的横坐标偏差Δx；

根据成像设备的焦距、所述横坐标偏差Δx，确定所述成像设备的偏航角；其中，所述偏航角为

f为所述成像设备的焦距。

9.根据权利要求7所述的方法，其特征在于，所述第二标志点和第三标志点位于车辆行驶方向的左侧车道标志物的延伸线上或者位于车辆行驶方向的右侧车道标志物的延伸线上；所述左侧车道标志物的延伸线和右侧车道标志物的延伸线平行；

所述在成像设备所在的绝对坐标系下确定在车辆行驶方向的成像设备前方的一第一标志点，及车道标志物的延伸线上的第二标志点和第三标志点，包括：

在成像设备所在的绝对坐标系下确定在车辆行驶方向的成像设备前方的一第一标志点，及车道标志物的延伸线上的第二标志点和第三标志点，并根据待求解的相对距离d、预先获知的两侧的车道标志物的延伸线的中线距离一侧车道标志物的延伸线的距离w，以及预先设置的坐标值表示第一标志点、第二标志点和第三标志点在绝对坐标系的坐标。

10.根据权利要求9所述的方法，其特征在于，所述根据所述偏航角和俯仰角，确定第一标志点、第二标志点和第三标志点在图像中分别对应的第一投影点、第二投影点和第三投影点，包括：

根据所述偏航角、俯仰角以及预先确定的稳定翻滚角确定由绝对坐标系投影到相机坐标系的旋转矩阵R；

根据所述旋转矩阵R将第一标志点、第二标志点和第三标志点进行转化到相机坐标系下，分别得到第一相机坐标系标志点、第二相机坐标系标志点、第三相机坐标系标志点；其中，A_cam＝RA,B_cam＝RB,C_cam＝RC；A、B、C分别表示第三标志点在绝对坐标系的坐标、第二标志点在绝对坐标系的坐标和第一标志点在绝对坐标系的坐标；A_cam、B_cam、C_cam分别表示第三相机坐标系标志点的坐标、第二相机坐标系标志点的坐标和第一相机坐标系标志点的坐标；A_cam＝(A_x，A_y，A_z)，其中A_x、A_y、A_z为相机坐标系下A_cam在三个轴的坐标值，B_cam＝(B_x，B_y，B_z)，其中B_x、B_y、B_z为相机坐标系下B_cam在三个轴的坐标值，C_cam＝(C_x，C_y，C_z)，其中C_x、C_y、C_z为相机坐标系下C_cam在三个轴的坐标值；

将所述第一相机坐标系标志点、第二相机坐标系标志点、第三相机坐标系标志点分别投影到归一化成像平面上，得到第一归一化标志点、第二归一化标志点、第三归一化标志点；其中，

A′_cam、B′_cam、C′_cam分别表示第三归一化标志点的坐标、第二归一化标志点的坐标、第一归一化标志点的坐标；

根据预先确定的相机内参矩阵K将第一归一化标志点、第二归一化标志点、第三归一化标志点投影到图像平面上，分别得到第一投影点、第二投影点和第三投影点；其中，A′＝KA′_cam，B′＝KB′_cam，C′＝KC′_cam；A′、B′和C′分别为第三投影点、第二投影点和第一投影点在图像平面上的坐标。

11.根据权利要求10所述的方法，其特征在于，所述获得第二投影点处该三角形的角的三角函数向量关系，包括：

根据向量

和

确定第二投影点B′处三角形的角γ的余弦函数：

12.根据权利要求11所述的方法，其特征在于，所述根据所述三角函数向量关系，求解得到绝对坐标系下成像设备所在位置点到两侧的车道标志物的延伸线的中线的相对距离，包括：

在所述图像平面上确定角γ的大小，并计算得到角γ的余弦值cosγ；

根据所述余弦函数：

确定cosγ与待求解的相对距离d的单调函数关系；其中，待求解的相对距离d的取值范围为[-w，w]；

根据所述单调函数关系，采用二分法不断在待求解的相对距离d的取值范围内取中值，将中值代入所述单调函数关系得到余弦结果，与所述cosγ进行比较，并根据比较结果更新待求解的相对距离d的取值范围，直至所述比较结果小于等于预先设置的阈值，得到所述相对距离d。

13.一种成像设备的位置和姿态确定装置，其特征在于，所述成像设备固定设置于车辆的一侧；

所述装置包括：

成像设备控制单元，用于控制所述成像设备采集图像；

图像识别单元，用于确定图像中的成像设备的光心点坐标，并识别图像中的两侧的车道标志物，得到图像中车道标志物边缘的至少两条参数化线，并根据所述至少两条参数化线确定一消失点坐标；

偏离角度计算单元，用于根据所述光心点坐标、消失点坐标和成像设备的焦距，确定所述成像设备的俯仰角；

其中，所述图像识别单元进一步用于：

步骤1、确定所述至少两条参数化线中的任两条参数化线的交点的坐标；

步骤2、计算得到各交点的当前均值点坐标，根据各交点的坐标和当前均值点坐标，计算当前各交点对应的标准偏差，并保留满足标准偏差条件的交点；所述标准偏差条件为交点的坐标与当前均值点坐标的距离小于等于所述标准偏差的n倍；其中，n为预设倍数阈值；

重复执行步骤2，并当当前均值点坐标与上一次均值点坐标的距离小于等于预先设置的距离阈值时，确定当前均值点坐标为图像中的消失点坐标。

14.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1至12任一项所述的成像设备的位置和姿态确定方法。

15.一种计算机设备，包括存储器、处理器及存储在存储上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1至12任一项所述的成像设备的位置和姿态确定方法。

Images