CN115690205B - 基于点线综合特征的视觉相对位姿测量误差估计方法 - Google Patents

Abstract

本发明提供了一种基于点线综合特征的视觉相对位姿测量误差估计方法，该基于点线综合特征的视觉相对位姿测量误差估计方法包括：S1、建立视觉相对位姿测量模型，包括建立基于点特征的视觉相对位姿测量模型和基于线特征的视觉相对位姿测量模型；S2、对视觉相对位姿测量模型进行误差影响参数分析，包括相机内参数的影响分析和特征提取像素误差的影响分析；S3、对视觉相对位姿测量模型进行误差仿真分析以完成视觉相对位姿测量误差估计。应用本发明的技术方案，能够解决无人机自主起降过程中视觉相对位姿测量精度易受目标特征提取精度和相机内参数标定误差影响的技术问题。

Description

基于点线综合特征的视觉相对位姿测量误差估计方法

技术领域

本发明涉及无人机技术领域，尤其涉及一种基于点线综合特征的视觉相对位姿测量误差估计方法。

背景技术

为满足无人机自主起降相对位姿测量精度的要求，解决无人机起降过程中相对位姿测量精度易受视觉传感器位置和安装方式、视觉传感器参数、飞行环境及光照条件等因素影响的问题，必须摸清视觉传感器关键参数标定误差以及点线视觉特征提取精度和配置方案对相对位姿测量精度的影响规律，建立准确的误差模型，为实现实时、可靠、精确的无人机自主起降奠定理论基础。

视觉测量系统的精度分析与误差分析主要集中在工业检测和控制领域的应用，暂时未见有在飞行器自主着陆导航中应用的报道。目前，视觉测量误差机理研究思路主要还是通过建立视觉测量模型，并通过分析测量模型中相关参数对测量误差的影响，确定模型参数与测量误差之间的关系，从而最终建立误差机理模型。例如，2007年《摄像机标定参数误差对位姿测量精度的影响》分析了摄像机标定参数误差与位姿测量精度之间的关系，通过实验表明测量距离方向位置精度主要受焦比误差和光轴方向平移量误差的影响，姿态角精度主要受主点误差和外参旋转角误差的影响，该结论对视觉测量系统的工程应用具有一定的指导意义。2010年《单目视觉检测系统的误差分析》利用小孔成像模型重点分析了特征提取算法、光照强度、光照方向、镜头焦距、摄像头分辨率、曝光时间等因素对视觉测量误差的影响，较为全面的研究了小孔成像模型的误差。2014年《基于等腰三角形模型的单目视觉位姿测量系统误差分析》开始针对P3P模型展开误差分析，通过仿真实验认为增加不同类型的噪声从而获得每个摄像机输入参数误差对位姿测量误差的影响，但并未给出P3P模型完整的误差模型。

发明内容

本发明旨在至少解决现有技术中存在的技术问题之一。

本发明提供了一种基于点线综合特征的视觉相对位姿测量误差估计方法，该基于点线综合特征的视觉相对位姿测量误差估计方法包括：S1、建立视觉相对位姿测量模型，包括建立基于点特征的视觉相对位姿测量模型和基于线特征的视觉相对位姿测量模型；S2、对视觉相对位姿测量模型进行误差影响参数分析，包括相机内参数的影响分析和特征提取像素误差的影响分析；S3、对视觉相对位姿测量模型进行误差仿真分析以完成视觉相对位姿测量误差估计。

进一步地，基于点特征的视觉相对位姿测量模型为其中，/>是表示从世界坐标系到相机坐标系的旋转矩阵，/>是相机坐标系原点在世界坐标系下的坐标，/>为空间点P_i对应的世界坐标系中的齐次坐标，/>为空间点P_i从世界坐标系中的齐次坐标/>投影到图像坐标系中的齐次坐标，K为相机内参数矩阵，/>f为相机镜头焦距，s_x和s_y是图像水平和垂直方向的像元尺寸，[c_x，c_y]^T是图像的主点坐标；函数F₁和F₂的具体表达形式根据实际应用场景中的点特征对应的相机内参数、世界坐标系中的齐次坐标和图像坐标系中的齐次坐标确定。

进一步地，基于线特征的视觉相对位姿测量模型为其中，/>和分别表示基于线特征求解的旋转矩阵和平移向量；L为跑道特征线在世界坐标系中的三维坐标，/>m为直线的矩，/>为直线的方向向量；l为L对应图像坐标系中的像素坐标；函数F₃和F₄的具体表达形式根据实际应用场景中的线特征对应的相机内参数、世界坐标系中的三维坐标和图像坐标系中的像素坐标获取。

