CN112184561A - 一种基于多元搜索的火箭级段位姿纠偏方法及其装置 - Google Patents

Abstract

本发明公开了一种基于多元搜索的火箭级段位姿纠偏方法及其装置。该方法包括：将火箭级段在空间坐标系的旋转矩阵和平移矩阵叠加以获得空间坐标转换矩阵；通过测试矩阵计算出火箭级段的测试数据；利用提出的搜索纠偏方法进行测试数据与理论数据的纠偏，并计算纠偏误差，使得纠偏误差在设定阈值以内，验证所提出纠偏方法的准确性；然后根据火箭级段实际测量数据和理论数据，利用所提出纠偏方法进行纠偏并获得纠偏后点坐标。本发明通过实测火箭级段测量值与其理论值的搜索纠偏，完成实测数据的校正，进而进行火箭实测级段位姿偏差的自动补偿，将有效提高火箭级段装配的精度、稳定性和可重复性。

Description

一种基于多元搜索的火箭级段位姿纠偏方法及其装置

技术领域

本发明涉及火箭装配技术领域的一种误差补偿方法，尤其涉及一种基于多元搜索的火箭级段位姿纠偏方法，还涉及一种基于多元搜索的火箭级段位姿纠偏装置。

背景技术

在航天航空制造业中，飞机和火箭等物体由于尺寸非常大，通常采用分段制造，整体装配的制造方法。相对于飞机装配来说，火箭的特征比较明显，其主体主要是由一些圆柱形的部段(通常称之为“级段”)组成。

大型运载火箭属于空间大尺寸物件，其加工制造精度很大程度影响火箭的最终装配精度，但是实践证明，火箭等这类物体的最终质量，很难或者不可能通过提高制造准确度来提高装配精度，有效可行的方法是通过改善和优化装配方法。但由于其直径往往比较大，如我国新一代运载火箭长征-5(CZ-5)的芯级直径约为5m，因此火箭级段对接和装配是很复杂的。现阶段总装过程中，人工对接装配精度不高而且效率低下，实现起来也很困难，很难保证装配的效率和质量。

发明内容

为解决现有的火箭级段位姿纠偏方法存在测量精度低的技术问题，本发明提供一种基于多元搜索的火箭级段位姿纠偏方法及其装置。

本发明采用以下技术方案实现：一种基于多元搜索的火箭级段位姿纠偏方法，其包括以下步骤：

(1)将火箭级段在空间坐标系的旋转矩阵和平移矩阵叠加以获得空间坐标转换矩阵；

(2)先输入所述空间坐标系中三维坐标轴的旋转参数和平移参数至所述空间坐标转换矩阵中以得到不同的多个旋转矩阵以分别作为多个测试矩阵，再利用所述火箭级段的理论数据，通过所述测试矩阵计算出所述火箭级段的测试数据，所述测试数据包括多个纠偏前测量点坐标；

(3)将所述理论数据与所述测试数据进行多元搜索纠偏，且多元搜索纠偏方法包括以下步骤：

设定参数搜索范围和搜索变量；

根据所述参数搜索范围和所述搜索变量，进行逐步逼近搜索并实现第一次搜索，并根据第一次搜索结果对所述参数搜索范围进行优化，并进行下一次搜索；

判断当前搜索的贴合精度是否高于上一次搜索的贴合精度，是则根据当前搜索结果对所述参数搜索范围进行优化，并进行下一次搜索，否则改变搜索变量优化方向并重新优化所述参数搜索范围；

判断所述贴合精度是否满足一个预设精度或迭代参数是否小于一个设定阈值，是则终止搜索，并输出当前迭代结果；其中，所述当前迭代结果包括当前贴合精度和当前搜索变量；

(4)先根据所述当前搜索变量和所述空间坐标转换矩阵，计算出纠偏矩阵，再根据所述纠偏前测量点坐标与所述纠偏矩阵，进行纠偏并获得纠偏后点坐标。

本发明先通过旋转矩阵和平移矩阵叠加获得空间坐标转换矩阵，进而给出空间坐标变换矩阵与坐标旋转参数和平移参数变量之间的对应关系，再利用已知火箭级段理论数据通过测试矩阵计算出测试数据，然后通过搜索纠偏方法逐步逼近搜索，进行测试数据与理论数据的纠偏，并计算纠偏误差，使得纠偏误差在设定阈值以内，从而验证纠偏方法的有效性。最后根据火箭级段实际测量数据和理论数据，利用所提出纠偏方法计算出纠偏矩阵，并将纠偏前测量点坐标通过纠偏矩阵纠偏以补偿实测数据，实现对火箭级段火箭级段位姿纠偏全过程，解决了现有的火箭级段位姿纠偏方法存在测量精度低的技术问题，得到了有效提高测量精度、稳定性和可重复性的技术效果。

