CN112378383B - 基于圆和线特征非合作目标相对位姿双目视觉测量方法 - Google Patents

Abstract

本发明属于非合作航天器相对导航系统设计领域，特别涉及一种基于圆和线特征非合作目标相对位姿双目视觉测量方法，包括以下步骤：通过图像处理算法和双目视觉特性，消除圆二义性；利用双目视觉下圆法向量和线平面法向量，定义特征提取误差的自适应系数；根据自适应系数和特征间的平行约束，测量目标翻滚角，并且提高姿态测量精度。本方法保障了复杂干扰环境对特征提取影响较大情况时仍然满足相对导航系统的测量精度，并且对翻滚角进行测量，具有较强的自适应能力和实现非合作目标六自由度相对位姿测量。

Description

基于圆和线特征非合作目标相对位姿双目视觉测量方法

技术领域

本发明属于非合作航天器相对导航系统设计领域，特别涉及一种基于圆和线特征非合作目标相对位姿双目视觉测量方法。

背景技术

目前，针对空间非合作目标的空间任务越来越受关注，例如在轨加注、在轨维护与装配等工作，非合作目标相对位姿的精确测量是保障空间任务正常进行的前提。空间非合作目标具有无法彼此通讯、无法安装人工靶标的特点，如何利用有限的测量实现非合作目标的相对位姿测量是航天器导航系统的一个关键技术。

在非合作目标导航任务中的接近段，通常可以利用喷嘴或者对接环等特征，实现对非合作目标的抓捕。而目前基于圆特征的位姿测量，由于圆的旋转不变性，导致此种测量方案的翻滚角姿态缺失，另外，受空间中光照、相对运动的影响，图像处理技术会存在一定的误差，从而导致位姿测量精度较差，这些往往会不利于空间非合作目标的抓捕任务实施。因此，如何利用非合作目标的有限结构信息和测量信息，实现对非合作目标六自由度位姿精确测量是当前航天器相对导航工程领域中亟待解决的问题。

发明内容

针对在实际的非合作目标导航的抓捕任务中，非合作目标相对位姿测量存在目标通信失效和缺乏人工靶标、翻滚角缺失与精度不足等问题，本发明提供了一种基于圆和线特征非合作目标相对位姿双目视觉测量方法，本方法考虑直线与圆特征平行约束和不同特征图像处理误差，设计了一种自适应系数下的位姿测量方法。本发明实现了基于圆姿态测量方法中缺失翻滚角的测量，同时提高了相对姿态的测量精度，保证了非合作目标抓捕的精确位姿测量任务。

为实现上述目的，本发明提供了一种基于圆和线特征非合作目标相对位姿双目视觉测量方法，包括如下步骤：

S1：通过图像处理算法，提取非合作目标的圆特征和直线特征，得到圆特征成像后椭圆参数和直线特征参数；

S2：基于步骤S1中得到的椭圆参数，计算圆特征的圆心位置和圆法向量，然后利用双目视觉消除圆二义性，得到消除圆二义性后的圆法向量；

S3：基于步骤S1中得到的直线特征参数，求得直线与相机原点构成线平面的法向量，并且基于步骤S2中得到的消除圆二义性后的圆法向量，定义圆和线特征图像处理算法下误差的自适应系数；

S4：基于步骤S3定义的自适应系数以及非合作目标直线与圆平行的约束，实现非合作目标翻滚角姿态测量，同时对消除圆二义性后的圆法向量进行修正，提高非合作目标姿态测量精度。

进一步，步骤S1中得到的圆特征成像后椭圆参数为：

au²+bv²+cuv+du+ev+f＝0 (1)

其中，[a,b,c,d,e,f]为椭圆方程参数，[u,v]为椭圆像素坐标；

直线特征方程为：

其中，

为直线对应的齐次坐标；k₁，k₂，k₃为直线特征参数，且k₁u+k₂v+k₃＝0等式成立。

进一步，步骤S2中利用双目视觉消除圆二义性具体过程为：

设定左目相机坐标系Σ_l为O_l-X_lY_lZ_l，右目相机坐标系Σ_r为O_r-X_rY_rZ_r，

采用双目视觉得到4个圆法向量

和对应的4个圆心坐标

计算4个圆法向量的夹角：

其中，

为左目相机坐标系Σ_l下的圆法向量，

为右目相机坐标系Σ_r下的圆法向量，ψ₁为圆法向量

的夹角，ψ₂为圆法向量

的夹角，ψ₃为圆法向量

的夹角，ψ₄为圆法向量

的夹角；