进一步地，基于线特征求解的旋转矩阵 C_θ、C_γ、/>分别表示对应俯仰角、横滚角和偏航角的旋转矩阵，θ、γ、/>分别表示俯仰角、横滚角、偏航角。

进一步地，根据分析相机内参数的影响，其中，j＝1,2,3，k₁表示镜头焦距与像元尺寸之间的比值f/s_x，k₂表示镜头焦距与像元尺寸之间的比值f/s_y，k₃表示图像的主点坐标[c_x，c_y]^T。

进一步地，根据 分析特征提取像素误差的影响，其中，/>和/>分别表示由第i个特征点行坐标像素提取误差Δx_i引起的姿态误差和位置误差，/>和/>分别表示由第i个特征点列坐标像素提取误差Δy_i引起的姿态误差和位置误差，/>和/>分别表示由图像坐标系内第i条直线斜率变化Δk_i所引起的姿态误差和位置误差，/>和/>分别表示由图像坐标系内第i条直线截距Δb_i所引起的姿态误差和位置误差。

进一步地，利用Halcon软件对视觉相对位姿测量模型进行误差仿真分析。

进一步地，利用Halcon软件对视觉相对位姿测量模型进行误差仿真分析具体包括：S31、根据测量场景中的点特征和线特征分布，利用Halcon软件模拟应用场景，通过设定相机内外参数，生成场景成像真值模板；S32、通过Halcon软件编程，实现基于点线特征的视觉相对位姿测量；S33、通过Halcon代码分别给相机内参数、真值模板的特征点和特征线添加误差扰动，以此分析相机内参数标定误差和特征提取误差对视觉相对位姿测量误差的影响。

进一步地，相机外参数包括相机位置。

应用本发明的技术方案，提供了一种基于点线综合特征的视觉相对位姿测量误差估计方法，该基于点线综合特征的视觉相对位姿测量误差估计方法通过建立视觉相对位姿测量模型，进行相机内参数和特征提取像素误差的影响分析，并进行误差仿真分析，掌握了视觉相对位姿测量误差的影响规律，完成了视觉相对位姿测量误差估计。本发明的技术方案能够解决无人机自主起降过程中视觉相对位姿测量精度易受目标特征提取精度和相机内参数标定误差影响的技术问题。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了根据本发明的具体实施例提供的基于点线综合特征的视觉相对位姿测量误差估计方法的流程示意图；

图2示出了根据本发明的具体实施例提供的机场跑道场景成像真值模板的示意图；

图3示出了根据本发明的具体实施例提供的Halcon软件编程中测量值的输出示意图；

图4示出了本发明的坐标系示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

如图1所示，根据本发明的具体实施例提供了一种基于点线综合特征的视觉相对位姿测量误差估计方法，该基于点线综合特征的视觉相对位姿测量误差估计方法包括：S1、建立视觉相对位姿测量模型，包括建立基于点特征的视觉相对位姿测量模型和基于线特征的视觉相对位姿测量模型；S2、对视觉相对位姿测量模型进行误差影响参数分析，包括相机内参数的影响分析和特征提取像素误差的影响分析；S3、对视觉相对位姿测量模型进行误差仿真分析以完成视觉相对位姿测量误差估计。

应用此种配置方式，提供了一种基于点线综合特征的视觉相对位姿测量误差估计方法，该基于点线综合特征的视觉相对位姿测量误差估计方法通过建立视觉相对位姿测量模型，进行相机内参数和特征提取像素误差的影响分析，并进行误差仿真分析，掌握了视觉相对位姿测量误差的影响规律，完成了视觉相对位姿测量误差估计。本发明的技术方案能够解决无人机自主起降过程中视觉相对位姿测量精度易受目标特征提取精度和相机内参数标定误差影响的技术问题。

进一步地，在本发明中，为了实现基于点线综合特征的视觉相对位姿测量误差估计，首先，建立视觉相对位姿测量模型。

根据小孔成像原理，空间点P_i从世界坐标系中的齐次坐标投影到图像坐标系中的齐次坐标/>的过程可表示为：/>其中，/>为空间点P_i在世界坐标系中的齐次坐标，/>x_wi、y_wi和z_wi为空间点P_i对应的世界坐标系中的x、y和z轴的坐标；/>为/>投影到图像坐标系中的齐次坐标，/>x_i与y_i分别为世界坐标系中三维点P_i在图像坐标系中的列像素坐标与行像素坐标；/>z_c是P_i点在相机坐标系中z坐标分量；K为相机内参数矩阵，/>f为相机镜头焦距，s_x和s_y是图像水平和垂直方向的像元尺寸，[c_x，c_y]^T是图像的主点坐标；/>是表示从世界坐标系到相机坐标系的旋转矩阵，/>为/>的第p行第q列的取值，p＝1,2,3，q＝1,2,3；/>是相机坐标系原点在世界坐标系下的坐标，和/>分别为相机坐标系原点在世界坐标系x、y和z轴的坐标；I为单位矩阵。