作为上述方案的进一步改进，所述搜索变量为实测级段位姿偏差

其中，δ_α、δ_β、δ_γ分别表示所述三维坐标轴的旋转误差，δ_m、δ_n、δ_k分别表示所述三维坐标轴的平移误差；每一次贴合精度的计算公式为：

F(δ)＝∑Δ²(δ_α,δ_β,δ_γ,δ_m,δ_n,δ_k)

式中，F(δ)为所述贴合精度，Δ(δ_α,δ_β,δ_γ,δ_m,δ_n,δ_k)为每一次搜索的搜索点与纠偏点的距离。

进一步地，所述空间坐标转换矩阵的计算公式为：

式中，T表示所述空间坐标转换矩阵，α、β、γ分别表示所述三维坐标轴的三轴各自的旋转量；m、n、k分别表示所述三维坐标轴的三轴各自的平移量。

作为上述方案的进一步改进，所述空间坐标转换矩阵的计算公式为：

T＝T_r+T_s

式中，T表示所述空间坐标转换矩阵；T_s表示所述平移矩阵，T_s＝(m,n,k)^T，m、n、k分别表示所述三维坐标轴的三轴各自的平移量；T_r表示所述旋转矩阵，T_r＝Γ_ZΓ_YΓ_X，Γ_Z、Γ_Y、Γ_X分别表示所述三维坐标轴的三个单轴旋转矩阵。

进一步地，在误差δ_α、δ_β、δ_γ、δ_m、δ_n、δ_k中，对每个误差按照正方向、负方向、0方向进行搜索，每一个方向进行纠偏后都计算贴合精度，并选择最小的贴合精度作为当前纠偏结果进行下一次纠偏计算。

作为上述方案的进一步改进，将理论点集合记作Q_th＝{P_thi,i＝1,2,3,…N}，测量点集合记作Q_me＝{P_mei,i＝1,2,3,…N}，测量点P_mei对应理论点P_t′_hi，纠偏点集合记作Q_ma＝{P_mai,i＝1,2,3,…N}，N为离散点个数；所述贴合精度的计算公式为：

F(δ)＝∑(P_maij-P′_thij)²i＝1,2,3,…N,j＝x,y,z

式中，F(δ)为所述贴合精度。

作为上述方案的进一步改进，所述参数搜索范围包括旋转参数搜索范围和平移参数搜索范围；所述平移参数搜索范围为100um，所述旋转参数搜索范围为0.1°。

进一步地，所述纠偏方法具体包括以下步骤：

步骤1：读取理论点坐标集Q_th，并计算测试点坐标集

执行步骤2；

步骤2：设定最大迭代次数N，贴合精度F；存储初始搜索变量

并计算初始搜索矩阵T⁰，设定迭代次数K为0，执行步骤3；

步骤3：迭代次数K增加一次，执行步骤4；

步骤4：搜索最优变量

并计算最优变换矩阵T^K，执行步骤5；

步骤5：计算测试变换点集：

执行步骤6；

步骤6：计算变换后贴合精度

执行步骤7；

步骤7：判断F(δ^K)是否小于F，是则执行步骤8，否则执行步骤9；

步骤8：输出所述当前贴合精度和所述当前位姿偏差，并终止纠偏；

步骤9：判断k是否大于N，是则执行步骤8，否则执行步骤10；

步骤10：判断F(δ^K)是否小于F(δ^K-1)，是则执行步骤11，否则执行步骤12；

步骤11：继续当前方向优化所述搜索变量，执行步骤3；

步骤12：改变搜索变量优化方向并重新优化所述参数搜索范围，执行步骤3。

作为上述方案的进一步改进，还计算纠偏补偿后的试验误差，并将所述试验误差与现有误差比对；在所述试验误差小于所述现有误差时，判定补偿有效，否则判定补偿无效。

本发明还提供一种基于多元搜索的火箭级段位姿纠偏装置，其应用于上述任意所述的基于多元搜索的火箭级段位姿纠偏方法中，其包括：

转换矩阵获取模块，其用于将火箭级段在空间坐标系的旋转矩阵和平移矩阵叠加以获得空间坐标转换矩阵；

测试数据获取模块，其用于先输入所述空间坐标系中三维坐标轴的旋转参数和平移参数至所述空间坐标转换矩阵中以得到不同的多个旋转矩阵以分别作为多个测试矩阵，再利用所述火箭级段的理论数据，通过所述测试矩阵计算出所述火箭级段的测试数据，所述测试数据包括多个纠偏前测量点坐标；

