CN114296069B - 一种基于视觉雷达的小天体探测器多模型导航方法 - Google Patents

Abstract

本发明公开了一种基于视觉雷达的小天体探测器柔性多模型导航方法，以解决探测器附着小天体时刚性碰撞问题，提高导航精度。本发明实现方法为：首先将柔性材料和探测器结合，构成柔性探测器物理模型，并根据柔性材料的刚柔耦合特性，构建动力学模型；随后将视觉雷达数据融合，完成视觉/雷达结合导航滤波；最后运用航迹关联算法和小天体柔性探测器多模型滤波算法，实现在柔性多模型下，柔性探测器对小天体表面的附着导航。本方案根据柔性材料在不同柔变系数下的柔性多模型，将柔性材料与多模型滤波算法结合，提高导航精度。

Description

一种基于视觉雷达的小天体探测器多模型导航方法

技术领域

本发明属于深空探测器自主导航领域，特别适用于深空探测器跟踪附着小天体的过程，具体涉及一种基于视觉雷达的小天体探测器柔性多模型导航方法。

背景技术

近年来，深空探测不断发展，小天体探测也已经成为一项艰巨且具有科学意义的深空探测任务。然而，与行星探测任务相比，小天体探测任务具有探测目标小、所处环境多变、引力小、着陆点不规则等特点。当前小天体探测器在进行小天体附着时，由于单个探测器、单一传感器的限制，以及对目标运动模型建模的不确定，导致探测器无法准确跟踪小天体；由于探测器与小天体接触时发生刚性碰撞，导致小天体探测器附着时发生反弹而失败，探测器无法准确附着小天体，不能完成小天体样品采集的任务。

柔性材料发展迅速，柔性材料以其刚柔耦合特性，成为一种极具应用前景的航天探测器材料。但是，柔性材料有复杂的非线性特性，这使得柔性材料结构复杂且极易受到扰动。将雷达与视觉导航方法结合，提高导航精度，增强准确性；与此同时，运用多模型交互导航的方式，引入柔性探测器的不同动力学模型，将柔性材料与多模型交互导航算法结合，增强组合导航的可靠性的同时，大大加强了探测器附着小天体成功的概率。交互式多模型算法作为一种常见的数据融合算法，精度高，可靠性强，用于深空小天体探测任务，与组合导航算法和新型的柔性材料结合，完成深空探测任务，国内对该方面的研究仍在不断进行中。

发明内容

本发明的目的是解决小天体跟踪精度低、小天体与探测器刚性碰撞导致附着失败等问题，提出了一种基于视觉雷达小天体多模型柔性探测器导航方法。将视觉/雷达探测器与柔性材料相结合，建立小天体柔性探测器物理模型；同时根据柔性材料的刚柔耦合特性，建立不同位置状态下的多种简化动力学模型；研究视觉/雷达数据融合目标导航算法，设计了将视觉和雷达信息作为观测量的卡尔曼滤波器，基于该滤波器和不同状态下的动力学模型，运用了一种视觉/雷达的多模型导航算方法，提高导航精度，进而解决柔性探测器在小天体表面附着等方面问题。

本发明是通过下述技术方案实现的。

本发明公开的柔性探测器多模型导航方法，根据刚柔耦合特性对柔性探测器上的多个探测器点建立多种动力学观测模型，将视觉雷达融合信息作为输入量，引入多种动力学模型，实现整体柔性探测器对小天体表面特征点的导航：将视觉/雷达探测器与柔性材料相结合，建立柔性探测器物理模型；基于柔性体的刚柔耦合特性建立不同位置状态下的多种动力学模型；运用视觉/雷达数据融合目标跟踪算法，提出了将视觉和雷达信息作为观测量的卡尔曼滤波器，基于该滤波器和不同状态下柔性探测器的动力学模型，提出了一种视觉/雷达的小天体柔性探测器多模型导航方法，提高导航精度，进而解决柔性探测器在小天体表面附着等方面问题。

本发明公开的一种基于视觉雷达的小天体探测器柔性多模型导航方法，包括以下步骤：

