一种火箭初始方位对准修正方法与流程

1.本发明涉及火箭方位修正的技术领域，具体而言，涉及一种火箭初始方位对准修正方法。


背景技术：

2.固体火箭是一种依靠发动机喷射工作介质所产生的反作用力向前推进的飞行器，具有机动性强、准备时间短等特点，能够满足快速机动、快速部署和快速进入空间的需求，可以广泛应用于军事、民用以及商用的中小型卫星发射任务。
3.火箭起飞后通过初始方位对准得到的初始姿态一般存在误差，如果采用低精度惯组进行自对准，误差则会更大，而在火箭起飞后如果不尽快进行方位修正，经过长时间飞行导航计算后，将会累积大量导航误差，致使火箭不能顺利进入轨道，或导致入轨精度较低，且由于商业火箭低成本的要求，目前在进行初始方位对准时大量采用较低精度的惯组，使得火箭初始方位对准误差很大，箭上计算机计算得到的姿态与火箭实际姿态之间的误差也很大。


技术实现要素：

4.本发明要解决的问题是：提供一种火箭初始方位对准修正方法，根据卫星导航数据和惯组导航数据对实时姿态四元数进行智能修正，通过多次修正消除方位误差，保证火箭顺利进入轨道。
5.为解决上述问题，本发明提供一种火箭初始方位对准修正方法，包括：
6.步骤s1，于火箭起飞后的至少一预设时刻，控制火箭上的箭上计算机持续采集预设数量的卫星导航数据和各所述卫星导航数据对应的一惯组导航数据，并采集所述预设时刻火箭对应的标准弹道速度，以及记录每个所述卫星导航数据对应的一采集时刻；
7.步骤s2，对各所述卫星导航数据进行预处理得到多个有效导航数据；
8.步骤s3，根据各所述有效导航数据的数量和所述预设数量处理得到一第一数量百分比，并判断所述第一数量百分比是否大于预设的一第一标准百分比：
9.若是，则对各所述有效数据进行最小二乘拟合处理得到对应的一第一拟合数据，随后转向步骤s4；
10.若否，则将惯导误差置零并转向步骤s5；
11.步骤s4，根据所述第一拟合数据、所述预设时刻和各所述有效导航数据对应的所述采集时刻得到对应的所述惯导误差；
12.步骤s5，控制所述箭上计算机获取火箭的一实时姿态四元数，根据所述惯导误差、所述标准弹道速度、预设的初始状态数据和所述实时姿态四元数得到一修正后姿态四元数，并控制所述箭上计算机将所述实时姿态四元数替换为所述修正后姿态四元数完成初始方位对准修正。
13.本方案中，考虑到目前进行火箭初始方位对准时普遍采用低精度惯组进行自对
准，导致火箭初始方位对准误差很大，箭上计算机计算得到的姿态与火箭实际姿态之间的误差也很大，因此，本方案中，通过所述箭上计算机持续采集预设数量的所述卫星导航数据和所述惯组导航数据，将各所述卫星导航数据进行过滤筛选后，基于各所述有效数据和所述第一拟合数据求得所述惯导误差，最后通过所述惯导误差、所述标准弹道速度、所述初始状态数据和所述实时姿态四元数求得所述修正后姿态四元数，此时求得的所述修正后姿态四元数已经具备很高的精确度，将其替换掉所述实时姿态四元数即可完成初始方位对准修正，使得火箭顺利进入轨道。
14.进一步的，在火箭起飞后的多个预设时刻，均可以基于本方案进行一次初始方位对准修正，以使得所述实时姿态四元数越来越精准，在多次修正后，便可以消除方位误差，保证火箭顺利进入轨道。
15.优选的，所述步骤s1中，控制所述箭上计算机于所述预设时刻开始，持续采集30个所述卫星导航数据且每间隔0.1秒采集一个所述卫星导航数据和所述卫星导航数据对应的所述惯组导航数据。
16.优选的，采集到的每个所述卫星导航数据均对应有一定位标志，所述步骤s2包括：
17.步骤s21，针对每个所述卫星导航数据，判断所述卫星导航数据对应的所述定位标志是否表征未定位：
18.若是，则删除所述定位标志对应的所述卫星导航数据，随后转向步骤s22；
19.若否，则保留所述定位标志对应的所述卫星导航数据作为第一导航数据，随后转向步骤s22；
20.步骤s22，根据各所述第一导航数据和各所述第一导航数据对应的所述惯组导航数据处理得到多个所述有效导航数据。
21.本方案中，在采集所述卫星导航数据时会关联一个所述定位标志，基于所述定位标志可以来判断所述卫星导航数据是否有效，对所述卫星导航数据进行第一步筛选得到第一导航数据以提高后续初始方位对准修正的准确性。
22.优选的，每个所述第一导航数据内均包含第一x方向导航数据、第一y方向导航数据和第一z方向导航数据，每个所述惯组导航数据内均包含第二x方向导航数据、第二y方向导航数据和第二z方向导航数据，则所述步骤s22包括：
23.步骤s221，针对每个所述第一导航数据和所述第一导航数据对应的所述惯组导航数据，根据所述第一x方向导航数据和所述第二x方向导航数据进行求差计算得到对应的一第一偏差值，并根据所述第一y方向导航数据和所述第二y方向导航数据进行求差计算得到对应的一第二偏差值，以及根据所述第一z方向导航数据和所述第二z方向导航数据进行求差计算得到对应的一第三偏差值；
24.步骤s222，对所述第一偏差值、所述第二偏差值和所述第三偏差值分别进行绝对值计算得到对应的一第一绝对值、第二绝对值和第三绝对值；
25.步骤s223，判断所述第一绝对值、所述第二绝对值和所述第三绝对值中是否存在至少一个绝对值大于第一预设值：
26.若是，则删除所述第一导航数据，随后转向步骤s224；
27.若否，则保留所述第一导航数据作为第二导航数据，随后转向步骤s224；
28.步骤s224，对各所述第二导航数据进行最小二乘拟合处理得到对应的一第二拟合
数据，并根据所述第二拟合数据和各所述第二导航数据处理得到多个所述有效导航数据。
29.本方案中，通过所述第一绝对值、所述第二绝对值和所述第三绝对值对所述第一导航数据进行第二步筛选得到第二导航数据，以提高后续初始方位对准修正的准确性。
30.优选的，所述步骤s224包括：
31.步骤s2241，根据各所述第二导航数据的数量和所述预设数量处理得到一第二数量百分比，并判断所述第二数量百分比是否大于预设的一第二标准百分比：
32.若是，则对各所述第二导航数据进行最小二乘拟合处理得到对应的一第一拟合值、一第二拟合值和一第三拟合值作为所述第二拟合数据，随后转向步骤s2242；
33.若否，则将惯导误差置零并转向步骤s3；
34.步骤s2242，针对每个所述第二导航数据，根据所述第一x方向导航数据和所述第一拟合值进行求差计算得到对应的一第四偏差值，并根据所述第一y方向导航数据和所述第二拟合值进行求差计算得到对应的一第五偏差值，以及根据所述第一z方向导航数据和所述第三拟合值进行求差计算得到对应的一第六偏差值；
35.步骤s2243，对所述第四偏差值、所述第五偏差值和所述第六偏差值分别进行绝对值计算得到对应的一第四绝对值、一第五绝对值和一第六绝对值；
36.步骤s2244，判断所述第四绝对值、所述第五绝对值和所述第六绝对值中是否存在至少一个绝对值大于第二预设值：
37.若是，则删除所述第二导航数据，随后转向步骤s3；
38.若否，则保留所述第二导航数据作为所述有效导航数据，随后转向步骤s3。
39.本方案中，通过所述第四绝对值、所述第五绝对值和所述第六绝对值对所述第二导航数据进行第三步筛选得到有效导航数据，以提高后续初始方位对准修正的准确性。
40.优选的，所述第一预设值为15，所述第二预设值为0.15。
41.优选的，所述第一标准百分比为25％，所述第二标准百分比为50％。
42.优选的，所述第一拟合数据包含x方向关联的第四拟合值及第五拟合值、y方向关联的第六拟合值及第七拟合值、z方向关联的第八拟合值及第九拟合值，则所述步骤s4包括：
43.步骤s41，根据各所述有效导航数据对应的所述采集时刻进行平均值计算得到对应的一平均时刻；
44.步骤s42，根据所述第四拟合值、所述第五拟合值、所述采集时刻和所述平均时刻得到一第一误差值，并根据所述第六拟合值、所述第七拟合值、所述采集时刻和所述平均时刻得到一第二误差值，以及根据所述第八拟合值、所述第九拟合值、所述采集时刻和所述平均时刻得到一第三误差值；
45.步骤s43，对所述第一误差值、所述第二误差值和所述第三误差值分别进行绝对值计算得到对应的一第七绝对值、一第八绝对值和一第九绝对值；
46.步骤s44，判断所述第七绝对值、所述第八绝对值和所述第九绝对值中是否存在至少一个绝对值大于第三预设值：
47.若是，则将所述第一误差值、所述第二误差值和所述第三误差值置零并作为所述惯导误差，随后转向所述步骤s5；
48.若否，则将所述第一误差值、所述第二误差值和所述第三误差值作为所述惯导误
差，随后转向所述步骤s5。
49.优选的，预设的所述初始状态数据包含一初始俯仰角、一初始偏航角和一理论弹道速度，所述标准弹道速度包含x方向标准弹道速度和z方向标准弹道，则所述步骤s5包括：
50.步骤s51，控制所述箭上计算机获取火箭的所述实时姿态四元数，并根据所述第三误差值、所述标准弹道速度、所述x方向标准弹道速度和所述z方向标准弹道速度得到一初始滚动角偏差；
51.步骤s52，根据所述初始滚动角偏差、所述初始俯仰角和所述初始偏航角得到一对比姿态四元数，并根据所述对比姿态四元数和所述实时姿态四元数得到所述修正后姿态四元数；
52.步骤s53，控制所述箭上计算机将所述实时姿态四元数替换为所述修正后姿态四元数完成初始方位对准修正。
附图说明
53.图1为本发明的步骤流程图；
54.图2为本发明的步骤s2的具体流程图；
55.图3为本发明的步骤s22的具体流程图；
56.图4为本发明的步骤s224的具体流程图；
57.图5为本发明的步骤s4的具体流程图；
58.图6为本发明的步骤s5的具体流程图。
具体实施方式
59.为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
60.本发明的较佳的实施例中，基于现有技术中存在的上述问题，现提供一种火箭初始方位对准修正方法，如图1所示，包括：
61.步骤s1，于火箭起飞后的至少一预设时刻，控制火箭上的箭上计算机持续采集预设数量的卫星导航数据和各卫星导航数据对应的一惯组导航数据，并采集预设时刻火箭对应的标准弹道速度，以及记录每个卫星导航数据对应的一采集时刻；
62.步骤s2，对各卫星导航数据进行预处理得到多个有效导航数据；
63.步骤s3，根据各有效导航数据的数量和预设数量处理得到一第一数量百分比，并判断第一数量百分比是否大于预设的一第一标准百分比：
64.若是，则对各有效数据进行最小二乘拟合处理得到对应的一第一拟合数据，随后转向步骤s4；
65.若否，则将惯导误差置零并转向步骤s5；
66.步骤s4，根据第一拟合数据、预设时刻和各有效导航数据对应的采集时刻得到对应的惯导误差；
67.步骤s5，控制箭上计算机获取火箭的一实时姿态四元数，根据惯导误差、标准弹道速度、预设的初始状态数据和实时姿态四元数得到一修正后姿态四元数，并控制箭上计算机将实时姿态四元数替换为修正后姿态四元数完成初始方位对准修正。
68.具体地，本实施例中，考虑到目前进行火箭初始方位对准时普遍采用低精度惯组进行自对准，导致火箭初始方位对准误差很大，箭上计算机计算得到的姿态与火箭实际姿态之间的误差也很大，因此，本方案中，通过箭上计算机持续采集预设数量的卫星导航数据和惯组导航数据，将各卫星导航数据进行过滤筛选后，基于各有效数据和第一拟合数据求得惯导误差，最后通过惯导误差、标准弹道速度、初始状态数据和实时姿态四元数求得修正后姿态四元数，此时求得的修正后姿态四元数已经具备很高的精确度，将其替换掉实时姿态四元数即可完成初始方位对准修正，使得火箭顺利进入轨道。
69.优选的，在火箭起飞后的多个预设时刻，均可以基于本方案进行一次初始方位对准修正，以使得实时姿态四元数越来越精准，在多次修正后，便可以消除方位误差，保证火箭顺利进入轨道。
70.优选的，例如在具体操作时，可以在火箭起飞后20秒进行第一次初始方位对准修正，在火箭起飞后25秒进行第二次初始方位对准修正，在火箭起飞30秒进行第三次初始方位对准修正，通过三次初始方位对准修正基本可以消除方位误差。
71.优选的，并不限定火箭起飞后进行初始方位对准修正的次数，同样，也不限定相邻两次初始方位对准修正之间的间隔时间。
72.优选的，在火箭起飞后的预设时刻便需要开始初始方位对准修正的进程，以防止火箭在修正前的飞行段累计了较大的方位误差，需要消耗更多的推进剂来进行修正，对火箭能量造成损失。
73.本发明的较佳的实施例中，步骤s1中，控制箭上计算机于预设时刻开始，持续采集30个卫星导航数据且每间隔0.1秒采集一个卫星导航数据和卫星导航数据对应的惯组导航数据。
74.具体地，本实施例中，箭上计算机对卫星导航数据进行采样时采样频率为10hz，采样时长为3s，总共采样30组卫星导航数据,通过足量的卫星导航数据为初始方位对准修正提供数据支撑。
75.优选的，卫星导航数据的数量并不限定为30个，可以通过增加卫星导航数据的数量提高初始方位对准修正的准确性。
76.优选的，卫星导航数据的采集间隔并不限定为0.1秒，随着卫星导航数据数量的增加，可以适当减小采集间隔以保证采样时长基本维持不变。
77.本发明的较佳的实施例中，采集到的每个卫星导航数据均对应有一定位标志，步骤s2如图2所示，包括：
78.步骤s21，针对每个卫星导航数据，判断卫星导航数据对应的定位标志是否表征未定位：
79.若是，则删除定位标志对应的卫星导航数据，随后转向步骤s22；
80.若否，则保留定位标志对应的卫星导航数据作为第一导航数据，随后转向步骤s22；
81.步骤s22，根据各第一导航数据和各第一导航数据对应的惯组导航数据处理得到多个有效导航数据。
82.具体地，本实施例中，在采集卫星导航数据时会关联一个定位标志，基于定位标志可以来判断卫星导航数据是否有效，对卫星导航数据进行第一步筛选得到第一导航数据以
提高后续初始方位对准修正的准确性。
83.优选的，在通过第一步筛选得到第一导航数据后，可以每隔10ms记录一个第一导航数据，以方便后续计算。
84.本发明的较佳的实施例中，每个第一导航数据内均包含第一x方向导航数据、第一y方向导航数据和第一z方向导航数据，每个惯组导航数据内均包含第二x方向导航数据、第二y方向导航数据和第二z方向导航数据，则步骤s22如图3所示，包括：
85.步骤s221，针对每个第一导航数据和第一导航数据对应的惯组导航数据，根据第一x方向导航数据和第二x方向导航数据进行求差计算得到对应的一第一偏差值，并根据第一y方向导航数据和第二y方向导航数据进行求差计算得到对应的一第二偏差值，以及根据第一z方向导航数据和第二z方向导航数据进行求差计算得到对应的一第三偏差值；
86.步骤s222，对第一偏差值、第二偏差值和第三偏差值分别进行绝对值计算得到对应的一第一绝对值、第二绝对值和第三绝对值；
87.步骤s223，判断第一绝对值、第二绝对值和第三绝对值中是否存在至少一个绝对值大于第一预设值：
88.若是，则删除第一导航数据，随后转向步骤s224；
89.若否，则保留第一导航数据作为第二导航数据，随后转向步骤s224；
90.步骤s224，对各第二导航数据进行最小二乘拟合处理得到对应的一第二拟合数据，并根据第二拟合数据和各第二导航数据处理得到多个有效导航数据。
91.具体地，本实施例中，通过第一绝对值、第二绝对值和第三绝对值对第一导航数据进行第二步筛选得到第二导航数据，以提高后续初始方位对准修正的准确性。
92.优选的，第一偏差值、第二偏差值和第三偏差值的计算公式如下所示：
93.δvx(t)＝vx
bd
(t)-vx
ins
(t)
94.δvy(t)＝vy
bd
(t)-vy
ins
(t)
95.δvz(t)＝vz
bd
(t)-vz
ins
(t)
96.其中，
97.t表示采集时刻；
98.vx
bd
(t)表示第一x方向导航数据；
99.vy
bd
(t)表示第一y方向导航数据；
100.vz
bd
(t)表示第一z方向导航数据；
101.vx
ins
(t)表示第二x方向导航数据；
102.vy
ins
(t)表示第二y方向导航数据；
103.vz
ins
(t)表示第二z方向导航数据；
104.δvx(t)表示第一偏差值；
105.δvy(t)表示第二偏差值；
106.δvz(t)表示第三偏差值。
107.本发明的较佳的实施例中，步骤s224如图4所示，包括：
108.步骤s2241，根据各第二导航数据的数量和预设数量处理得到一第二数量百分比，并判断第二数量百分比是否大于预设的一第二标准百分比：
109.若是，则对各第二导航数据进行最小二乘拟合处理得到对应的一第一拟合值、一
第二拟合值和一第三拟合值作为第二拟合数据，随后转向步骤s2242；
110.若否，则将惯导误差置零并转向步骤s3；
111.步骤s2242，针对每个第二导航数据，根据第一x方向导航数据和第一拟合值进行求差计算得到对应的一第四偏差值，并根据第一y方向导航数据和第二拟合值进行求差计算得到对应的一第五偏差值，以及根据第一z方向导航数据和第三拟合值进行求差计算得到对应的一第六偏差值；
112.步骤s2243，对第四偏差值、第五偏差值和第六偏差值分别进行绝对值计算得到对应的一第四绝对值、一第五绝对值和一第六绝对值；
113.步骤s2244，判断第四绝对值、第五绝对值和第六绝对值中是否存在至少一个绝对值大于第二预设值：
114.若是，则删除第二导航数据，随后转向步骤s3；
115.若否，则保留第二导航数据作为有效导航数据，随后转向步骤s3。
116.具体地，本实施例中，通过第四绝对值、第五绝对值和第六绝对值对第二导航数据进行第三步筛选得到有效导航数据，以提高后续初始方位对准修正的准确性。
117.本发明的较佳的实施例中，第一预设值为15，第二预设值为0.15。
118.具体地，本实施例中，第一预设值并不限定为15，可以根据实际情况进行调整，同理第二预设值也不限定为0.15。
119.本发明的较佳的实施例中，第一标准百分比为25％，第二标准百分比为50％。
120.具体地，本实施例中，第一标准百分比不限定为为25％，可以根据实际情况进行调整，同理第二标准百分比也不限定为50％。
121.本发明的较佳的实施例中，第一拟合数据包含x方向关联的第四拟合值及第五拟合值、y方向关联的第六拟合值及第七拟合值、z方向关联的第八拟合值及第九拟合值，则步骤s4如图5所示，包括：
122.步骤s41，根据各有效导航数据对应的采集时刻进行平均值计算得到对应的一平均时刻；
123.步骤s42，根据第四拟合值、第五拟合值、采集时刻和平均时刻得到一第一误差值，并根据第六拟合值、第七拟合值、采集时刻和平均时刻得到一第二误差值，以及根据第八拟合值、第九拟合值、采集时刻和平均时刻得到一第三误差值；
124.步骤s43，对第一误差值、第二误差值和第三误差值分别进行绝对值计算得到对应的一第七绝对值、一第八绝对值和一第九绝对值；
125.步骤s44，判断第七绝对值、第八绝对值和第九绝对值中是否存在至少一个绝对值大于第三预设值：
126.若是，则将第一误差值、第二误差值和第三误差值置零并作为惯导误差，随后转向步骤s5；
127.若否，则将第一误差值、第二误差值和第三误差值作为惯导误差，随后转向步骤s5。
128.具体地，本实施例中，第一误差值、第二误差值和第三误差值的计算公式如下所示：
129.[0130][0131][0132]
其中，
[0133]
表示各有效导航数据的平均时刻；
[0134]
t表示预设时刻；
[0135]avx0
表示第四拟合值；
[0136]avy0
表示第六拟合值；
[0137]avz0
表示第八拟合值；
[0138]avx1
表示第五拟合值；
[0139]avy1
表示第七拟合值；
[0140]avz1
表示第九拟合值；
[0141]
δv
xn
表示第一误差值；
[0142]
δv
yn
表示第二误差值；
[0143]
δv
zn
表示第三误差值。
[0144]
具体地，本实施例中，最小二乘拟合算法如下：对于给定的n个数据点(xi,yi)(i＝1,2,
…
,n)，用作线性最小二乘拟合；
[0145][0146][0147]
其中，
[0148][0149][0150][0151][0152]
本发明的较佳的实施例中，预设的初始状态数据包含一初始俯仰角、一初始偏航角和一理论弹道速度，标准弹道速度包含x方向标准弹道速度和z方向标准弹道，则步骤s5如图6所示，包括：
[0153]
步骤s51，控制箭上计算机获取火箭的实时姿态四元数，并根据第三误差值、理论弹道速度、x方向标准弹道速度和z方向标准弹道速度得到一初始滚动角偏差；
[0154]
步骤s52，根据初始滚动角偏差、初始俯仰角和初始偏航角得到一对比姿态四元数，并根据对比姿态四元数和实时姿态四元数得到修正后姿态四元数；
[0155]
步骤s53，控制箭上计算机将实时姿态四元数替换为修正后姿态四元数完成初始方位对准修正。
[0156]
具体地，本实施例中，初始滚动角偏差的计算公式如下所示：
[0157][0158][0159]
其中，
[0160]
dvg表示理论弹道速度；
[0161]
vx0表示x方向标准弹道速度；
[0162]
vz0表示z方向标准弹道速度；
[0163]
δγ
0new
表示初始滚动角偏差。
[0164]
具体地，本实施例中，对比姿态四元数的计算公式如下所示：
[0165][0166]cφ0
＝cos(δφ0/2)
[0167]sφ0
＝sin(δφ0/2)
[0168]cψ0
＝cos(δψ0/2)
[0169]sψ0
＝sin(δψ0/2)
[0170]cγ0
＝cos(δγ
0new
/2)
[0171][0172]
其中，
[0173]
δφ0表示初始俯仰角；
[0174]
δψ0表示初始偏航角；
[0175]
δγ
0new
表示初始滚动角偏差。
[0176]
优选的，通过以下计算公式得到修正后姿态四元数：
[0177][0178]
其中，
[0179]
表示修正后姿态四元数；
[0180]
表示对比姿态四元数；
[0181]
表示实时姿态四元数。
[0182]
虽然本公开披露如上，但本公开的保护范围并非仅限于此。本领域技术人员，在不脱离本公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。