纠偏搜索模块，其用于将所述理论数据与所述测试数据进行纠偏，且包括设定单元、逐步逼近搜索单元、判断单元一以及判断单元二；所述设定单元用于设定参数搜索范围和搜索变量；所述逐步逼近搜索单元用于根据所述参数搜索范围和所述搜索变量，进行逐步逼近搜索并实现第一次搜索，并根据第一次搜索结果对所述参数搜索范围进行优化，并进行下一次搜索；所述判断单元一用于判断当前搜索的贴合精度是否高于上一次搜索的贴合精度，是则根据当前搜索结果对所述参数搜索范围进行优化，并进行下一次搜索，否则改变搜索变量优化方向并重新优化所述参数搜索范围；所述判断单元二用于判断所述贴合精度是否满足一个预设精度或迭代参数是否小于一个设定阈值，是则终止搜索，并输出当前迭代结果；其中，所述当前迭代结果包括当前贴合精度和当前搜索变量；

纠偏模块，其用于先根据所述当前搜索变量和所述空间坐标转换矩阵，计算出纠偏矩阵，再根据所述纠偏前测量点坐标与所述纠偏矩阵，进行纠偏并获得纠偏后点坐标。

相较于现有的火箭级段位姿纠偏方法，本发明的基于多元搜索的火箭级段位姿纠偏方法及其装置具有以下有益效果：

1、该基于多元搜索的火箭级段位姿纠偏方法，其先旋转矩阵和平移矩阵叠加获得空间坐标转换矩阵，进而给出空间坐标变换矩阵与坐标旋转参数和平移参数变量之间的对应关系，再利用已知火箭级段理论数据通过测试矩阵计算出的测试数据，然后通过搜索纠偏方法逐步逼近搜索，进行测试数据与理论数据的纠偏，并计算纠偏误差，使得纠偏误差在设定阈值以内，从而验证纠偏方法的有效性。最后根据火箭级段实际测量数据和理论数据，利用所提出纠偏方法计算出纠偏矩阵，并将纠偏前测量点坐标通过纠偏矩阵纠偏以补偿实测数据，实现对火箭级段火箭级段位姿纠偏全过程。由于通过实测与其理论的精确纠偏，完成实测数据的校正，进而进行火箭实测级段位姿偏差的自动补偿，将有效提高火箭级段装配的精度、稳定性和可重复性。

2、该基于多元搜索的火箭级段位姿纠偏方法，其在纠偏测试数据与理论数据时，先读取理论点坐标集并计算出测试点坐标集，再设定搜索范围，然后根据搜索范围，采用逐步逼近搜索的方法进行第一次搜索并保存结果，然后根据搜索结果对搜索范围进行优化，再进行下一次搜索，若下一次搜索的贴合精度高于上一次，则继续对搜索范围进行优化，再进行下一次搜索，若搜索贴合精度不高于上一次，则改变搜索变量优化方向重新优化搜索范围，再进行搜索，直至贴合精度满足要求或者达到迭代次数，搜索终止。而搜索过程中，贴合精度用所有搜索点与纠偏点的距离的平方和进行表示，而该精度反映了纠偏后的离散点与原始点的贴合程度，使数据更加贴近实际，提高测量的准确性。而贴合精度所涉及的每个变量均按照+、-、0三个方向进行搜索，则每一次搜索方向共有729个，每一个方向进行纠偏后都进行贴合精度的计算，并选择最小的贴合精度作为当前纠偏结果进行下一次纠偏计算，逐步迭代进行，直至贴合精度满足要求或者迭代参数小于设定阈值，结束本次搜索，并输出本次迭代结果。如此，这样的纠偏搜索会大大提升贴合精度，进而能够进一步提高测量数据的准确度，实现高精度补偿。

3、该基于多元搜索的火箭级段位姿纠偏装置，其有益效果与上述基于多元搜索的火箭级段位姿纠偏方法的有益效果相同，在此不再做赘述。

附图说明

图1为本发明实施例1的基于多元搜索的火箭级段位姿纠偏方法的流程图。

图2为本发明实施例1的基于多元搜索的火箭级段位姿纠偏方法所对应的运载火箭级段模型图。

图3为本发明实施例1的基于多元搜索的火箭级段位姿纠偏方法所对应的级段自动对接模型。

图4为本发明实施例2的基于多元搜索的火箭级段位姿纠偏方法中的搜索流程图。

图5为本发明实施例2的基于多元搜索的火箭级段位姿纠偏方法中纠偏前后测量点的对比示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

实施例1

请参阅图1、图2以及图3，本实施例提供了一种基于多元搜索的火箭级段位姿纠偏方法。在大型运载火箭级段在进行对接装配时，可以看成是两个相同直径的圆柱筒进行自动对接，一般先固定一个级段，然后移动另外一个级段，通过不断旋转和移动来实现级段之间的对接。这里不妨把固定的级段称为固定件，需要移动和调整的级段称为移动件。可通过级段的一些特征点来确定级段的空间位姿。可以看出，级段在对接过程中会产生位姿偏差，记为

