CN111750870B - 一种空间翻滚火箭箭体的运动参数估计方法 - Google Patents

Abstract

本发明公开一种空间翻滚火箭箭体的运动参数估计方法，该方法针对空间翻滚运动状态下的火箭箭体，利用激光成像雷达获得的点云，通过随机一致性采样算法求得自旋轴，随后采用带有遗忘因子的最小二乘法求得翻滚轴参数和章动角，接着求解得出空间章动角速率。本发明方法的优点是适用于空间翻滚运动状态下的火箭箭体，利用激光成像雷达对回转体结构的火箭箭体进行观测，通过点云数据处理，实现对翻滚火箭箭体的运动参数估计。本发明方法能够稳定地计算得到火箭箭体的翻滚轴、章动角以及空间章动角速率，具有精度高的特点。

Description

一种空间翻滚火箭箭体的运动参数估计方法

技术领域

本发明属于空间自主导航技术领域，具体是一种基于点云数据的空间翻滚火箭箭体运动参数估计方法。

背景技术

空间碎片主动移除的主要目标是安全地捕获和处置太空垃圾，使低地球轨道区域碎片进入大气层烧毁，或使地球静止轨道区域碎片轨道抬高进入坟墓轨道，从而达到保护在轨航天器不受碎片撞击。根据待捕获的空间目标是否带有有效的先验信息，可将空间目标分为合作目标与非合作目标。空间非合作目标未配备星箭通讯装置或其它主动传感器，其余的航天器难以通过与其建立信息传输或通过感知识别装置等方式来获取目标的位置、姿态等状态信息。空间碎片为典型的空间非合作目标，其包括完成任务的火箭箭体、失效的卫星本体等。失效卫星和火箭箭体由于尺寸大、包含未使用的燃料，其存在碰撞以及点火爆炸的潜在风险，是在轨主动清除任务中高优先级的目标。与卫星相比，火箭箭体呈回转体结构，其成像具有表面特征点稀少的特点，采用特征点进行运动参数估计的传统方法难度较大，因此，如何获取翻滚运动状态下的火箭箭体的运动参数是后续近距离逼近的重要前提。

非扫描式激光成像雷达可以实时获取目标的三维量测数据，包括三维点云、二维深度图和强度图数据，具有对背景杂散光的抑制能力强、探测距离远、且不存在运动模糊、帧率高等优点，可以满足实时测量。

发明内容

本发明的目的在于提出一种空间翻滚火箭箭体的运动参数估计方法，利用激光成像雷达对回转体结构的火箭箭体进行观测，通过对获取的点云数据进行处理，实现对翻滚运动状态下的火箭箭体的运动参数估计。

实现本发明目的的技术解决方案为：一种空间翻滚火箭箭体的运动参数估计方法，针对空间翻滚运动状态下的火箭箭体，利用激光成像雷达获得点云数据，通过随机一致性采样算法求得自旋轴，随后采用带有遗忘因子的最小二乘法求得翻滚轴参数和章动角，接着求解得出绕翻滚轴的空间章动角速率。

具体实现步骤如下：

步骤(1)、利用激光成像雷达获取第K帧的目标点云Q_K。

步骤(2)、对第K帧目标点云Q_K进行点云表面法向量估计，得到每个点对应的法向量n_K{n_K,1,n_K,2,…,n_K,N}。

步骤(3)、利用目标点云Q_K和法向量n_K{n_K,1,n_K,2,…,n_K,N}采用随机采样一致性算法进行自旋轴的估计。

步骤(4)、当K＜2时，K＝K+1并回到步骤1。当K≥2时，对翻滚轴m_K进行估计，采用带有遗忘因子的最小二乘递推算法进行求解，得到翻滚轴的公共点m_K,0(m_a,m_b,m_c)和方向向量m_K,dir(m_dir,x,m_dir,y,m_dir,z)。

步骤(5)、利用自旋轴l_K和翻滚轴m_K的夹角公式，求解得到章动角θ_K。

步骤(6)、利用自旋轴l_K-1和l_K求得其投影线l_K-1′和l_K′，计算l_K-1′和l_K′的夹角，除以相邻帧之间的时间，得到空间章动角速率Ω_K。