步骤一、搭建柔性探测器物理模型：

将柔性材料与探测装置结合，设每个质量聚集体为一个节点，可以设置N个节点。其中柔性材料基本由硅橡胶与有机硅组成，根据材料构成不同，柔性材料有不同的柔变系数；每个探测装置由视觉相机、激光雷达组成，用于定位与导航。

步骤二、构建柔性探测器动力学模型：

根据柔性探测器的刚柔耦合特性，将柔性探测器节点之间的连接等效为弹性连接，得到探测器节点的非线性动力学模型。

本发明采用混合坐标系法，其中包括描述单个节点与着陆点处的距离、姿态倾斜角关系的聚集体平动系o-x_i-y_i-z_i(i＝1,2,...,N)和描述整个柔性探测器与着陆点处地面的距离、姿态倾斜角关系的柔性探测器平动系o_t-x_t-y_t-z_t，其中，i表示为柔性探测器上第i个探测器节点。

柔性探测器节点绕x轴转过的角度为θ，则其转动角速度与角加速度可分别表示为和/>记聚集体平动系原点在柔性探测器平动系下的位置矢量为r₀,记聚集体平动系到柔性探测器平动系的空间转换矩阵为A。可得柔性探测器中的第i个探测器节点在柔性探测器平动系下的位置矢量R_i为：

R_i＝r₀+A(r_i+Nq)

其中，r_i为节点i在聚集体平动系下未发生运动前的位置矢量，Nq为节点i在聚集体平动系下的弹性变形。从而，节点i在柔性探测器平动系下的速度与加速度分别为：

单元动能可表示为：

对单元动能进行变分，并在时间上进行积分，通过哈密顿原理得到柔性探测器节点间刚柔耦合非线性动力学模型为：

其中，C为阻尼等效矩阵，G为刚柔耦合引起的附加质量矩阵，M为节点质量矩阵，Q_T为刚性运动加速度引起的外载荷，Q_П为预应变产生的载荷列阵。

柔性探测器节点在不同位置下，建立不同刚柔耦合非线性模型，得到各个柔性探测器节点的独立状态方程和独立观测方程。

步骤三、通过视觉/雷达目标导航算法进行单模型探测器导航：

通过柔性探测器搭载相机拍摄的图像进行解算得到目标的深度信息，雷达信息经过杂质过滤，将视觉信息与雷达数据进行数据融合，设计一种将融合后的像素坐标作为观测量的卡尔曼滤波，将融合结果输入到滤波器中，得到目标特征点的位置估计，原理如图4所示。

其中，针对传统的视觉/雷达融合的目标导航方法，即将视觉与雷达解算的位置信息直接作为卡尔曼滤波的观测量的方法，视觉解算结果误差为非高斯白噪声，导致滤波精度不高，所以提出了一种将像素坐标作为观测量的视觉/雷达信息融合的卡尔曼滤波器，以提高目标跟踪精度。

将柔性探测器节点的状态变量选取为：

X＝[x,y,z,V_x,V_y,V_z,a_x,a_y,a_z]^T

其中，x,y,z为跟踪目标的位置信息，V_x,V_y,V_z为跟踪目标三个方向的速度分量，a_x,a_y,a_z为跟踪目标三个方向的加速度信息。

则其目标状态方程为：

X(k+1)＝ΦX(k)+ΓW(k)

其中Φ为状态转移矩阵，G为过程噪声矩阵，W为过程噪声。观测方程为：

其中，(u₁,v₁)与(u₂,v₂)分别为柔性探测器节点1与柔性探测器节点2搭载相机拍摄到目标天体部分特征点的像素坐标，(x_r,y_r,z_r)为雷达测量到的小天体目标特征点的三维坐标。

步骤四、运用最近临界法进行特征点关联：

雷达和视觉方法采集的小天体表面的数据复杂，其中包括不同的特征点和干扰数据，本发明采用最近临界法进行特征点匹配和关联，提高探测器对小天体表面数据采集的准确度。