其中δ_α,δ_β,δ_γ分别表示坐标轴方向的旋转偏差，δ_m,δ_n,δ_k表示坐标轴方向的平移偏差。级段位姿偏差不仅会影响火箭级段之间装配精度，还会对火箭整体质量产生重要影响，因此需要对级段位姿偏差进行实时调整。基于此，该火箭级段位姿纠偏方法包括以下这些步骤。

(1)将火箭级段在空间坐标系的旋转矩阵和平移矩阵叠加以获得空间坐标转换矩阵。在本实施例中，利用空间几何解析法，给出空间坐标变换矩阵与坐标旋转参数和平移参数变量之间的对应关系。空间坐标转换矩阵T由旋转矩阵T_r和平移矩阵T_s组成，空间坐标转换矩阵T的计算公式为：

T＝T_r+T_s

式中，T表示空间坐标转换矩阵，T_s表示平移矩阵，而且T_s＝(m,n,k)^T，m、n、k分别表示三维坐标轴的三轴各自的平移量。T_r表示旋转矩阵，多轴旋转可以看作单轴旋转的叠加，且旋转叠加结果与旋转轴顺序相关，以依次旋转x、y、z轴为例，空间坐标系旋转矩阵T_r＝Γ_ZΓ_YΓ_X，Γ_Z、Γ_Y、Γ_X分别表示三维坐标轴的三个单轴旋转矩阵，可分别表示如下：

式中，α、β、γ分别表示三维坐标轴的三轴各自的旋转量。

因此，空间坐标转换矩阵的计算公式为：

(2)先输入空间坐标系中三维坐标轴的旋转参数和平移参数至空间坐标转换矩阵中以得到不同的多个旋转矩阵以分别作为多个测试矩阵，再利用火箭级段的理论数据，通过测试矩阵计算出火箭级段的测试数据，测试数据包括多个纠偏前测量点坐标。在本实施例中，利用已知火箭级段理论数据(包括理论点坐标和理论点法向量)，通过测试矩阵计算出测试数据。火箭级段理论数据，就是火箭级段上的一些点的坐标和单位法向量，这些数据可通过文献进行查阅，本实施例中部分点坐标信息举例如表1所示。

表1移动级段部分理论点坐标表

x(mm)	y(mm)	z(mm)
			0	0	0
5000	0	0
			0	5000	0
-5000	0	0
			0	-5000	0
5000	0	5000
			0	5000	5000
-5000	0	5000
			0	-5000	5000
5000	0	10000
			0	5000	10000
-5000	0	10000
			0	-5000	10000
0	0	10000

输入坐标轴x、y、z旋转和平移参数(0.03，0.06，0.1，1，10，50)，根据空间坐标转换矩阵的计算公式可计算坐标变换矩阵，如下：

可通过改变旋转参数和平移参数，进而计算不同旋转矩阵，该旋转矩阵都被视为测试矩阵。测试矩阵的变量是人为设定且已知，其用于验证后续纠偏算法的正确性。

点坐标变换计算举例：

实际就是矩阵乘法，假设点P₀(x₀,y₀,z₀)，则其经过变换矩阵后，得到点P₁(x₁,y₁,z₁)，计算如下：

即：T(x₀,y₀,z₀,1)^T＝(x₁,y₁,z₁,1)^T。

以上述测试矩阵T_test为例，点坐标经过矩阵变换后，得到待纠偏的测试点坐标，如表2所示。

表2待纠偏的测试点坐标表

(3)将理论数据与测试数据进行多元搜索纠偏，且多元搜索纠偏方法包括以下这些步骤。

首先，设定参数搜索范围和搜索变量。旋转参数单位为弧度，为统一标准，将角度转换为弧长，采用计算如下：

δ_S＝R*δ_α

为满足精度要求，R值应大于等于测量点所在圆周半径的最大值R_max，可以选取R_max为500mm，旋转参数搜索范围可以设定为0.1°。

其次，根据参数搜索范围和搜索变量，进行逐步逼近搜索并实现第一次搜索，并根据第一次搜索结果对参数搜索范围进行优化，并进行下一次搜索。

然后，判断当前搜索的贴合精度是否高于上一次搜索的贴合精度，是则根据当前搜索结果对参数搜索范围进行优化，并进行下一次搜索，否则改变搜索变量优化方向并重新优化参数搜索范围。

最后，判断贴合精度是否满足一个预设精度或迭代参数是否小于一个设定阈值，是则终止搜索，并输出当前迭代结果；其中，当前迭代结果包括当前贴合精度和当前搜索变量。

在本实施例中，采用逐步逼近搜索的方法进行第一次搜索并保存结果，然后根据搜索结果对搜索范围进行优化，再进行下一次搜索，若下一次搜索的贴合精度高于上一次，则继续对搜索范围进行优化，再进行下一次搜索，若搜索贴合精度不高于上一次，则改变搜索变量优化方向重新优化搜索范围，再进行搜索，直至贴合精度满足要求或者达到迭代次数，搜索终止。