本发明与现有技术相比，其显著优点为：本发明方法针对翻滚运动状态下的火箭箭体，利用激光成像雷达获得的点云，通过随机一致性采样算法求得自旋轴，随后采用带有遗忘因子的最小二乘法求得翻滚轴参数和章动角，接着求解得出绕翻滚轴的空间章动角速率。本发明方法适用于空间翻滚运动状态下的火箭箭体，能够稳定地计算得到火箭箭体的翻滚轴、章动角以及空间章动角速率，具有精度高的特点。

附图说明

图1是本发明一种空间翻滚火箭箭体的运动参数估计方法的流程图。

图2是圆柱体模型示意图。

图3是相邻自旋轴示意图。

图4是自旋轴投影示意图。

图5是本发明实施例中火箭箭体运动参数估计的翻滚轴曲线图。(a)翻滚轴公共点参数曲线，(b)翻滚轴公共点参数误差曲线，(c)翻滚轴方向向量参数曲线，(d)翻滚轴方向向量参数误差曲线。

图6是本发明实施例中火箭箭体运动参数估计的章动角曲线图。(a)章动角曲线，(b)章动角误差。

图7是本发明实施例中火箭箭体运动参数估计的空间章动角速率曲线图。(a)空间章动角速率曲线，(b)空间章动角速率误差曲线。

具体实施方式

下面结合说明书附图对本发明作进一步说明。

如图1所示，本发明为一种空间翻滚火箭箭体的运动参数估计方法，包括以下步骤：

步骤1：利用激光成像雷达获取第K帧的目标点云Q_K。

步骤2：对第K帧目标点云Q_K进行点云表面法向量估计，得到每个点对应的法向量n_K{n_K,1,n_K,2,…,n_K,N}。

步骤3：对第K帧目标点云Q_K以及每个点对应的法向量n_K{n_K,1,n_K,2,…,n_K,N}进行自旋轴的估计，采用的方法为随机采样一致性算法，步骤3具体包括：

步骤3.1：从目标点云Q_K中随机选取2个点{Q_K,i,Q_K,j|i,j∈[1,N]且i≠j}即其对应的法向量n_K,i和n_K,j；

步骤3.2：由点Q_K,i和Q_K,j及对应的法向量n_K,i和n_K,j求解出点Q_K,i和Q_K,j所在的圆柱体模型的参数，得到圆柱体的圆柱轴l以及其半径r；

圆柱体模型参数由圆柱轴l(l_dir,l_cen)和半径r表示，如图2所示，其可通过任意两点确定，求解过程如下所示：从点云集中任意选取两点Q_K,1和Q_K,2，分别计算得到它们的法向量n_K,1和n_K,2，则可以确定Q_K,1和Q_K,2所在圆柱体的圆柱轴的方向向量l_dir＝n_K,1×n_K,2。取经过点Q_K,1并垂直l_dir的平面A，将直线Q_K,1+tn_K,1和Q_K,₂+tn_K,2(t为直线参数)投影到平面A，投影后的交点即为圆柱轴上的一点l_cen。而l_cen和Q_K,1在该平面上的投影点之间的距离为圆柱体的半径r。

步骤3.3：对输入点云数据Q_K中的每一个点{Q_K,d|d∈[1,N]}，计算点Q_K,d到估计的圆柱体模型的圆柱轴l的距离r_d。设置阈值δ，当满足r_d∈[r-δ,r+δ]时，点Q_K,d被记作为内点，否则点Q_K,d被记作为外点；

步骤3.4：统计所有内点的个数，判断其是否大于设置的最小数目值，如果是，则记录求得的圆柱体模型参数，并存储所有内点作为分割结果，如果不是，则跳到步骤3.1进行下一轮计算；

步骤3.5：重复步骤3.1到步骤3.4共Z次，从中选取内点个数最多的一次，该次求得的圆柱体模型参数即为最符合数据的模型参数，存储的内点为最后的分割数据。此时求得的圆柱轴l即为第K帧自旋轴l_K。