其中，假设检测到小天体表面的特征点有c个类别ω₁，ω₂，…，ω_c，每类有N_i(i＝1,2,…c)个标明类别的样本。则规定ω_i类的判别函数为：

其中，i表示ω_i类，k表示ω_i类N_i个样本中的第k个。

最近邻法应用到小天体表面多目标导航系统中的过程如图5所示。假设存在三个小天体目标特征点，相应传感器在各时刻分别可以得到三组数据(a_x(t),a_y(t),a_z(t))、(b_x(t),b_y(t),b_z(t))与(c_x(t),c_y(t),c_z(t))，将属于同一类的数据串联起来。

步骤五、进行柔性探测器多模型轨迹导航：

步骤三运用的是单一模型的导航算法，现在运用多模型算法，根据各个柔性探测器节点之间的位置参数不同，得到的不同动力学模型，将各种状态下的模型引入系统中，使各个运动模型尽可能准确的描述柔性探测器不同的运动状态，经过输入交互、状态滤波、模型概率更新和状态融合输出四个步骤，完成精确导航。多模型算法原理如图6所示。

本发明采用多模型算法将柔性探测器的各种运动模型引入系统中，使各个运动模型尽可能准确的描述柔性探测器的运动状态。

多模型算法主要包括输入交互、状态滤波、模型概率更新和状态融合四个步骤。通过输入交互，可以得到系统内每一个运动模型滤波器的初始状态以及初始状态协方差的估计值，将通过输入交互得到的估计值作为各自模型滤波器的输入值将通过输入交互得到的初始状态估计值与初始状态协方差估计值作为滤波器当前输入，估计出下一时刻的状态与状态协方差；经过滤波器更新后，可以得到第j个滤波器的新息和新息协方差，结合预测模型转移概率μ_ij(k+1∣k)，可以得到当前时刻的模型概率估计；将求得的模型概率作为加权值，将状态估计与状态协方差估计的加权和作为整个算法的最终输出值：

其中与/>分别为k时刻第j个模型的状态估计值与状态协方差估计值,u_ij(k∣k+1)为预测模型i转移到模型j的转移概率,可由下式求得：

通过上述过程可以分别得到各个柔性探测器节点对目标小天体表面特征点的导航信息，将这些信息经过卡尔曼滤波器可实现对各轨迹的融合，得到最终的导航轨迹估计信息。

与现有技术相比，本发明的优点和积极效果在于：

1、本发明公开的一种视觉雷达的小天体探测器柔性多模型导航方法，将探测器柔性化，根据柔性材料的柔变系数不同，将柔性体内力、内力矩等效为弹簧、扭簧、阻尼器的作用力及力矩，建立各个探测器之间的不同动力学模型，将视觉与雷达信息进行融合，将融合后的数据作为卡尔曼滤波的输入之一，用于导航轨迹预测，提高导航精度。

2、本发明公开的一种视觉雷达的小天体探测器柔性多模型导航方法，利用雷达和视觉得到的小天体特征点和各个探测器节点的位置信息，引入不同位置下不同的柔性探测器模型，各个探测器之间数据交互，得到不同的观测方程，通过滤波，提高对小天体表面特征点导航的准确性，完成探测器对小天体表面的着陆导航。

附图说明

图1为本发明的总体流程图；

图2为本发明柔性探测器物理模型俯视图；

图3为本发明柔性探测器物理模型仰视图；

图4为本本发明的视觉雷达单一模型跟踪原理示意图；

图5为本发明航迹关联算法示意图；

图6为本发明多模型算法示意图；

图7为本发明仿真轨迹图；

图8为本发明仿真位置误差图；

图9为本发明多模型概率估计曲线。

具体实施方式

为了能够更加清楚地理解本发明的目的、特征和优点，下面结合附图及实例对本发明做进一步说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用不同于在此描述的其他方式来实施，因此，本发明并不限于下面公开的具体实施例。

一种基于视觉雷达的小天体探测器柔性多模型导航方法，包括以下步骤：

步骤一、构建柔性探测器物理模型；

步骤二、构建柔性探测器动力学模型；