基于点特征的视觉相对位姿测量的本质是通过一组已知的“点对应”(即世界坐标系中的三维空间点坐标与图像坐标系中的像点)求解相机的位姿，上述方法被称为Perspective-n-Point(PnP)算法。

由此，基于点特征的视觉相对位姿测量模型可表示为：其中，函数F₁和F₂的具体表达形式根据实际应用场景中的点特征对应的相机内参数、世界坐标系中的齐次坐标和图像坐标系中的齐次坐标确定。

此外，在摄像机映射T的作用下，用Plücker矩阵定义的直线L表示图像坐标系下对应直线的像l:

上式可简化为其中，L为跑道特征线在世界坐标系中的三维坐标，m为直线的矩，/>为直线的方向向量；l为L对应图像坐标系中的像素坐标；K^*表示相机内参数矩阵的余因子矩阵，K^*＝|K|K^-T。

通过上述关系式可获得飞行器着陆过程中，世界坐标系中的跑道边线，中线、起始线与图像坐标系中的像之间的“线对应”关系，并利用消影点、消影线以及直线之间的平行以及垂直关系确定飞行器位姿解算方程组为：其中，α、β为常值系数，p_∞、l_∞、n_π分别为消影点、消影线以及平面的法向量；/>和/>分别表示基于线特征求解的旋转矩阵和平移向量；/> C_θ、C_γ、/>分别表示对应俯仰角、横滚角和偏航角的旋转矩阵，θ、γ、/>分别表示俯仰角、横滚角、偏航角。

由此，基于线特征的视觉相对位姿测量模型可表示为：其中，L为跑道特征线在世界坐标系中的三维坐标，/>m为直线的矩，/>为直线的方向向量；l为L对应图像坐标系中的像素坐标；函数F₃和F₄的具体表达形式根据实际应用场景中的线特征对应的相机内参数、世界坐标系中的三维坐标和图像坐标系中的像素坐标获取。

进一步地，在建立视觉相对位姿测量模型后，对视觉相对位姿测量模型进行误差影响参数分析。

由上述基于点特征以及线特征的视觉相对位姿测量模型可知，测量模型的输入参数包括跑道特征点及特征线在世界坐标系中的三维坐标P_wi、L，对应图像坐标系中的像素坐标P_ii、l，相机内参数矩阵K包括相机镜头焦距f，水平及垂直方向的像元尺寸s_x、s_y，和图像的主点坐标[c_x，c_y]^T。另外，合作目标的点线配置也会影响视觉相对位姿测量的误差。本发明中所有推导均忽略相机镜头的畸变影响。

首先，进行相机内参数的影响分析：

相机内参数主要是指镜头焦距与像元尺寸之间的比值f/s_x和f/s_y，以及图像的主点坐标[c_x，c_y]^T，这些参数均可通过摄像机标定获取，因此相机内参数的标定误差势必会影响基于线特征的视觉相对位姿测量的精度。

由此，根据前述获取的基于线特征的视觉相对位姿测量模型可分析获取相机内参数的影响为：其中，j＝1,2,3，k₁表示镜头焦距与像元尺寸之间的比值f/s_x，k₂表示镜头焦距与像元尺寸之间的比值f/s_y，k₃表示图像的主点坐标[c_x，c_y]^T。

其次，进行特征提取像素误差的影响分析：

根据前述视觉相对位姿测量模型可知，飞行器固定机场着陆过程中的目标特征坐标包括跑道特征点及特征线在世界坐标系中的三维空间坐标P_wi、L，和对应图像坐标系中的像素坐标P_ii、l，其中P_wi和L均需事先人为通过高精度测量仪器测量获取，主要涉及跑道端点的坐标和跑道长度以及跑道宽度；而图像坐标系中特征的像素坐标P_ii和l主要通过目标识别与特征提取算法获取，特征点提取精度用对应像素坐标的像素误差表示，而特征线的提取精度主要用直线l:y_i＝kx_i+b的斜率k以及截距b的提取精度表示。