步骤4：当K＜2时，K＝K+1并回到步骤1。当K≥2时，对翻滚轴m_K进行估计，采用带有遗忘因子的最小二乘递推算法进行求解，步骤4具体包括：

步骤4.1：由第K-1帧和第K帧的自旋轴可确定与翻滚轴相交的公共点，考虑相邻帧之间求得的自旋轴不一定相交，如图3所示，l_K-1和l_K分别表示K-1时刻和K时刻的火箭箭体的自旋轴，线段pq与l_K-1、l_K垂直，点p和点q为相邻两帧自旋轴之间的最近点。此时最近点可近似看作为翻滚轴公共点，具体的求解方法如下：

已知l_K-1和l_K的参数方程：

式中：W_K-1和W_K分别为自旋轴l_K-1和l_K上一点，而l_K-1,dir和l_K,dir分别为自旋轴l_K-1和l_K的方向向量。

因此，点p和点q可表示为：

式中：t_p和t_q分别为点p和点q的参数。

由于线段pq与l_K-1、l_K垂直，其点乘为0，如式(3)所示，将式(2)代入式(3)中，联立求解可得，如式(4)所示。

式中：a,b,c,d,e如下表示：

将式(4)代入到式(2)中，可求得点p和点q的坐标轴值，选取点p作为翻滚轴上的公共点m_K,0。

步骤4.2：自旋轴l_K和翻滚轴m_K相交于点m_K,0，如图4所示，由向量的夹角公式可得：

m_dir,x×l_K,x+m_dir,y×l_K,y+m_dir,z×l_K,z＝cos(θ_K) (6)

式中l_K(l_K,x,l_K,y,l_K,z)和m_K,dir(m_dir,x,m_dir,y,m_dir,z)分别表示自旋轴l_K和翻滚轴m_K的方向向量，θ_K为章动角。

将式(6)两边同时除以cos(θ_K)，0≤θ_K＜π/2得到：

用矩阵表示，可得：

式中：ψ_K＝[l_K,x l_K,y l_K,z]^T，y_K＝1。

步骤4.3：按照式(8)求解得到协方差矩阵P_K，增益矩阵G_K，其中ρ为遗忘因子；

步骤4.4：将求得的P_K和G_K代入式(10)，求解出K时刻的参数η_K；

步骤4.5：将求得的参数η_K单位化，得到翻滚轴m_K的方向向量m_K,dir。

步骤5：利用自旋轴l_K和翻滚轴m_K的夹角公式，求解得到章动角θ_K。求解如式(11)所示：

步骤6：从前后两帧中估算出自旋轴绕翻滚轴旋转的空间章动角速率Ω_K。如图4所示，平面B为一垂直于翻滚轴m_K的平面，将自旋轴l_K-1和l_K分别投影到平面B上，得到两条投影线l_K-1′和l_K′，投影线l_K-1′和l_K′的夹角υ_K(0≤υ_K＜π/2)为相邻帧间的空间章动角度，将空间章动角度除以相邻帧间的时间间隔就可以得到绕翻滚轴旋转的空间章动角速率。相邻帧间的空间章动角度和空间章动角速率的求解如式(12)所示：

式中：m_K,dir表示翻滚轴m_K的方向向量，l_K-1,dir和l_K,dir表示自旋轴l_K-1和l_K的方向向量，l_K-1,dir′和l_K,dir′表示投影线l_K-1′和l_K′的方向向量,υ_K表示相邻帧间的空间章动角度，Δt表示相邻帧间的时间间隔。

实施例

为了对本发明算法的有效性进行说明，充分展现出该方法具有更加准确的位姿获取性能，完成实验如下：

(1)实验初始条件及参数设置

仿真实验采用虚拟激光雷达和目标模型点云，根据激光成像雷达的参数和成像原理，通过软件仿真产生的传感器的量测数据。激光成像雷达的参数设置为：分辨率为500×500，视场角20°×20°。假设火箭箭体在空间中处于带章动的翻滚运动状态，服务航天器在空间中定点对目标航天器进行观测。假设在激光成像雷达测量坐标系O_Cx_Cy_Cz_C下，目标惯性坐标系的坐标原点O_O为(40,0,0)，翻滚轴公共点m₀为(40,0,0)，翻滚轴方向向量m_dir为(0,1,0)。