步骤三、通过视觉/雷达目标导航算法进行单模型柔性探测器导航；

步骤四、运用最近临界法进行特征点关联；

步骤五、进行多模型轨迹导航。

具体的：

步骤一、构建柔性探测器物理模型：

本发明将柔性材料与探测装置结合，每个探测装置为一个节点，以4个探测节点为例。其中柔性材料基本由硅橡胶与有机硅组成，根据材料不同，柔性材料有不同的柔变系数；每个探测装置由视觉相机、激光雷达成，用于定位与导航，柔性探测器物理模型结构俯视和仰视图如图2和图3所示。

步骤二、构建柔性探测器动力学模型：

将探测器节点之间的连接等效为弹性连接，根据柔性探测器的刚柔耦合特性，得到探测器节点的非线性动力学模型。

本发明采用混合坐标系法，其中包括描述单个节点与着陆点处的距离、姿态倾斜角关系的聚集体平动系o-x_i-y_i-z_i(i＝1,2,...,N)和描述整个柔性探测器与着陆点处地面的距离、姿态倾斜角关系的探测器平动系o_t-x_t-y_t-z_t，其中i为柔性探测器上不同的探测器节点。

柔性探测器节点绕x轴转过的角度为θ，转动角速度与角加速度分别为和/>记聚集体平动系原点在柔性探测器平动系下的位置矢量为r₀,记聚集体平动系到柔性探测器平动系的空间转换矩阵为A。可得柔性探测器中的第i个柔性探测器节点在柔性探测器平动系下的位置矢量R_i为：

R_i＝r₀+A(r_i+Nq)

其中，r_i为节点i在聚集体平动系下未发生运动前的位置矢量，Nq为节点i在聚集体平动系下的弹性变形。从而，节点i在柔性探测器平动系下的速度与加速度分别为：

单元动能可表示为：

对单元动能进行变分，并在时间上进行积分，通过哈密顿原理得到柔性探测器节点间刚柔耦合非线性动力学模型为：

其中，C为阻尼等效矩阵，G为刚柔耦合引起的附加质量矩阵，M为节点质量矩阵，Q_T为刚性运动加速度引起的外载荷，Q_Π为预应变产生的载荷列阵。根据刚柔耦合非线性模型，得到各个探测器节点的独立状态方程和独立观测模型。

步骤三、通过视觉/雷达目标导航算法进行单模型柔性探测器导航：

通过探测器搭载相机拍摄的图像进行解算得到目标的数据信息，雷达信息经过杂质过滤，将视觉信息与雷达数据进行数据融合，设计一种将融合后的像素坐标作为观测量的卡尔曼滤波，将融合结果输入到滤波器中，得到目标特征点的位置估计，原理如图4所示。

其中，针对传统的视觉/雷达融合的目标导航方法，即将视觉与雷达解算的位置信息直接作为卡尔曼滤波的观测量的方法，视觉解算结果误差为非高斯白噪声，导致滤波精度不高，所以提出了一种将融合后的像素坐标作为观测量的视觉/雷达信息融合的卡尔曼滤波器，以提高目标跟踪精度。

将柔性探测器节点的状态变量选取为：

X＝[x,y,z,V_x,V_y,V_z,a_x,a_y,a_z]^T

其中，x,y,z为跟踪目标的位置信息，V_x,V_y,V_z为跟踪目标三个方向的速度分量，a_x,a_y,a_z为跟踪目标三个方向的加速度信息。

则其目标状态方程为：

X(k+1)＝ΦX(k)+ΓW(k)

其中Φ为状态转移矩阵，G为过程噪声矩阵，W为过程噪声，观测方程为：

其中，(u₁,v₁)与(u₂,v₂)分别为探测器节点1与探测器节点2搭载相机拍摄到目标天体部分特征点的像素坐标，(x_r,y_r,z_r)为雷达测量到的小天体目标特征点的三维坐标。

步骤四、运用最近临界法进行特征点关联；

雷达和视觉采集的小天体表面的数据信息复杂，其中包括不同的特征点和干扰数据，本发明采用最近临界法进行特征点匹配和关联，提高探测器对小天体表面数据采集的准确度。