由此，根据前述获取的基于点特征和线特征的视觉相对位姿测量模型可分析获取特征提取像素误差的影响为： 其中，和/>分别表示由第i个特征点行坐标像素提取误差Δx_i引起的姿态误差和位置误差，/>和/>分别表示由第i个特征点列坐标像素提取误差Δy_i引起的姿态误差和位置误差，/>和/>分别表示由图像坐标系内第i条直线斜率变化Δk_i所引起的姿态误差和位置误差，/>和/>分别表示由图像坐标系内第i条直线截距Δb_i所引起的姿态误差和位置误差。

进一步地，在本发明中，在对视觉相对位姿测量模型进行误差影响参数分析后，对视觉相对位姿测量模型进行误差仿真分析。

作为本发明的一个具体实施例，可利用Halcon软件对视觉相对位姿测量模型进行误差仿真分析。

具体地，S31，首先根据测量场景中的点特征和线特征分布，利用Halcon软件模拟应用场景，通过设定相机内外参数，生成场景成像真值模板；S32，然后通过Halcon软件编程，实现基于点线特征的视觉相对位姿测量；S33，最后通过Halcon代码分别给相机内参数、真值模板的特征点和特征线添加误差扰动，以此分析相机内参数标定误差和特征提取误差对视觉相对位姿测量误差的影响。其中，相机内参数包括相机焦距和分辨率等，相机外参数包括相机位置等。

本发明的基于点线综合特征的视觉相对位姿测量误差估计方法，通过理论推导建立了视觉相对位姿测量误差模型，掌握了视觉传感器关键参数标定误差以及点线特征提取误差对相对位姿测量精度的影响规律，有利于进一步提升无人机自主起降过程中视觉相对位姿测量的精度，为实现实时、可靠和精确的无人机自主起降提供指导规范。

为了对本发明有进一步地了解，下面结合图1至图3对本发明的基于点线综合特征的视觉相对位姿测量误差估计方法进行详细说明。

如图1至图3所示，根据本发明的具体实施例提供了一种基于点线综合特征的视觉相对位姿测量误差估计方法，具体包括以下步骤。

S1，建立视觉相对位姿测量模型。

根据实际应用场景中的点线特征形式，可分别选择基于点特征和线特征的视觉相对位姿测量方法，并建立该场景下视觉相对位姿测量的基本模型。一般测量模型的输入参数包括特征点及特征线在世界坐标系中的三维坐标P_wi、L，对应图像坐标系中的像素坐标P_ii、l，相机内参数矩阵K包括相机镜头焦距f，垂直及水平方向的像元尺寸s_x和s_y，主点坐标[c_x，c_y]^T。另外，还考虑到合作目标的点线配置形式。

S2，在上述确定的基于点特征以及线特征的视觉相对位姿测量模型的基础上，分别进行相机内参数和特征提取像素误差的影响分析。

S31，根据设定的相机内外参数矩阵以及跑道长度以及宽度生成的机场跑道场景真值模板，如图2所示。

S32，通过Halcon软件编程，实现基于点/线特征的视觉相对位姿测量。如图3所示测量值的编程示意图，可作为后续误差分析的真实测量值。

S33，在上述视觉相对位姿测量模型的Halcon代码中，通过分别给相机内参数(如分辨率、焦距等参数)添加误差扰动，由此分析相机内参数标定误差对视觉相对位姿测量误差的影响程度；通过给场景成像所生成的真值模板中的特征点、特征线添加像素误差(如跑道左右边线端点，左右边线与起始线)，从而模拟特征提取误差对视觉相对位姿测量误差的影响。

综上所述，本发明提供了一种基于点线综合特征的视觉相对位姿测量误差估计方法，该基于点线综合特征的视觉相对位姿测量误差估计方法通过建立视觉相对位姿测量模型，进行相机内参数和特征提取像素误差的影响分析，并进行误差仿真分析，掌握了视觉相对位姿测量误差的影响规律，完成了视觉相对位姿测量误差估计。本发明的技术方案能够解决无人机自主起降过程中视觉相对位姿测量精度易受目标特征提取精度和相机内参数标定误差影响的技术问题。

如图4所示，本发明中各坐标系的定义如下：

世界坐标系(w系)：以跑道起始线与跑道中心线的交点为原点o_w；x_w轴与跑道起始线重合，右向为正；y_w轴垂直于跑道平面，向下为正；z_w轴沿跑道中心线，前向为正；o_wx_wy_wz_w构成右手坐标系；世界坐标系下某点的坐标用(x_w,y_w,z_w)表示。

机场坐标系：以跑道起始线与跑道中心线的交点为原点o_a；x_a轴沿跑道中心线，前向为正；y_a轴垂直于跑道平面，向上为正；z_a轴与跑道起始线重合，右向为正；o_ax_ay_az_a构成右手坐标系；机场坐标系下某点的坐标用(x_a,y_a,z_a)表示。