由欧拉动力学方程可知，为获得带章动的翻滚运动，本体坐标系上各个轴旋转的旋转角速率应满足：

式中：为绕自旋轴的自旋角速率，/>和/>为构成章动角θ的系数，Ω_r为本体章动角速率。

对式(13)中的w_x、w_y和w_z做积分：

式中：φ、λ分别为各个时刻对应的滚转角、俯仰角、偏航角。

将式(14)代入欧拉变换矩阵，得到目标模型点云数据变换到激光成像雷达观测坐标系下，仿真生成激光成像雷达的点云数据，令生成的章动角为6.96°，空间章动角速率为10°/s，t按照0,1,2...99,100选取，间隔为1s，共生成101个点云数据。

(2)实验结果分析

图5为本方法得到的翻滚轴参数曲线和误差曲线，图6为求得的章动角曲线和误差曲线，图7为求得的空间章动角速率曲线和误差曲线。本章求解翻滚轴方向向量参数的方法采用的是带有遗忘因子的最小二乘递推算法，因此，曲线的变化有一收敛的过程。从图5中的曲线变化可知方法在第15帧时收敛，得到的翻滚轴公共点参数误差在2m以内，翻滚轴方向向量参数误差在0.02以内。由图6可知，章动角收敛后的估计误差小于0.5°。由图7可知，空间章动角速率收敛后的估计误差小于2°/s。

Claims (2)

1.一种空间翻滚火箭箭体的运动参数估计方法，其特征在于：针对空间翻滚运动状态下的火箭箭体，利用激光成像雷达获得点云数据，通过随机一致性采样算法求得自旋轴，随后采用带有遗忘因子的最小二乘法求得翻滚轴参数和章动角，接着求解得出绕翻滚轴的空间章动角速率；所述方法具体步骤如下：

步骤(1)、利用激光成像雷达获取第K帧的目标点云Q_K；

步骤(2)、对第K帧目标点云Q_K进行点云表面法向量估计，得到每个点对应的法向量n_K{n_K,1,n_K,2,…,n_K,N}；

步骤(3)、利用目标点云Q_K和法向量n_K{n_K,1,n_K,2,…,n_K,N}采用随机采样一致性算法进行自旋轴的估计；具体实现如下：

步骤3.1：从目标点云Q_K中随机选取2个点{Q_K,i,Q_K,j|i,j∈[1,N]且i≠j}及其对应的法向量n_K,i和n_K,j；

步骤3.2：由点Q_K,i和Q_K,j及对应的法向量n_K,i和n_K,j，求解出点Q_K,i和Q_K,j所在的圆柱体模型的参数，得到圆柱体的圆柱轴l以及其半径r；

步骤3.3：对输入点云数据Q_K中的每一个点{Q_K,d|d∈[1,N]}，计算点Q_K,d到估计的圆柱体模型的圆柱轴l的距离r_d；设置阈值δ，当满足r_d∈[r-δ,r+δ]时，点Q_K,d被记作为内点，否则点Q_K,d被记作为外点；

步骤3.4：统计所有内点的个数，判断其是否大于设置的最小数目值，如果是，则记录求得的圆柱体模型参数，并存储所有内点作为分割结果，如果不是，则跳到步骤3.1进行下一轮计算；

步骤3.5：重复步骤3.1到步骤3.4共Z次，从中选取内点个数最多的一次，该次求得的圆柱体模型参数即为最符合数据的模型参数，存储的内点为最后的分割数据；此时求得的圆柱轴l即为第K帧自旋轴l_K；

步骤(4)、当K＜2时，K＝K+1并回到步骤1；当K≥2时，对翻滚轴m_K进行估计，采用带有遗忘因子的最小二乘递推算法进行求解，得到翻滚轴的公共点m_K,0(m_a,m_b,m_c)和方向向量m_K,dir(m_dir,x,m_dir,y,m_dir,z)；

具体实现如下：