其中，假设检测到小天体表面有c个类别ω₁，ω₂，…，ω_c，每类有N_i(i＝1,2,…c)个标明类别的样本。则规定ω_i类的判别函数为：

其中，i表示ω_i类，k表示ω_i类N_i个样本中的第k个。

最近邻法应用到小天体表面多目标导航系统中的过程如图5所示。假设存在三个小天体目标特征点，相应传感器在各时刻分别可以得到三组数据(a_x(t),a_y(t),a_z(t))、(b_x(t),b_y(t),b_z(t))与(c_x(t),c_y(t),c_z(t))，将属于同一类的数据串联起来。

步骤五、进行多模型轨迹导航：

步骤三运用的是单一模型的跟踪算法，现在运用多模型算法将柔性探测器节点不同受力情况下的运动模型引入系统中，使各个运动模型可以尽可能准确的描述各柔性探测器节点的运动状态，经过输入交互、状态滤波、模型概率更新和状态融合输出四个步骤，得到各探测器点的最终运动轨迹，算法详细原理如图6所示。

采用多模型算法将柔性探测器节点的各种运动模型引入系统中，尽可能准确的描述柔性探测器节点的运动状态。

多模型算法主要包括输入交互、状态滤波、模型概率更新和状态融合四个步骤。具体过程如下：

输入交互：通过输入交互，可以得到系统内每一个运动模型滤波器的初始状态以及初始状态协方差的估计值，将通过输入交互得到的估计值作为各自模型滤波器的输入值。输入交互主要包括状态交互与状态协方差交互两个方面。

状态交互：

状态协方差交互：

其中，与/>分别为k时刻第j个模型的状态估计值与状态协方差估计值，u_ij(k∣k+1)为预测模型i转移到模型j的转移概率：

状态滤波：将通过输入交互得到的初始状态估计值与初始状态协方差估计值/>作为滤波器当前输入，估计出下一时刻的状态与状态协方差，其过程与单模型滤波过程相同。

预测：

滤波增益计算：

K^j(k+1)＝P^j(k+1∣k)H^j(k)^T[H^j(k)P^j(k+1∣k)H^j(k)^T+R^j(k)]^-1

状态更新：

X^j(k+1∣k+1)＝X^j(k+1∣k)+K^j(k)[Z(k)-H^j(k)X^j(k+1∣k)]

模型概率更新：经过滤波器更新后，可以得到第j个滤波器的新息和新息协方差，结合预测模型转移概率μ_ij(k+1∣k)，可以得到当前时刻的模型概率估计。

新息和新息协方差：

模型概率更新：

其中，π_ij为模型i转移到模型j的转移概率。

状态融合输出：将求得的模型概率作为加权值，将状态估计与状态协方差估计的加权和作为整个算法的最终输出值。

通过上述过程可以分别得到各个探测器节点对小天体目标表面特征点的导航信息，控制柔性探测器整体导航附着小天体。

为验证本方法的有效性，以三个柔性探测器节点为例，对小天体表面一个特征点进行导航估计，通过模型受力分析，设定模型一(M1)为匀速运动，模型二(M2)为顺时针运动，模型三(M3)为逆时针运动。

其中模型一：

模型二与模型三：

假设各柔性探测器节点的运动仿真条件如表1所示，小天体目标在1～150s、251～400s做匀速直线运动，151～250s以的速度做顺时针运动，以/>的速度401～500s做逆时针运动。在上述仿真条件下进行100次蒙特卡洛仿真，得到柔性探测器对小天体目标特征点的导航曲线、位置误差、模型概率估计曲线等，仿真结果如图7至9所示。

探测器节点运动仿真条件：

表1

从图7可以看出，该方法可以实现对小天体表面特征点的有效导航，从图8的位置误差图可以看出，该算法在三个方向的误差均小于1.5米，根据图9的模型概率更新来看，本发明提出的方法可以对模型改变做出及时响应，精度更高，可靠性更强。