摄像机坐标系：以光学系统的像方主点为原点o_c；当正对光学系统观察时，x_c轴平行于成像平面坐标系的水平轴，左向为正；y_c轴平行于成像平面坐标系的垂直轴，向下为正；z_c轴指向观察者，并与x_c轴和y_c轴构成右手坐标系。

图像坐标系：在摄像机光敏面所在的平面内建立图像坐标系，它是一个二维平面坐标系，以图像左上角为原点，沿图像水平方向向右为图像坐标系的x_i轴，沿图像垂直方向向下为图像坐标系的y_i轴，图像坐标系的单位是像素。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于点线综合特征的视觉相对位姿测量误差估计方法，其特征在于，所述基于点线综合特征的视觉相对位姿测量误差估计方法包括：

S1、建立视觉相对位姿测量模型，包括建立基于点特征的视觉相对位姿测量模型和基于线特征的视觉相对位姿测量模型；

S2、对所述视觉相对位姿测量模型进行误差影响参数分析，包括相机内参数的影响分析和特征提取像素误差的影响分析；

S3、对所述视觉相对位姿测量模型进行误差仿真分析以完成视觉相对位姿测量误差估计；

其中，所述基于点特征的视觉相对位姿测量模型为其中，/>是表示从世界坐标系到相机坐标系的旋转矩阵，/>是相机坐标系原点在世界坐标系下的坐标，为空间点P_i对应的世界坐标系中的齐次坐标，/>为空间点P_i从世界坐标系中的齐次坐标投影到图像坐标系中的齐次坐标，K为相机内参数矩阵，/>f为相机镜头焦距，s_x和s_y是图像水平和垂直方向的像元尺寸，[c_x，c_y]^T是图像的主点坐标；函数F₁和F₂的具体表达形式根据实际应用场景中的点特征对应的相机内参数、世界坐标系中的齐次坐标和图像坐标系中的齐次坐标确定；

所述基于线特征的视觉相对位姿测量模型为其中，/>和/>分别表示基于线特征求解的旋转矩阵和平移向量；K为相机内参数矩阵，L为跑道特征线在世界坐标系中的三维坐标，/>m为直线的矩，/>为直线的方向向量；l为L对应图像坐标系中的像素坐标；函数F₃和F₄的具体表达形式根据实际应用场景中的线特征对应的相机内参数、世界坐标系中的三维坐标和图像坐标系中的像素坐标获取；

根据分析相机内参数的影响，其中，j＝1,2,3，k₁表示镜头焦距与像元尺寸之间的比值f/s_x，k₂表示镜头焦距与像元尺寸之间的比值f/s_y，k₃表示图像的主点坐标[c_x，c_y]^T；

根据 分析特征提取像素误差的影响，其中，/>和/>分别表示由第i个特征点行坐标像素提取误差Δx_i引起的姿态误差和位置误差，/>和/>分别表示由第i个特征点列坐标像素提取误差Δy_i引起的姿态误差和位置误差，/>和/>分别表示由图像坐标系内第i条直线斜率变化Δk_i所引起的姿态误差和位置误差，/>和/>分别表示由图像坐标系内第i条直线截距Δb_i所引起的姿态误差和位置误差。

2.根据权利要求1所述的基于点线综合特征的视觉相对位姿测量误差估计方法，其特征在于，所述基于线特征求解的旋转矩阵 C_θ、C_γ、/>分别表示对应俯仰角、横滚角和偏航角的旋转矩阵，θ、γ、/>分别表示俯仰角、横滚角、偏航角。

3.根据权利要求1所述的基于点线综合特征的视觉相对位姿测量误差估计方法，其特征在于，利用Halcon软件对所述视觉相对位姿测量模型进行误差仿真分析。

4.根据权利要求3所述的基于点线综合特征的视觉相对位姿测量误差估计方法，其特征在于，利用Halcon软件对所述视觉相对位姿测量模型进行误差仿真分析具体包括：

S31、根据测量场景中的点特征和线特征分布，利用Halcon软件模拟应用场景，通过设定相机内外参数，生成场景成像真值模板；

S32、通过Halcon软件编程，实现基于点线特征的视觉相对位姿测量；

S33、通过Halcon代码分别给相机内参数、真值模板的特征点和特征线添加误差扰动，以此分析相机内参数标定误差和特征提取误差对视觉相对位姿测量误差的影响。

5.根据权利要求4所述的基于点线综合特征的视觉相对位姿测量误差估计方法，其特征在于，相机外参数包括相机位置。