其中，搜索变量为实测级段位姿偏差

其中，δ_α、δ_β、δ_γ分别表示三维坐标轴的旋转误差，δ_m、δ_n、δ_k分别表示三维坐标轴的平移误差。每一次贴合精度的计算公式为：

F(δ)＝∑Δ²(δ_α,δ_β,δ_γ,δ_m,δ_n,δ_k)

式中，F(δ)为贴合精度，Δ(δ_α,δ_β,δ_γ,δ_m,δ_n,δ_k)为每一次搜索的搜索点与纠偏点的距离，即每一次贴合精度用所有搜索点与纠偏点的距离的平方和进行表示，该贴合精度反映了纠偏后的离散点与原始点的贴合程度，符合纠偏定义。

由于实际测量存在误差，导致搜索点与原理论点并非一一对应关系，但又由于测量误差一般不会太大，因此每一搜索点实际对应的匹配点与初始对应的理论点偏差不会太大，应还在理论点附近，因此采用窗口平均的方法获得搜索点对应的理论点。设定窗口大小N(N为正整数)，然后选取该理论点前N个理论点和后N个理论点，再进行加权平均，得到该理论点的坐标，本实施例以N＝2为例，计算公式如下：

其中，P_ni表示点P_n在各个轴的分量，w_n表示点P_n的加权系数，与采样点序列变化趋势相关。

搜索最优变量，采用二分法思想，逐步缩小搜索变量，逐步逼近最优结果，直至满足搜索精度要求。在误差δ_α、δ_β、δ_γ、δ_m、δ_n、δ_k中，对每个误差按照正方向(+)、负方向(-)、0方向进行搜索，这样每一次搜索方向共有3⁶＝729个，每一个方向进行纠偏后都计算贴合精度，并选择最小的贴合精度作为当前纠偏结果进行下一次纠偏计算，逐步迭代进行，直至贴合精度满足要求或者迭代参数小于设定阈值，结束本次搜索，并输出本次迭代结果。

(4)先根据当前搜索变量和空间坐标转换矩阵，计算出纠偏矩阵，再根据纠偏前测量点坐标与纠偏矩阵，进行纠偏并获得纠偏后点坐标。在本实施例中，纠偏矩阵T_m与待纠偏点坐标计算得到纠偏后的点坐标，如表3所示：

表3纠偏后的测试点坐标表

在本实施例中，还计算纠偏补偿后的试验误差，并将试验误差与现有误差比对；在试验误差小于现有误差时，判定补偿有效，否则判定补偿无效。将理论点集合记作Q_th＝{P_thi,i＝1,2,3,…N}，测量点集合记作Q_me＝{P_mei,i＝1,2,3,…N}，测量点P_mei对应理论点P′_thi，纠偏点集合记作Q_ma＝{P_mai,i＝1,2,3,…N}，N为离散点个数。测量时，测量点按照理论点序号进行测量。

理论上来讲，测量点按照既定序号进行测量，测量点集与理论点集中的点按照序号一一对应，但是由于测量存在误差，导致测量点集合与理论点集合一一对应存在偏差，因此为了克服这种不对应造成的误差，采用窗口平均的方法，获得测量点P_mei对应的理论点P′_thi，然后可计算纠偏距离平方和如下：

F(δ)＝∑(P_maij-P′_thij)²i＝1,2,3,…N,j＝x,y,z

式中，F(δ)为贴合精度。

综上所述，本实施例的基于多元搜索的火箭级段位姿纠偏方法具有以下优点：

1、该基于多元搜索的火箭级段位姿纠偏方法，其先旋转矩阵和平移矩阵叠加获得空间坐标转换矩阵，进而给出空间坐标变换矩阵与坐标旋转参数和平移参数变量之间的对应关系，再利用已知火箭级段理论数据通过测试矩阵计算出的测试数据，然后通过搜索纠偏方法逐步逼近搜索，进行测试数据与理论数据的纠偏，并计算纠偏误差，使得纠偏误差在设定阈值以内，从而验证纠偏方法的有效性，最后根据纠偏结果和空间坐标转换矩阵，计算出纠偏矩阵，并将纠偏前测量点坐标通过纠偏矩阵纠偏以补偿实测数据，实现对火箭级段火箭级段位姿纠偏全过程。由于通过实测与其理论的精确纠偏，完成实测数据的校正，进而进行火箭实测级段位姿偏差的自动补偿，将有效提高火箭级段装配的精度、稳定性和可重复性。