以上所述，仅为本发明的较佳实例，并非是对本发明做其他限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所做的任何修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (2)

1.一种基于视觉雷达的小天体探测器柔性多模型导航方法，包括以下步骤：

步骤一、每个探测装置由视觉相机、激光雷达组成，搭建柔性探测器物理模型，用于定位与导航；

步骤二、根据柔性材料的刚柔特性，将探测器节点之间的耦合力等效为弹性连接，利用探测器节点之间的不同位置关系，建立不同参数下的非线性动力学模型，构建柔性探测器动力学模型；

在构建动力学模型时，采用混合坐标法：

采用混合坐标系法，其中包括描述单个节点与着陆点处的距离、姿态倾斜角关系的聚集体平动系o-x_i-y_i-z_i(i＝1,2,...,N)和描述整个柔性探测器与着陆点处地面的距离、姿态倾斜角关系的柔性探测器平动系o_t-x_t-y_t-z_t，其中，i为柔性探测器上不同的探测器节点；

探测器节点绕x轴转过的角度为θ，因此其转动角速度与角加速度可分别表示为和/>聚集体平动系原点在探测器平动系下的位置矢量为r₀,聚集体平动系到探测器平动系的空间装换矩阵为A；可得柔性探测器中的第i个探测器节点在探测器平动系下的位置矢量R_i为：

R_i＝r₀+A(r_i+Nq)

其中，r_i为节点i在聚集体平动系下未发生运动前的位置矢量，Nq为节点i在聚集体平动系下的弹性变形，从而，节点i在探测器平动系下的速度与加速度分别为：

探测器节点动能可表示为：

对探测器节点动能进行变分，并在时间上进行积分，通过哈密顿原理得到柔性探测器节点间刚柔耦合非线性动力学模型为：

其中，C为阻尼等效矩阵，G为刚柔耦合引起的附加质量矩阵，M为节点质量矩阵，Q_T为刚性运动加速度引起的外载荷，Q_∏为预应变产生的载荷列阵，探测器节点在不同位置下，建立不同刚柔耦合非线性模型，得到各个探测器节点的独立状态方程和独立观测模型；

步骤三、将视觉信息与雷达数据进行数据融合，设计一种将融合后的像素坐标作为观测量的卡尔曼滤波，将融合结果输入到滤波器中，得到目标特征点的位置估计，通过视觉/雷达目标导航算法进行探测器导航；

步骤四、采用最近临界法进行特征点匹配和关联，将同一特征点的数据串联，剔除无用干扰数据，提高导航精确度，运用最近临界法进行特征点关联；

步骤五、多个探测器节点不同柔性参数下的多种模型交互，进行多模型轨迹导航，提高导航的精确度。

2.根据权利要求1所述的一种基于视觉雷达小天体多模型柔性探测器导航方法，其特征在于，所述步骤五中，在进行柔性探测器轨迹整合时，将柔性探测器在不同位置状态不同柔变系数下的多模型算法引入轨迹整合过程，具体如下：

多模型算法主要包括输入交互、状态滤波、模型概率更新和状态融合四个步骤，通过输入交互，可以得到系统内每一个运动模型滤波器的初始状态以及初始状态协方差的估计值，将通过输入交互得到的估计值作为各自模型滤波器的输入值将通过输入交互得到的初始状态估计值与初始状态协方差估计值作为滤波器当前输入，估计出下一时刻的状态与状态协方差；经过滤波器更新后，可以得到第j个滤波器的新息和新息协方差，结合预测模型转移概率μ_ij+k+1∣k)，可以得到当前时刻的模型概率估计；将求得的模型概率作为加权值，将状态估计与状态协方差估计的加权和作为整个算法的最终输出值：

其中，与/>分别为k时刻第j个模型的状态估计值与状态协方差估计值,u_ij+k∣k+1)为预测模型i转移到模型j的转移概率,可由下式求得：

通过上述过程可以分别得到柔性探测器各个探测节点对目标小天体表面特征点的导航信息，将这些信息经过卡尔曼滤波器可实现对各轨迹的融合，得到最终的导航轨迹估计信息。