找到4个夹角中最小的夹角，从而消除圆的二义性，定义消除圆二义性后的左目相机坐标系Σ_l下的圆法向量为

圆心坐标为

右目相机坐标系Σ_r下的圆法向量为

圆心坐标为

将右目相机坐标系Σ_r下的圆心坐标

从右目相机坐标系Σ_r转换到左目相机坐标系Σ_l，得到非合作目标圆特征的圆心位置O_E为：

其中，^lR_r为双目视觉相对姿态矩阵，^lt_r为相对位置矩阵。

进一步，步骤S3具体过程如下：

计算两个圆法向量

和

的夹角ψ，并将其定义为圆图像处理算法的圆误差系数α：

α＝ψ (15)

定义非合作目标上有两条平行直线L₁、L₂，分别在双目中成像得到4个线平面法向量

其中

为左目相机坐标系Σ_l下的线平面法向量，

为右目相机坐标系Σ_r下的线平面法向量，4个线平面法向量

构成平面π₀，推导得到平面π₀的法向量

定义β表示直线图像处理算法的直线误差系数：

进一步，步骤S4具体过程如下：

直线L₁的方向向量

满足下列条件：

其中，π₁是直线L₁与相机原点形成的线平面，定义π_Q表示圆平面，根据L₁//π_Q得到：

其中，n_E为非合作目标圆特征的圆法向量，

直线L₁的方向向量

为：

定义非合作目标坐标系Σ_T为O_T-X_TY_TZ_T，其中Z_T轴与圆法向方向重合，Y_T轴与直线方向重合，X_T轴满足右手法则，直线L₁的方向向量

平行于非合作目标坐标系Y_T轴，因此，另外X_T轴和Z_T轴两个坐标轴的方向向量分别为：

其中，

分别为X_T轴、Y_T轴、Z_T轴的方向向量，

则非合作目标的姿态矩阵为

本发明的有益效果：

1)本发明考虑了不同特征提取的误差，设计了相应的自适应系数对提取精度进行了定量描述；

2)本发明利用直线特征实现对翻滚角的测量，并且通过引用自适应系数提高了姿态的测量精度。

附图说明

图1为本发明实施例的基于圆和线特征非合作目标相对位姿双目视觉测量方法流程图；

图2为本发明实施例的双目视觉对非合作目标测量模型示意图；

图3为本发明实施例的位置测量误差与相对误差仿真图；

图4为本发明实施例的非合作目标姿态测量误差仿真图。

具体实施方式

下面结合附图和实施例进一步描述本发明，应该理解，以下所述实施例旨在便于对本发明的理解，而对其不起任何限定作用。

如图1所示，本实施例的基于圆和线特征非合作目标相对位姿双目视觉测量方法，包括如下步骤：

S1：通过图像处理算法，提取非合作目标的圆特征和直线特征，得到圆特征成像后椭圆参数和直线特征参数；

设定双目相机的焦距为16.1mm，像素为1024×1024，像元尺寸为14μm×14μm，基线为0.24m，并且两相机间的旋转矩阵为I_3×3。在非合作目标空间抓捕中，喷嘴特征可以视为圆特征，多边形本体可以视为多条平行直线特征，并且喷嘴与多边形本体平面平行。设定目标圆半径为235mm，直线与圆心距离为470mm。圆心的位置设定为[T_z×tan(-10°),T_z×tan(-5°),T_z]，T_z为圆心的z坐标，其中考虑目标位置的变化，T_z的范围为2-10m，非合作目标圆的姿态设定为翻滚角30deg，俯仰角φ₀＝0，偏航角θ₀＝-15deg。如图2所示，左目相机坐标系Σ_l为O_l-X_lY_lZ_l，右目相机坐标系Σ_r为O_r-X_rY_rZ_r，两个相机系的齐次转换矩阵为^lT_r，u_l-v_l和u_r-v_r分别为左右目相机的像素坐标系，x_l-y_l和x_r-y_r分别为左右目相机的相平面π_l、π_r坐标系，L₁、L₂为空间中两条平行的直线，Q为圆特征，q为圆特征Q在左目相平面的投影，π_Q为圆平面，目标坐标系Σ_T为O_T-X_TY_TZ_T，直线L₁与左目原点O_l构成平面π₁，直线l₁为直线L₁在左目相平面的投影，直线L'₁、L'₂为直线L₁、L₂在平面π_Q的投影。

用椭圆图像处理算法，得到椭圆方程为：

au²+bv²+cuv+du+ev+f＝0 (1)

其中，[a,b,c,d,e,f]为检测到的椭圆方程参数，[u,v]为相应的像素坐标。