步骤4.1：由第K-1帧和第K帧的自旋轴可确定与翻滚轴相交的公共点，l_K-1和l_K分别表示K-1时刻和K时刻的火箭箭体的自旋轴，线段pq与l_K-1、l_K垂直，点p和点q为相邻两帧自旋轴之间的最近点；此时最近点可近似看作为翻滚轴公共点；

已知l_K-1和l_K的参数方程：

式中：W_K-1和W_K分别为自旋轴l_K-1和l_K上一点，l_K-1,dir和l_K,dir分别为自旋轴l_K-1和l_K的方向向量；

点p和点q表示为：

式中：t_p和t_q分别为点p和点q的参数；

线段pq与l_K-1、l_K垂直，其点乘为0，如式(3)所示，将式(2)代入式(3)中，联立求解可得，如式(4)所示；

式中：a,b,c,d,e如下表示：

将式(4)代入到式(2)中，求得点p和点q的坐标轴值，选取点p作为翻滚轴上的公共点m_K,0；

步骤4.2：自旋轴l_K和翻滚轴m_K相交于点m_K,0，由向量的夹角公式可得：

m_dir,x×l_K,x+m_dir,y×l_K,y+m_dir,z×l_K,z＝cos(θ_K) (6)

式中l_K(l_K,x,l_K,y,l_K,z)和m_K,dir(m_dir,x,m_dir,y,m_dir,z)分别表示自旋轴l_K和翻滚轴m_K的方向向量，θ_K为章动角；

将式(6)两边同时除以cos(θ_K)，0≤θ_K＜π/2得到：

用矩阵表示，可得：

式中：ψ_K＝[l_K,x l_K,y l_K,z]^T，y_K＝1；

步骤4.3：按照式(8)求解得到协方差矩阵P_K，增益矩阵G_K，其中ρ为遗忘因子；

步骤4.4：将求得的P_K和G_K代入式(10)，求解出K时刻的参数η_K；

步骤4.5：将求得的参数η_K单位化，得到翻滚轴m_K的方向向量m_K,dir；

步骤(5)、利用自旋轴l_K和翻滚轴m_K的夹角公式，求解得到章动角θ_K；求解公式如下

；

步骤(6)、利用自旋轴l_K-1和l_K求得其投影线l_K-1′和l_K′，计算l_K-1′和l_K′的夹角，除以相邻帧之间的时间，得到空间章动角速率Ω_K；具体实现方法为：

设平面B为一垂直于翻滚轴m_K的平面，将自旋轴l_K-1和l_K分别投影到平面B上，得到两条投影线l_K-1′和l_K′，投影线l_K-1′和l_K′的夹角υ_K为相邻帧间的空间章动角度，0≤υ_K＜π/2，将空间章动角度除以相邻帧间的时间间隔得到绕翻滚轴旋转的空间章动角速率；相邻帧间的空间章动角度和空间章动角速率的求解如式(12)所示：

式中：m_K,dir表示翻滚轴m_K的方向向量，l_K-1,dir和l_K,dir表示自旋轴l_K-1和l_K的方向向量，l_K-1,dir′和l_K,dir′表示投影线l_K-1′和l_K′的方向向量,υ_K表示相邻帧间的空间章动角度，Δt表示相邻帧间的时间间隔。

2.根据权利要求1所述的空间翻滚火箭箭体的运动参数估计方法，其特征在于，步骤3.2中所述圆柱体的圆柱轴l以及其半径r求解过程如下：

圆柱体模型参数由圆柱轴l(l_dir,l_cen)和半径r表示，从点云集中任意选取两点Q_K,1和Q_K,2，分别计算得到它们的法向量n_K,1和n_K,2，则可确定Q_K,1和Q_K,2所在圆柱体的圆柱轴的方向向量l_dir＝n_K,1×n_K,2；取经过点Q_K,1并垂直l_dir的平面A，将直线Q_K,1+tn_K,1和Q_K,2+tn_K,2投影到平面A，t为直线参数，投影后的交点即为圆柱轴上的一点l_cen；而l_cen和Q_K,1在该平面上的投影点之间的距离为圆柱体的半径r。