小天体柔性附着多节点相对位姿估计方法

1.本发明涉及一种多节点相对位姿估计方法，尤其涉及小天体柔性附着多节点相对位姿估计方法，属于深空探测技术领域。


背景技术：

2.近年来，小天体探测逐渐成为深空探测重点领域。目前人类已经开展了23次的小天体探测任务，小天体探测活动对人类了解宇宙和太阳系起源等具有重要的科学意义。小天体附着是开展小天体表面原位探测、采样返回的重要前提。然而，小天体引力场弱，传统刚性附着容易发生倾覆、反弹甚至逃逸，严重威胁着陆安全，进而导致任务失败。
3.为此，在小天体附着任务中，采用柔性附着方式可有效抑制末端残余速度所导致的着陆器发生倾覆、反弹的可能。然而，柔性着陆器尺度大、刚度低，在附着过程中具有一定形变，给柔性着陆器位姿估计带来困难。柔性着陆器的敏感器配置在柔性体局部刚性节点处，通过节点间相对位置信息可以表征柔性体在下降过程中的姿态。然而，柔性着陆器形变导致节点间相对位姿变化，需要对节点间相对位姿进行估计，从而获得节点间相对位置以近似表征着陆器整体姿态。所以，研究节点间相对位置为后续精确控制柔性着陆器整体姿态具有重要意义。


技术实现要素：