将式(1)写成统一形式为：

其中，

为椭圆方程参数的统一形式，X_c＝[u v 1]^T为像素坐标的统一形式。

利用直线图像处理算法，得到空间直线两条平行直线L₁和L₂。直线L_i对应的齐次坐标

为：

其中，

为直线特征参数，且

等式成立。

S2：基于步骤S1中得到的椭圆参数，计算圆特征的圆心位置和圆法向量，然后利用双目视觉消除圆二义性；

两个相机系的齐次转换矩阵为：

其中，^lR_r为双目视觉相对姿态矩阵，^lt_r为相对位置矩阵。设定非合作目标坐标系Σ_T为O_T-X_TY_TZ_T，其中Z_T轴与圆法向方向重合，Y_T轴与直线方向重合，X_T轴满足右手法则。

对于空间中一个任意点P＝[x y z]^T投影到相机平面上：

z[u v 1]^T＝K[x y z]^T (5)

其中，K为相机的标定内参矩阵。则椭圆的边缘点满足：

[x y z]K^TAK[x y z]^T＝0 (6)

定义Γ为椭圆边缘和相机中心构成的非标准圆锥曲面，其中Q＝K^TAK为曲面Γ的数学统一形式，通过对Q矩阵正交变换：

U^-1QU＝diag(λ₁ λ₂ λ₂) (7)

其中，U是Q的正交矩阵，λ_i和v_i(i＝1,2,3)是相应的特征值和特征向量，并且λ_i满足条件λ₁≥λ₂＞0＞λ₃。定义U＝[e₁ e₂ e₃]，如果[v₃]₃>0，那么e₃＝v₃，否则e₃＝-v₃；此外e₂＝v₂,e₁＝(e₂×e₃)/‖e₂×e₃‖。

在标准椭圆坐标系O_c-X'Y'Z'中，标准椭圆的圆心位置和法向量分别为：

其中，O'_i为标准椭圆的圆心，n'_i为标准椭圆的法向量，R为圆特征的半径。

通过将标准椭圆的圆心和法向量转换到相机参考坐标系中，得到圆的圆心位置和法向量为：

其中，

为圆的圆心位置，

为圆的圆法向量。

通过分析，基于圆的位姿测量方法有两种位姿解，这意味着圆位姿求解存在二义性。通过式(7)(9)，采用双目视觉可以得到4个圆法向量

其中

为坐标系Σ_l下的法向量，

为坐标系Σ_r下的法向量，并且通过姿态转移矩阵^lR_r进行了变换。计算4个法向量的夹角：

其中，ψ₁为圆法向量

的夹角，ψ₂为圆法向量

的夹角，ψ₃为圆法向量

的夹角，ψ₄为圆法向量

的夹角。

通过对比，找到4个夹角中最小的角，从而消除圆的二义性。定义消除二义性后的两个圆法向量为

和

圆心坐标为

将右目相机圆心坐标从坐标系Σ_r转换到坐标系Σ_l，圆特征的圆心坐标在坐标系Σ_l中为：

S3：基于步骤S1中得到的直线特征参数，求得直线与相机原点构成线平面的法向量，并且基于步骤S2利用双目视觉下圆法向量，定义圆和线特征图像处理算法下误差的自适应系数；

在左相机坐标系Σ_l中，π₁和π₂是两条平行直线L₁,L₂分别和相机原点O_l构成的线平面，并且投影矩阵M_l为：

其中，K_l为左目相机事先标定的内参矩阵。

因此，两个线平面π₁和π₂的齐次坐标为：

其中，[π_1x,π_1y,π_1z,π_1w]为线平面π₁的齐次坐标，以及[π_2x,π_2y,π_2z,π_2w]为线平面π₂的齐次坐标。

两个线平面π₁和π₂的法向量分别为：

根据公式(4)(14)，可以得到4个线平面的法向量

其中

为坐标系Σ_l下的线平面法向量，

为坐标系Σ_r下的线平面法向量，并且已通过姿态转移矩阵^lR_r进行了变换。

首先，计算两个圆法向量

和

的夹角，并将其定义为圆图像处理算法的误差系数：

α＝ψ (15)

其次，如果直线图像处理算法没有误差，那么4个线平面法向量是共面的，因此线平面法向量不共面特性反映直线图像处理算法的误差。4个向量

构成平面π₀，推导得到，平面π₀的法向量为

然后，β表示直线图像处理算法的误差系数：

S4：基于步骤S3定义的自适应系数以及直线与圆平行的约束，实现非合作目标翻滚角姿态测量，同时对圆法向量进行修正，提高非合作目标姿态测量精度。