2、该基于多元搜索的火箭级段位姿纠偏方法，其在纠偏测试数据与理论数据时，先读取理论点坐标集并计算出测试点坐标集，再设定搜索范围，然后根据搜索范围，采用逐步逼近搜索的方法进行第一次搜索并保存结果，然后根据搜索结果对搜索范围进行优化，再进行下一次搜索，若下一次搜索的贴合精度高于上一次，则继续对搜索范围进行优化，再进行下一次搜索，若搜索贴合精度不高于上一次，则改变搜索变量优化方向重新优化搜索范围，再进行搜索，直至贴合精度满足要求或者达到迭代次数，搜索终止。而搜索过程中，贴合精度用所有搜索点与纠偏点的距离的平方和进行表示，而该精度反映了纠偏后的离散点与原始点的贴合程度，使数据更加贴近实际，提高测量的准确性。而贴合精度所涉及的每个变量均按照+、-、0三个方向进行搜索，则每一次搜索方向共有729个，每一个方向进行纠偏后都进行贴合精度的计算，并选择最小的贴合精度作为当前纠偏结果进行下一次纠偏计算，逐步迭代进行，直至贴合精度满足要求或者迭代参数小于设定阈值，结束本次搜索，并输出本次迭代结果。如此，这样的纠偏搜索会大大提升贴合精度，进而能够进一步提高测量数据的准确度，实现高精度补偿。

实施例2

请参阅图4，本实施例提供了一种基于多元搜索的火箭级段位姿纠偏方法，该方法在实施例1的基础上进一步细化。其中，纠偏方法具体包括以下这些步骤，即步骤1-12。

步骤1：读取理论点坐标集Q_th，并计算测试点坐标集

执行步骤2；

步骤2：设定最大迭代次数N，贴合精度F；存储初始搜索变量

并计算初始搜索矩阵T⁰，设定迭代次数K为0，执行步骤3；

步骤3：迭代次数K增加一次，执行步骤4；

步骤4：搜索最优变量

并计算最优变换矩阵T^K，执行步骤5；

步骤5：计算测量变换点集：

执行步骤6；

步骤6：计算变换后贴合精度

执行步骤7；

步骤7：判断F(δ^K)是否小于F，是则执行步骤8，否则执行步骤9；

步骤8：输出所述当前贴合精度和所述当前位姿偏差，并终止纠偏；

步骤9：判断k是否大于N，是则执行步骤8，否则执行步骤10；

步骤10：判断F(δ^K)是否小于F(δ^K-1)，是则执行步骤11，否则执行步骤12；

步骤11：继续当前方向优化所述搜索变量，执行步骤3；

步骤12：改变搜索变量优化方向并重新优化所述参数搜索范围，执行步骤3。

请参阅图5，该火箭级段火箭级段位姿纠偏方法中的纠偏方法和补偿效果分别用Visual Studio和Matlab编写程序进行了验证。可以从图中看出，该方法可以完成测数据的校正，而且可以实现对火箭实测级段位姿偏差的自动补偿。

实施例3

本实施例提供了一种基于多元搜索的火箭级段位姿纠偏装置，该装置应用于实施例1或2中的基于多元搜索的火箭级段位姿纠偏方法中。其中，该装置包括转换矩阵获取模块、测试数据获取模块、纠偏搜索模块以及纠偏模块。

转换矩阵获取模块用于将火箭级段在空间坐标系的旋转矩阵和平移矩阵叠加以获得空间坐标转换矩阵。转换矩阵获取模块主要是利用空间几何解析法，给出空间坐标变换矩阵与坐标旋转参数和平移参数变量之间的对应关系。测试数据获取模块用于先输入空间坐标系中三维坐标轴的旋转参数和平移参数至空间坐标转换矩阵中以得到不同的多个旋转矩阵以分别作为多个测试矩阵，再利用火箭级段的理论数据，通过测试矩阵计算出火箭级段的测试数据，测试数据包括多个纠偏前测量点坐标。测试数据获取模块则是利用已知火箭级段理论数据，通过测试矩阵计算出测试数据

纠偏搜索模块用于将理论数据与测试数据进行纠偏，而且包括设定单元、逐步逼近搜索单元、判断单元一以及判断单元二。设定单元用于设定参数搜索范围和搜索变量。逐步逼近搜索单元用于根据参数搜索范围和搜索变量，进行逐步逼近搜索并实现第一次搜索，并根据第一次搜索结果对参数搜索范围进行优化，并进行下一次搜索。判断单元一用于判断当前搜索的贴合精度是否高于上一次搜索的贴合精度，是则根据当前搜索结果对参数搜索范围进行优化，并进行下一次搜索，否则改变搜索变量优化方向并重新优化参数搜索范围。判断单元二用于判断贴合精度是否满足一个预设精度或迭代参数是否小于一个设定阈值，是则终止搜索，并输出当前迭代结果。其中，当前迭代结果包括当前贴合精度和当前搜索变量，纠偏搜索模块所应用的纠偏方法可以采用实施例1或实施例2中所提及的纠偏搜索方法。纠偏模块用于先根据当前搜索变量和空间坐标转换矩阵，计算出纠偏矩阵，再根据纠偏前测量点坐标与纠偏矩阵，进行纠偏并获得纠偏后点坐标。纠偏模块利用实施例1或2中步骤(3)用的纠偏方法进行实测数据的补偿，并计算补偿后的误差，然后与现有误差比对，验证补偿方法的有效性。