4.本发明公开小天体柔性附着多节点相对位姿估计方法要解决的问题是：在柔性附着过程中，由于柔性着陆器形变导致的各节点相对位姿变化，通过各节点携带的光学相机获取图像，根据特征在不同图像中相对几何关系，对节点间相对位姿进行估计，将曲线特征作为观测量，提高节点间相对位姿估计精度，为后续精确控制柔性着陆器姿态提供支撑。
5.本发明的目的是通过下述技术方案实现的。
6.本发明公开小天体柔性附着多节点相对位姿估计方法，建立小天体柔性附着多节点相对位姿估计的特征曲线光学导航观测模型；在同帧下，利用包含信息多的特征曲线构建节点间相对位姿方程，通过解算所述节点间相对位姿方程得到更准确的单应矩阵；归一化单应矩阵，并对归一化后的单应矩阵进行奇异值分解，通过先验信息筛选得到节点间相对位姿，即利用包含信息多的特征曲线提高小天体柔性附着多节点间相对位姿估计精度。此外，本发明通过引入同帧曲线特征信息，对帧间信息进行补充，能够为后续修正帧间信息提供支撑。
7.本发明公开的小天体柔性附着多节点相对位姿估计方法，包括如下步骤：
8.步骤一：建立小天体柔性附着多节点相对位姿估计的特征曲线光学导航观测模型。
9.由于小天体表面具有能产生曲线特征的地貌特征，通过图像处理后获得特征点及曲线信息。所述能产生曲线特征的地貌特征包括陨石坑、岩石地貌特征。
10.特征点是最常见的观测特征，包括sift点、角点、陨石坑中心点三类。导航相机模
型采用小孔成像模型，小天体表面上任一特征点p在i个相机中测量量为
[0011][0012]
式中，u
i
,v
i
表示在第i个相机图像坐标，
c
x
i
,
c
y
i
,
c
z
i
表示在第i个相机坐标系下特征点p的三轴分量。
[0013][0014]
其中，c
i
(q)表示着陆坐标系分别到第i个相机坐标系的转换矩阵。
l
p表示着陆点p在着陆点坐标系的位置矢量。
[0015]
着陆区域为平面，且具有能产生曲线特征的地貌特征均位于该平面上。地貌特征边缘椭圆曲线特征在着陆点坐标系下表示为：
[0016][0017]
其中，矩阵θ表示曲线特征的二次型矩阵，表示着陆点坐标系下曲线特征上任一点，采用齐次坐标表示为
[0018]
结合式(1)和式(2)得地貌曲线特征上任一点
l
x的观测模型为
[0019][0020][0021]
其中，σ表示非零常数因子；c(q)表示着陆点坐标系到相机坐标系的转换矩阵；
l
x满足
l
x＝[
l
x l
y l
z]
t
；由于着陆区域近似为平面，则
l
z＝0。
[0022]
将式(4)改写为式(6)所示的齐次形式。
[0023][0024]
其中，表示地貌图像曲线特征上任一点的齐次坐标，m满足
[0025]
m＝kc(q)t
ꢀꢀ
(7)
[0026][0027]
当σ＝1，地貌曲线特征在图像坐标系中表达为
[0028][0029]
其中，e表示曲线特征的二次型矩阵。
[0030]
根据式(3)、式(6)、和式(9)，计算得到任意曲线特征的观测模型为
[0031]
e＝m
‑
t
θm
‑1ꢀꢀ
(10)
[0032]
以特征曲线的中心点、长短轴、倾角作为观测量，表示为：
[0033][0034][0035][0036][0037]
其中，u
cj
表示特征曲线中心点；u
cj
,v
cj
表示中心点坐标；
c
x
cj
,
c
y
cj
,
c
z
cj
分别表示在相机坐标系下特征曲线中心点的三轴分量；a
j
,b
j
分别表示特征曲线的长半轴和短半轴；(u
mj
,v
mj
)和(u
nj
,v
nj
)分别表示具有曲线特征像的曲线特征的长短轴与像曲线特征的交点坐标；θ
j
表示特征曲线长轴倾角。
[0038]
特征曲线的光学导航观测模型表示为：
[0039][0040]
式(15)为小天体柔性附着多节点相对位姿估计的特征曲线光学导航观测模型。
[0041]
步骤二：在同帧下，利用包含信息多的特征曲线构建节点间相对位姿方程，通过解算所述节点间相对位姿方程得到更准确的单应矩阵。
[0042]
观测特征点p在不同相机本体系下的相对几何关系由式(16)描述。
[0043]
p
j
＝h
ji
p
i
ꢀꢀ
(16)
[0044]
式中，p
i
,p
j
表示在第i,j相机本体下观测特征点，h
ji
表示单应矩阵，满足
[0045][0046]
式中，表示相机i和相机j的相对位置，矩阵表示相机i到相机j相机坐标系转换的方向余弦矩阵，表示相机i与相机j的相对姿态；表示小天体表面在相机i本体系下的方位法向量；表示小天体表面距离探相机i本体系原点的垂直距离。
[0047]
光学相机模型采用小孔成像模型。特征点p在第i和第j相机中图像坐标系下的相对几何关系如式(18)表示。
[0048][0049]
式中，ρ表示非零常数，分别表示相机i、相机j中观测特征点p对应的像点。
[0050]
具有曲线特征的地貌特征q
k
(k＝1,2,3,.....)像曲线在相机i和相机j中图像坐标系满足关系如式(19)表示。
[0051]
[0052]
式中，a
k
,b
k
∈r3×3，为非奇异对称矩阵，表示地貌特征像的边缘曲线。
[0053]
结合式(18)和式(19)，得到：
[0054][0055]
结合式(19)和式(20)，得到式(21)
[0056]
h
jit
b
k
h
ji
＝ξ
i
a
k
ꢀꢀ
(21)
[0057]
其中，ξ
ij
表示非零常数。
[0058]
由于，矩阵a
k
,b
k
,h
ji
均为非奇异矩阵，所以将式(21)变换为式
[0059][0060]
对于不同地貌特征，结合式(22)得到：
[0061][0062]
式中，ξ
12
表示非零常数。
[0063]
结合式(23)，各相机在同一时刻获得图像中任意两对成功匹配的地貌特征特征建立相机本体系下相对位姿约束方程：
[0064][0065]
其中，i,j为正整数且1≤i＜j≤n；矩阵a
i
,a
j
分别表示相机i中两个地貌特征像边缘曲线；矩阵b
i
,b
j
分别表示相机j中两个具有曲线特征像的边缘曲线。
[0066]
相对位姿约束方程式(24)即为在同帧下，利用包含信息多的特征曲线构建的节点间相对位姿方程。
[0067]
结合式(24)，利用kronecker积建立节点间相对位姿的线性方程：
[0068]
jh
ji
＝0
ꢀꢀ
(25)
[0069]
其中，h
ji
表示单应矩阵h
ji
向量化形式，即h
ji
＝(h1,
…
,h9)
t
，矩阵j满足：
[0070][0071]
其中，
[0072][0073]
其中，i为3
×
3的单位矩阵。
[0074]
根据成功匹配具有曲线特征的地貌特征边缘曲线方程，通过计算相对位姿方程得到单应矩阵h
ji
。所述单应矩阵h
ji
相对于根据特征点计算得到的单应矩阵更精确。
[0075]
步骤三：归一化单应矩阵，并对归一化后的单应矩阵进行奇异值分解，通过先验信息筛选得到节点间相对位姿，即利用包含信息多的特征曲线提高小天体柔性附着多节点间相对位姿估计精度。
[0076]
为降低噪声灵敏度，对步骤二得到的节点间单应矩阵进行归一化，得到标准化矩阵为：
[0077]
[0078]
其中，
[0079][0080]
其中，对于曲线匹配的导航方案，(u
i
,v
i
)表示曲线图像中心点；对于特征点匹配的导航方案，(u
i
,v
i
)表示特征点的像点坐标。
[0081]
结合式(28)得到归一化后像曲线a
i
′
,a
j
′
,b
i
′
,b
j
′
，满足关系如下：
[0082][0083]
式中，l
i
,l
j
表示相机i,j中图像特征根据式(28)求得的标准化矩阵。
[0084]
图像特征标准化后，单应矩阵通过反标准化得到，即
[0085][0086]
其中，表示利用归一化变换后特征估计得到的单应矩阵。
[0087]
利用奇异值分解得到式(31)，
[0088]
h
ji
＝u
ji
s
ji
v
jit
ꢀꢀ
(32)
[0089]
结合任一节点安装的激光测距仪输出的高度信息，得到节点间相对位姿如式(33)：
[0090][0091]
其中，ω为比例因子，u1,u3为矩阵u的列向量，r'表示为式(34)。
[0092][0093]
式中，α,β,η,δ表示为式(35)。
[0094][0095]
通过分解得到多个相对位姿结果，根据先验信息筛选得到正确相对位姿，即利用包含信息多的特征曲线提高小天体柔性附着多节点间相对位姿估计精度。
[0096]
有益效果
[0097]
1、本发明公开的小天体柔性附着多节点相对位姿估计方法，在小天体柔性附着过程中，利用同帧下包含信息多的特征曲线构建节点间相对位姿方程，通过解算所述节点间相对位姿方程得到更准确的单应矩阵，通过解算更精确的单应矩阵并筛选得到节点间相对位姿，解决柔性着陆器由于形变带来各节点相对位姿变化问题，进而提高节点间相对位姿估计精度。
[0098]
2、本发明公开的小天体柔性附着多节点相对位姿估计方法，引入同帧曲线特征信息，对帧间信息进行补充，为后续修正帧间信息提供支撑。
附图说明
[0099]
图1为本发明公开的小天体柔性附着多节点相对位姿估计方法的流程图；
[0100]
图2为具体实施案例中三节点柔性着陆器示意图；
[0101]
图3为具体实施案例中三个相机同一时刻图像。
具体实施方式
[0102]
为了更好的说明本发明的目的和优点，下面结合一个实施例和相应附图对发明内容做进一步说明。
[0103]
实施例1：为验证本发明方法的可行性和有益效果，本实施例以三节点柔性着陆器为例。
[0104]
如图1所示，本实施例公开的小天体柔性附着多节点相对位姿估计方法，具体实现步骤如下：
[0105]
步骤一：建立小天体柔性附着多节点相对位姿估计的特征曲线光学导航观测模型。
[0106]
由于小天体表面具有陨石坑、岩石等地貌特征，通过图像处理后可以获得特征点及二次曲线等信息。导航方案采用光学导航，每个节点安装光学相机，其中一个节点安装激光测距仪。以三节点为例，示意图如图2所示。
[0107]
导航相机参数如表1所示。
[0108]
表1导航相机参数
[0109][0110]
特征点是最常见的观测特征，主要包括sift点、角点、陨石坑中心点三类。导航相机模型采用小孔成像模型，小天体表面上任一特征点p在三相机中测量量为
[0111][0112]
式中，u
i
,v
i
表示在第i个相机图像坐标，
c
x
i
,
c
y
i
,
c
z
i
表示在第i个相机坐标系下特征点p的三轴分量。
[0113][0114]
其中，c
i
(q)表示着陆坐标系分别到三个相机坐标系的转换矩阵。
l
p表示着陆点p在着陆点坐标系的位置矢量。
[0115]
着陆区域为平面，且陨石坑、岩石等均位于该平面上。陨石坑或岩石等边缘椭圆曲线特征在着陆点坐标系下表示为：
[0116][0117]
其中，矩阵θ表示陨石坑曲线特征的二次型矩阵，表示着陆点坐标系下陨石坑曲线特征上任一点，采用齐次坐标表示为
[0118]
结合式(36)和式(37)可得在一个尺度因子下着陆平面陨石坑曲线特征上任一点
l
x的观测模型为
[0119][0120][0121]
其中，σ表示非零常数因子，c(q)表示着陆点坐标系到相机坐标系的转换矩阵，
l
x满足
l
x＝[
l
x l
y l
z]
t
，由于着陆区域近似为平面，则
l
z＝0。
[0122]
式(39)可以改写为式(41)所示。
[0123][0124]
其中，表示陨石坑图像曲线特征上任一点的齐次坐标，m满足
[0125]
m＝kc(q)t
ꢀꢀ
(42)
[0126][0127]
当σ＝1，陨石坑曲线特征在图像坐标系中表达为
[0128][0129]
其中，e表示曲线特征的二次型矩阵。
[0130]
根据式(38)，式(41)，和式(44)，可以计算得到任意曲线特征的观测模型为
[0131]
e＝m
‑
t
θm
‑1ꢀꢀ
(45)
[0132]
以特征曲线的中心点、长短轴、倾角作为观测量，可以表示为：
[0133][0134][0135][0136][0137]
其中，u
cj
表示特征曲线中心点；u
cj
,v
cj
表示中心点坐标；
c
x
cj
,
c
y
cj
,
c
z
cj
分别表示在相机坐标系下特征曲线中心点的三轴分量；a
j
,b
j
分别表示特征曲线的长半轴和短半轴；(u
mj v
mj
)和(u
nj v
nj
)分别表示陨石坑像曲线特征的长短轴与像曲线特征的交点坐标；θ
j
表示特征曲线长轴倾角。
[0138]
特征曲线的观测模型可以表示为：
[0139][0140]
对图3中三个相机拍摄的图像的特征曲线进行计算，得到特征曲线的观测模型。
[0141]
步骤二：在同帧下，利用包含信息多的特征曲线构建节点间相对位姿方程，通过解算所述节点间相对位姿方程得到更准确的单应矩阵。
[0142]
观测特征点p在不同相机本体系下的相对几何关系由式描述。
[0143][0144]
式中，h
31
＝h
32
h
21
，h
ij
表示单应矩阵，满足
[0145]
[0146]
式中，表示相机1和相机2的相对位置，矩阵表示相机1到相机2相机坐标系转换的方向余弦矩阵，表示相机1与相机2的相对姿态；表示小天体表面在相机1本体系下的方位法向量；表示小天体表面距离探相机1本体系原点的垂直距离。
[0147]
光学相机模型采用小孔成像模型。特征点p在三个相机中图像坐标系下的相对几何关系如式(53)表示。
[0148][0149]
式中，ρ表示非零常数，分别表示相机1、相机2和相机3中观测特征点p对应的像点。
[0150]
陨石坑q
k
(k＝1,2,3,.....)像曲线在相机1和相机2中图像坐标系满足关系如式(54)表示。
[0151][0152]
式中，a
k
,b
k
,c
k
∈r3×3，为非奇异对称矩阵，表示陨石坑像的边缘曲线。
[0153]
结合式(53)和式(54)，可以得到：
[0154][0155]
结合式(54)和式(55)，得到式(56)
[0156][0157]
其中，ξ
i
,ξ
j
表示非零常数。
[0158]
由于，矩阵a
k
,b
k
,c
k
,h
21
,h
31
均为非奇异矩阵，所以将式(56)变换为式
[0159][0160]
对于不同陨石坑、岩石等，结合式(57)可以得到：
[0161][0162]
式中，ξ
12
,ξ
13
表示非零常数。
[0163]
结合式(58)，3个相机在同一时刻获得3幅图像中任意两对成功匹配的陨石坑等特征建立相机本体系下相对位姿约束方程：
[0164][0165]
根据式(59)和式(51)得到式(60)
[0166][0167]
其中，i,j为正整数且1≤i＜j≤n；矩阵a
i
,a
j
分别表示相机1中两个陨石坑像边缘曲线；矩阵b
i
,b
j
分别表示相机2中两个陨石坑像边缘曲线；矩阵c
i
,c
j
分别表示相机3中两个陨石坑像边缘曲线。
[0168]
结合式(59)，利用kronecker积建立节点间相对位姿的线性方程：
[0169][0170]
其中，h
21
,h
31
分别表示单应矩阵h
21
,h
31
向量化形式，矩阵j,q满足：
[0171][0172]
其中，
[0173][0174]
其中，i为3
×
3的单位矩阵。
[0175]
根据成功匹配陨石坑或者岩石等边缘曲线方程，通过计算相对位姿方程得到单应矩阵h
21
,h
31
,h
32
。
[0176]
根据步骤二中得到的观测模型及像曲线，建立相对位姿方程得到更精确的单应矩阵。
[0177]
步骤三：归一化单应矩阵，并对归一化后的单应矩阵进行奇异值分解，通过先验信息筛选得到节点间相对位姿，即利用包含信息多的特征曲线提高小天体柔性附着多节点间相对位姿估计精度。
[0178]
为降低噪声灵敏度，对步骤二得到的节点间单应矩阵进行归一化，得到标准化矩阵为：
[0179][0180]
其中，
[0181][0182]
其中，对于曲线匹配的导航方案，(u
i
,v
i
)表示曲线图像中心点；对于特征点匹配的导航方案，(u
i
,v
i
)表示特征点的像点坐标。
[0183]
结合式得到归一化后像曲线a
′
i
,a
′
j
,b
′
i
,b
′
j
,c
i
',c
j
'，满足关系如下：
[0184][0185]
式中，l1,l2,l3分别表示相机1，2,3中图像特征根据式(64)求得的标准化矩阵。
[0186]
图像特征标准化后，单应矩阵可以通过反标准化得到，即
[0187][0188]
其中，表示利用归一化变换后特征估计得到的单应矩阵。
[0189]
利用奇异值分解得到式(68)，
[0190][0191]
结合节点1安装的激光测距仪输出的高度信息，可以得到节点间相对位姿如式：
[0192][0193]
其中，ω为比例因子，u1,u3为矩阵u的列向量，r'表示为式(70)。
[0194][0195]
式中，α,β,η,δ表示为式(71)。
[0196][0197]
得到相对位姿如表2，表3所示。
[0198]
表2相机1,2相对位姿4种情况
[0199][0200]
表3相机1,3相对位姿4种情况
[0201]
[0202][0203]
通过分解得到多个相对位姿结果，根据先验信息进行筛选得到正确相对位姿结果如表4，表5所示。
[0204]
表4相机1,2正确相对位姿结果
[0205][0206]
表5相机1,3正确相对位姿结果
[0207][0208]
至此，实现利用包含信息多的特征曲线提高小天体柔性附着多节点间相对位姿估计精度。
[0209]
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。