直线L₁的方向向量

满足下列条件：

其中π₁是直线L₁与相机原点形成的线平面，定义π_Q表示圆平面，根据L₁//π_Q得到：

其中，n_E为圆特征的圆法向量。

因此，直线L₁的方向向量

为：

直线方向向量

平行于Y_T轴，因此，另外两个坐标轴的方向向量为：

其中，

分别为X_T轴、Y_T轴、Z_T轴的方向向量。

最后，非合作目标的姿态矩阵为

采用本实施例的方法获得的非合作目标位置误差和姿态仿真结果分别如图3以及图4所示。由图3可知，目标相对位置误差始终小于2％；同时，根据图4可知，三轴姿态角误差均在1.4deg以内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以对本发明的实施例做出若干变型和改进，这些都属于本发明的保护范围。

Claims (1)

1.一种基于圆和线特征非合作目标相对位姿双目视觉测量方法，其特征在于，包括如下步骤：

S1：通过图像处理算法，提取非合作目标的圆特征和直线特征，得到圆特征成像后椭圆参数为：

au²+bv²+cuv+du+ev+f＝0 (1)

其中，a,b,c,d,e,f为椭圆方程参数，u,v为椭圆像素坐标；

直线特征方程为：

其中，

为直线对应的齐次坐标；k₁，k₂，k₃为直线特征参数，且k₁u+k₂v+k₃＝0等式成立；

S2：基于步骤S1中得到的椭圆参数，计算圆特征的圆心位置和圆法向量，然后利用双目视觉消除圆二义性，得到消除圆二义性后的圆法向量，具体过程为：

设定左目相机坐标系Σ_l为O_l-X_lY_lZ_l，右目相机坐标系Σ_r为O_r-X_rY_rZ_r，

采用双目视觉得到4个圆法向量

和对应的4个圆心坐标

计算4个圆法向量的夹角：

其中，

为左目相机坐标系Σ_l下的圆法向量，

为右目相机坐标系Σ_r下的圆法向量，ψ₁为圆法向量

的夹角，ψ₂为圆法向量

的夹角，ψ₃为圆法向量

的夹角，ψ₄为圆法向量

的夹角；

找到4个夹角中最小的夹角，从而消除圆的二义性，定义消除圆二义性后的左目相机坐标系Σ_l下的圆法向量为

圆心坐标为

右目相机坐标系Σ_r下的圆法向量为

圆心坐标为

将右目相机坐标系Σ_r下的圆心坐标

从右目相机坐标系Σ_r转换到左目相机坐标系Σ_l，得到非合作目标圆特征的圆心位置O_E为：

其中，^lR_r为双目视觉相对姿态矩阵，^lt_r为相对位置矩阵；

S3：基于步骤S1中得到的直线特征参数，求得直线与相机原点构成线平面的法向量，并且基于步骤S2中得到的消除圆二义性后的圆法向量，定义圆和直线特征图像处理算法下误差的自适应系数；具体过程如下：

计算两个圆法向量

和

的夹角ψ，并将其定义为圆图像处理算法的圆误差系数α：

α＝ψ (15)

定义非合作目标上有两条平行直线L₁、L₂，分别在双目中成像得到4个线平面法向量

其中

为左目相机坐标系Σ_l下的线平面法向量，

为右目相机坐标系Σ_r下的线平面法向量，4个线平面法向量

构成平面π₀，推导得到平面π₀的法向量

定义β表示直线图像处理算法的直线误差系数：

S4：基于步骤S3定义的自适应系数以及非合作目标直线与圆平行的约束，实现非合作目标翻滚角姿态测量，同时对消除圆二义性后的圆法向量进行修正，提高非合作目标姿态测量精度；具体过程如下：

直线L₁的方向向量

满足下列条件：

其中，π₁是直线L₁与相机原点形成的线平面，

是平面π₁的法向量，定义π_Q表示圆平面，根据L₁//π_Q得到：

其中，n_E为非合作目标圆特征的圆法向量，

直线L₁的方向向量

为：

定义非合作目标坐标系Σ_T为O_T-X_TY_TZ_T,其中Z_T轴与圆法向方向重合，Y_T轴与直线方向重合，X_T轴满足右手法则，直线L₁的方向向量

平行于非合作目标坐标系Y_T轴，因此，另外X_T轴和Z_T轴两个坐标轴的方向向量分别为：

其中，

分别为X_T轴、Y_T轴、Z_T轴的方向向量，

则非合作目标的姿态矩阵为

Images