实施例4

本实施例提供了一种计算机终端，其包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序。处理器执行程序时实现实施例1或2的基于多元搜索的火箭级段位姿纠偏方法的步骤。

实施例1或2的基于多元搜索的火箭级段位姿纠偏方法在应用时，可以软件的形式进行应用，如设计成独立运行的程序，安装在计算机终端上，计算机终端可以是电脑、智能手机、控制系统以及其他物联网设备等。实施例1或2的基于多元搜索的火箭级段位姿纠偏方法也可以设计成嵌入式运行的程序，安装在计算机终端上，如安装在单片机上。

实施例5

本实施例提供了一种计算机可读存储介质，其上存储有计算机程序。程序被处理器执行时，实现实施例1或2的基于多元搜索的火箭级段位姿纠偏方法的步骤。

实施例1或2的基于多元搜索的火箭级段位姿纠偏方法在应用时，可以软件的形式进行应用，如设计成计算机可读存储介质可独立运行的程序，计算机可读存储介质可以是U盘，设计成U盾，通过U盘设计成通过外在触发启动整个方法的程序。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于多元搜索的火箭级段位姿纠偏方法，其特征在于，其包括以下步骤：

(1)将火箭级段在空间坐标系的旋转矩阵和平移矩阵叠加以获得空间坐标转换矩阵；

(2)先输入所述空间坐标系中三维坐标轴的旋转参数和平移参数至所述空间坐标转换矩阵中以得到不同的多个旋转矩阵以分别作为多个测试矩阵，再利用所述火箭级段的理论数据，通过所述测试矩阵计算出所述火箭级段的测试数据，所述测试数据包括多个纠偏前测量点坐标；

(3)将所述理论数据与所述测试数据进行多元搜索纠偏，且多元搜索纠偏方法包括以下步骤：

设定参数搜索范围和搜索变量；

根据所述参数搜索范围和所述搜索变量，进行逐步逼近搜索并实现第一次搜索，并根据第一次搜索结果对所述参数搜索范围进行优化，并进行下一次搜索；

判断当前搜索的贴合精度是否高于上一次搜索的贴合精度，是则根据当前搜索结果对所述参数搜索范围进行优化，并进行下一次搜索，否则改变搜索变量优化方向并重新优化所述参数搜索范围；

判断所述贴合精度是否满足一个预设精度或迭代参数是否小于一个设定阈值，是则终止搜索，并输出当前迭代结果；其中，所述当前迭代结果包括当前贴合精度和当前搜索变量；

(4)先根据所述当前搜索变量和所述空间坐标转换矩阵，计算出纠偏矩阵，再根据所述纠偏前测量点坐标与所述纠偏矩阵，进行纠偏并获得纠偏后点坐标。

2.如权利要求1所述的基于多元搜索的火箭级段位姿纠偏方法，其特征在于，所述搜索变量为实测级段位姿偏差

其中，δ_α、δ_β、δ_γ分别表示所述三维坐标轴的旋转误差，δ_m、δ_n、&_k分别表示所述三维坐标轴的平移误差；每一次贴合精度的计算公式为：

F(δ)＝∑Δ²(δ_α，δ_β，δ_γ，δ_m，δ_n，&_k)

式中，F(δ)为所述贴合精度，Δ(δ_α，δ_β，δ_γ，δ_m，δ_n，&_k)为每一次搜索的搜索点与纠偏点的距离。

3.如权利要求2所述的基于多元搜索的火箭级段位姿纠偏方法，其特征在于，所述空间坐标转换矩阵的计算公式为：

式中，T表示所述空间坐标转换矩阵，α、β、γ分别表示所述三维坐标轴的三轴各自的旋转量；m、n、k分别表示所述三维坐标轴的三轴各自的平移量。

4.如权利要求1所述的基于多元搜索的火箭级段位姿纠偏方法，其特征在于，所述空间坐标转换矩阵的计算公式为：

T＝T_r+T_s

式中，T表示所述空间坐标转换矩阵；T_s表示所述平移矩阵，T_s＝(m，n，k)^T，m、n、k分别表示所述三维坐标轴的三轴各自的平移量；T_r表示所述旋转矩阵，T_r＝Γ_ZΓ_YΓ_X，Γ_Z、Γ_Y、Γ_X分别表示所述三维坐标轴的三个单轴旋转矩阵。

5.如权利要求2所述的基于多元搜索的火箭级段位姿纠偏方法，其特征在于，在误差δ_α、δ_β、δ_γ、δ_m、δ_n、&_k中，对每个误差按照正方向、负方向、0方向进行搜索，每一个方向进行纠偏后都计算贴合精度，并选择最小的贴合精度作为当前纠偏结果进行下一次纠偏计算。

6.如权利要求1所述的基于多元搜索的火箭级段位姿纠偏方法，其特征在于，将理论点集合记作Q_th＝{P_thi，i＝1，2，3，...N}，测量点集合记作Q_me＝{P_mei，i＝1，2，3，...N}，测量点P_mei对应理论点P′_thi，纠偏点集合记作Q_ma＝{P_mai，i＝1，2，3，...N}，N为离散点个数；所述贴合精度的计算公式为：

F(δ)＝∑(P_maij-P′_thij)² i＝1，2，3，...N，j＝x，y，z

式中，F(δ)为所述贴合精度。

7.如权利要求1所述的基于多元搜索的火箭级段位姿纠偏方法，其特征在于，所述参数搜索范围包括旋转参数搜索范围和平移参数搜索范围；所述平移参数搜索范围为100um，所述旋转参数搜索范围为0.1°。

8.如权利要求1所述的航空精密的火箭级段火箭级段位姿纠偏方法，其特征在于，所述纠偏方法具体包括以下步骤：

步骤1：读取理论点坐标集Q_th，并计算测试点坐标集

执行步骤2；

步骤2：设定最大迭代次数N，贴合精度F；存储初始搜索变量

并计算初始搜索矩阵T⁰，设定迭代次数K为0，执行步骤3；

步骤3：迭代次数K增加一次，执行步骤4；

步骤4：搜索最优变量

并计算最优变换矩阵T^K，执行步骤5；

步骤5：计算测试变换点集：

执行步骤6；

步骤6：计算变换后贴合精度

执行步骤7；

步骤7：判断F(δ^K)是否小于F，是则执行步骤8，否则执行步骤9；

步骤8：输出所述当前贴合精度和所述当前位姿偏差，并终止纠偏；

步骤9：判断K是否大于N，是则执行步骤8，否则执行步骤10；

步骤10：判断F(δ^K)是否小于F(δ^K-1)，是则执行步骤11，否则执行步骤12：

步骤11：继续当前方向优化所述搜索变量，执行步骤3；

步骤12：改变搜索变量优化方向并重新优化所述参数搜索范围，执行步骤3。

9.如权利要求1所述的基于多元搜索的火箭级段位姿纠偏方法，其特征在于，还计算纠偏补偿后的试验误差，并将所述试验误差与现有误差比对；在所述试验误差小于所述现有误差时，判定补偿有效，否则判定补偿无效。

10.一种基于多元搜索的火箭级段位姿纠偏装置，其应用于如权利要求1-9中任意一项所述的基于多元搜索的火箭级段位姿纠偏方法中，其特征在于，其包括：

转换矩阵获取模块，其用于将火箭级段在空间坐标系的旋转矩阵和平移矩阵叠加以获得空间坐标转换矩阵；

测试数据获取模块，其用于先输入所述空间坐标系中三维坐标轴的旋转参数和平移参数至所述空间坐标转换矩阵中以得到不同的多个旋转矩阵以分别作为多个测试矩阵，再利用所述火箭级段的理论数据，通过所述测试矩阵计算出所述火箭级段的测试数据，所述测试数据包括多个纠偏前测量点坐标；

纠偏搜索模块，其用于将所述理论数据与所述测试数据进行纠偏，且包括设定单元、逐步逼近搜索单元、判断单元一以及判断单元二；所述设定单元用于设定参数搜索范围和搜索变量；所述逐步逼近搜索单元用于根据所述参数搜索范围和所述搜索变量，进行逐步逼近搜索并实现第一次搜索，并根据第一次搜索结果对所述参数搜索范围进行优化，并进行下一次搜索；所述判断单元一用于判断当前搜索的贴合精度是否高于上一次搜索的贴合精度，是则根据当前搜索结果对所述参数搜索范围进行优化，并进行下一次搜索，否则改变搜索变量优化方向并重新优化所述参数搜索范围；所述判断单元二用于判断所述贴合精度是否满足一个预设精度或迭代参数是否小于一个设定阈值，是则终止搜索，并输出当前迭代结果；其中，所述当前迭代结果包括当前贴合精度和当前搜索变量；

纠偏模块，其用于先根据所述当前搜索变量和所述空间坐标转换矩阵，计算出纠偏矩阵，再根据所述纠偏前测量点坐标与所述纠偏矩阵，进行纠偏并获得纠偏后点坐标。

Images