CN102749089A - Method for determining three-probe star sensor gesture - Google Patents

Abstract

The invention discloses a method for determining the attitude of a three-probe star sensor. The data processing module periodically corrects the time of two probe modules, which compensates for the increased time difference between the two imaging probe modules after the star sensor has been running for a long time. It makes up for the shortcomings of the poor attitude accuracy of the star sensor rolling axis of a single imaging probe module; even if one imaging probe module fails, on the basis of ensuring the attitude accuracy, the other two imaging probe modules can still output attitude, which improves the data quality. reliability.

Description

A method for determining the attitude of a three-probe star sensor

technical field

The invention relates to space technology, specifically a method for determining the attitude of a three-probe star sensor.

Background technique

A star sensor is an optical attitude sensor that senses a star's radiation and measures the satellite's orientation relative to that star. Since the opening angle of the star is very small and the direction of starlight in the inertial coordinate system is known precisely, the measurement accuracy of the star sensor is very high, which is an order of magnitude higher than that of the sun sensor. However, since starlight is very weak, signal detection is difficult, and its imaging requires the use of high-sensitivity image sensors, such as image resolution tubes or charge coupled devices (CCD, Charge Coupled Device). There are many stars in the sky. On the one hand, it brings the advantages of more target stars to choose from and convenient application, but it also brings difficulties in identifying the detected stars. Therefore, it needs to be equipped with strong data storage and processing capabilities. onboard digital computer. In order to reduce the influence of external stray light, a hood is often added in front of the lens of the star sensor. The parallel light from the star is focused and imaged on the image plane array after passing through the optical system, and the center position of the star image can be determined according to the energy center method (the accuracy can reach arc seconds). According to the focusing geometric relationship, the direction of the starlight vector in the star sensor coordinate system is further obtained, and then the observation vector of the starlight vector in the satellite body coordinate system is obtained from the installation matrix of the star sensor.

The star sensor can sense multiple stars at the same time (usually stars above magnitude 6), and the number of stars used as the benchmark for three-axis attitude measurement after star map identification is generally more than 3. The three-axis attitude of the satellite relative to the inertial space (celestial coordinate system) can be obtained by using the multi-vector attitude determination method. When the orbital elements of the aircraft are given, the attitude of the aircraft relative to the orbital coordinate system can be obtained through coordinate transformation.

The attitude determination accuracy of the star sensor is determined by the measurement accuracy of the star position. However, there is a contradiction between the measurement accuracy of the star position and the size of the probe angle. In order to further improve the attitude accuracy of the star sensor, many designers reduce the probe of the star sensor. For a single-probe star sensor, the error of roll angle is generally 5-10 times of the error of yaw angle and pitch angle. Therefore, it is not possible to increase the accuracy of the roll angle to the accuracy of the yaw and pitch angles by lowering the star sensor probe.

Moreover, the number of navigation stars that can be captured by the star sensor probe of the small probe is relatively small, which leads to the reduction of the star detection ability of the star sensor, which is not conducive to the star map recognition and the dynamic performance of the aircraft; Take enough navigation stars. This will limit the star detection capability of the star sensor and cause a decrease in the accuracy of attitude determination. In order to solve the problem of star sensor measurement accuracy and star detection ability under the high dynamic flight condition of spacecraft, three probe star sensor schemes can be adopted.

Contents of the invention

The purpose of the present invention is to provide a method for determining the attitude of a three-probe star sensor. The object of the present invention is achieved in that method steps are as follows:

Step 1: Read separately

First acquire the attitude Q ₁ of the probe and the exposure time t ₁ ;

second acquisition of the attitude Q ₂ and exposure time t ₂ of the probe;

The third acquisition is the attitude Q ₃ and the exposure time t ₃ of the probe;

Take the maximum value of t ₁ , t ₂ and t ₃ ;

If t ₁ is the largest; calculate Δt ₂ =t ₁ -t ₂ , Δt ₃ =t ₁ -t ₃ ;

Step 2: Using Q ₂ , Δ ₂ and formula (11)

$\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{c}{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; \&times;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="t"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">t</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

Calculate the attitude Q′ ₂ of the second acquisition probe at the moment t ₁ , and use Q ₃ , Δt ₃ and formula (11) to calculate the attitude Q ′ ₃ of the third acquisition probe at the moment t ₁ ;

Step 3: Use Q ₁ to calculate the optical axis pointing S ₁ of the first acquisition probe at time t ₁ , use Q′ ₂ to calculate the optical axis pointing S ₂ of the second acquisition probe at time t ₁ , use Q′ ₃ to calculate the third Obtain the optical axis of the probe pointing to S′ ₃ at time _t1 ;

Step 4: The image space coordinate V ₁ that the first acquisition probe optical axis points to is (0, 0, 1), using

Formula (5) $u_r=\left[\begin{array}{c}{u}_{\mathrm{r\; x}}\\ u_{\mathrm{ry}}\\ u_{\mathrm{rz}}\end{array}\right],$ $V_r=\left[\begin{array}{c}{V}_{\mathrm{r\; x}}\\ V_{\mathrm{ry}}\\ V_{\mathrm{rz}}\end{array}\right],$ $W_r=\left[\begin{array}{c}{W}_{\mathrm{r\; x}}\\ W_{\mathrm{ry}}\\ W_{\mathrm{rz}}\end{array}\right]---\left(5\right)$

Convert the image space coordinate V ₂ (0, 0, 1) pointed by the optical axis of the second acquisition probe to the image space coordinate V′ ₂ in the coordinate system of the first acquisition probe, using formula (5)

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

Transform the image space coordinate V ₃ (0, 0, 1) pointed to by the optical axis of the third acquisition probe to the image space coordinate V′ ₃ in the coordinate system of the first acquisition probe;

Step 5: Using V ₁ , V′ ₂ , V′ ₃ and S ₁ , S′ ₂ , S′ ₃ , using formula (8)

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; br</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; br</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

Calculate the attitude Q of the three probes, and output the time t ₁ and attitude Q;

if _t2 max;

Calculate Δt ₁ =t ₂ -t ₁ , Δt ₃ =t ₂ -t ₃ ;

Step 6: Using Q ₁ , Δt ₁ and formula (11)

$\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{c}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; \&times;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="t"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">t</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

Calculate the attitude Q′ ₁ of the first acquisition probe at time t ₂ , using Q ₃ , Δt ₃ and formula (11)

Calculate the attitude Q′ ₃ of the third acquisition probe at time t ₂ ;

Step 7: Use Q′ ₁ to calculate the optical axis pointing S′ ₁ of the first acquisition probe at time t ₂ , use Q ₂ to calculate the optical axis pointing S ₂ of the second acquisition probe at time t ₂ , use Q′ ₃ to calculate the third Obtain the optical axis of the probe pointing to S′ ₃ at time t ₂

Step 8: The image space coordinate V ₁ pointed to by the optical axis of the first acquisition probe is (0, 0, 1), and the image space coordinate V ₂ (0, 0, 1) pointed to by the optical axis of the second acquisition probe is calculated using formula (5). ) to the image space coordinate V′ ₂ in the coordinate system of the first acquisition probe, and use formula (5) to convert the image space coordinate V ₃ (0, 0, 1) pointed to by the optical axis of the third acquisition probe to the first acquisition probe Image space coordinate V′ ₃ under the coordinate system;

Using V ₁ , V′ ₂ , V′ ₃ and S′ ₁ , S ₂ , S′ ₃ , use formula (8) to calculate the attitude Q of the three probes, and output time t ₂ and attitude Q;

if t ₃ max;

Calculate Δt ₁ =t ₃ -t ₁ , Δt ₂ =t ₃ -t ₂

Use Q ₁ , Δt ₁ and formula (11) to calculate the attitude Q′ ₁ of the first acquisition probe at time t ₃ , use Q ₂ , Δt ₂ and formula (11) to calculate the attitude Q′ ₂ of the second acquisition probe at time t ₃

Use Q′ ₁ to calculate the optical axis direction S′ ₁ of the first acquisition probe at time t ₃ , use Q′ ₂ to calculate the optical axis direction S′ ₂ of the second acquisition probe at time t ₃ , use Q ₃ to calculate the third acquisition probe The optical axis at time _t3 points to _S3

The image space coordinate V ₁ pointed to by the optical axis of the first acquisition probe is (0, 0, 1), and the image space coordinate V ₂ (0, 0, 1) pointed to by the optical axis of the second acquisition probe is converted to Using the formula (5) to transform _{the image space coordinate V 3 (} 0, 0, 1) pointed by the optical axis of the third acquisition probe into the coordinate system of the first acquisition probe The image space coordinate V′ ₃

Step 9: Use V ₁ , V′ ₂ , V′ ₃ and S′ ₁ , S′ ₂ , S ₃ to calculate the attitude Q of the three probes using formula (8), and output time t ₃ and attitude Q.

A method for determining the attitude of a three-probe star sensor in the present invention, the data processing module periodically corrects the time of the two probe modules, which makes up for the increased time difference between the two imaging probe modules after the star sensor has been running for a long time Disadvantages; make up for the disadvantage of poor attitude accuracy of the star sensor rolling axis of a single imaging probe module; even if a certain imaging probe module is effective, on the basis of ensuring the attitude accuracy, the other two imaging probe modules can still output attitude, which improves data reliability sex.

Description of drawings

Figure 1 is a flow chart of attitude calculation for a three-probe star sensor;

Figure 2 is a schematic diagram of a three-probe star sensor;

Figure 3 is the calculation of the three-axis attitude error curve using the probe 1 star map;

Figure 4 shows the test results of the three-probe star sensor.

Detailed ways

The present invention will be further described below with reference to the accompanying drawings.

Example 1:

In conjunction with Fig. 1 and Fig. 2, a method for determining the attitude of a three-probe star sensor of the present invention, the steps are as follows:

The method steps are as follows:

Step 1: Read separately

First acquire the attitude Q ₁ of the probe and the exposure time t ₁ ;

second acquisition of the attitude Q ₂ and exposure time t ₂ of the probe;

The third acquisition is the attitude Q ₃ and the exposure time t ₃ of the probe;

Take the maximum value of t ₁ , t ₂ and t ₃ ;

If t ₁ is the largest; calculate Δt ₂ =t ₁ -t ₂ , Δt ₃ =t ₁ -t ₃ ;

Step 2: Using Q ₂ , Δt ₂ and formula (11)

$\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{c}{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; \&times;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="t"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">t</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

Calculate the attitude Q′ ₂ of the second acquisition probe at the moment t ₁ , and use Q ₃ , Δt ₃ and formula (11) to calculate the attitude Q ′ ₃ of the third acquisition probe at the moment t ₁ ;

Step 3: Use Q ₁ to calculate the optical axis pointing S ₁ of the first acquisition probe at time t ₁ , use Q′ ₂ to calculate the optical axis pointing S ₂ of the second acquisition probe at time t ₁ , use Q′ ₃ to calculate the third Obtain the optical axis of the probe pointing to S′ ₃ at time _t1 ;

Step 4: first obtain the image space coordinate V ₁ pointed to by the optical axis of the probe is (0, 0, 1), using formula (5)

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

Convert the image space coordinate V ₂ (0, 0, 1) pointed by the optical axis of the second acquisition probe to the image space coordinate V′ ₂ in the coordinate system of the first acquisition probe, using formula (5)

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

Transform the image space coordinate V ₃ (0, 0, 1) pointed to by the optical axis of the third acquisition probe to the image space coordinate V′ ₃ in the coordinate system of the first acquisition probe;

Step 5: Using V ₁ , V′ ₂ , V′ ₃ and S ₁ , S′ ₂ , S′ ₃ , using formula (8)

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; br</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; br</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

Calculate the attitude Q of the three probes, and output the time t ₁ and attitude Q;

if _t2 max;

Calculate Δt ₁ =t ₂ -t ₁ , Δt ₃ =t ₂ -t ₃ ;

Step 6: Using Q ₁ , Δt ₁ and formula (11)

$\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{c}{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; \&times;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="t"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">t</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

Calculate the attitude Q′ ₁ of the first acquisition probe at _t2 , and use _Q3 , _Δt3 and formula (11) to calculate the attitude Q′ ₃ of the third acquisition probe at _t2 ;

Step 7: Use Q′ ₁ to calculate the optical axis pointing S′ ₁ of the first acquisition probe at time t ₂ , use Q ₂ to calculate the optical axis pointing S ₂ of the second acquisition probe at time t ₂ , use Q′ ₃ to calculate the third Obtain the optical axis of the probe pointing to S′ ₃ at time t ₂

Step 8: The image space coordinate V ₁ pointed to by the optical axis of the first acquisition probe is (0, 0, 1), and the image space coordinate V ₂ (0, 0, 1) pointed to by the optical axis of the second acquisition probe is calculated using formula (5). ) to the image space coordinate V′ ₂ in the coordinate system of the first acquisition probe, and use formula (5) to convert the image space coordinate V ₃ (0, 0, 1) pointed to by the optical axis of the third acquisition probe to the first acquisition probe Image space coordinate V′ ₃ under the coordinate system;

Using V ₁ , V′ ₂ , V′ ₃ and S′ ₁ , S ₂ , S′ ₃ , use formula (8) to calculate the attitude Q of the three probes, and output time t ₂ and attitude Q;

if t ₃ max;

Calculate Δt ₁ =t ₃ -t ₁ , Δt ₂ =t ₃ -t ₂

Use Q ₁ , Δt ₁ and formula (11) to calculate the attitude Q′ ₁ of the first acquisition probe at time tX, and use Q ₂ , Δt ₂ and formula (11) to calculate the attitude Q′ ₂ of the second acquisition probe at time t ₃

Use _Q'1 to calculate the optical axis pointing S'X of the first acquisition probe at time _t3 , use _Q'2 to calculate the optical axis direction _S'2 of the second acquisition probe at time _t3 , use _Q3 to calculate the third acquisition probe The optical axis at time _t3 points to _S3

The image space coordinate V ₁ pointed to by the optical axis of the first acquisition probe is (0, 0, 1), and the image space coordinate V ₂ (0, 0, 1) pointed to by the optical axis of the second acquisition probe is converted to Using the formula (5) to transform _{the image space coordinate V 3 (} 0, 0, 1) pointed by the optical axis of the third acquisition probe into the coordinate system of the first acquisition probe The image space coordinate V′ ₃

Step 9: Use V ₁ , V′ ₂ , V′ ₃ and S′ ₁ , S′ ₂ , S ₃ to calculate the attitude Q of the three probes using formula (8), and output time t ₃ and attitude Q.

Example 2:

Combined with Figure 1 and Figure 2, the attitude determination principle of the three-probe star sensor:

Three unit vectors X _n , Y _n , Z _n form a mutually orthogonal coordinate system, where: the modulus of Y _n and Z _n |Y _n |=1 and |Z _n |=1, record the new coordinate system F _n is

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\\ {\mathrm{<span\; class="notranslate">\; Y</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\\ {\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

The body coordinate system of the spacecraft is F _b , the space reference coordinate system is F _r , and their coordinate bases are respectively

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; Y</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; Y</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

Among them, X _b , Y _b , Z _b are F _b unit vectors, and X _r , Y _r , Z _r are F _r unit vectors. X _n , Y _n , Z _n are the three measured vectors, and their description with the coordinate system of F _n and F _r is

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}$

${\mathrm{<span\; class="notranslate">\; Y</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}$

Among them, U _b , V _b , U _r , V _r , W _b , and W _b are the coordinate arrays (direction cosines) of X _n , Y _n , and Z _n in F _b and F _r respectively, and the expansion formula is written as

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; bx</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; by</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; bz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; bx</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; by</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; bz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; bx</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; by</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; bz</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; ,</span>$ ${\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; ry</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; rz</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

Since the three unit vectors X _n , Y _n , Z _n are orthogonal to each other,

therefore,

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\\ {\mathrm{<span\; class="notranslate">\; Y</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\\ {\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}\end{array}\right]{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

C _nb is the attitude matrix (direction cosine matrix) between F _n and F _b .

In the same way,

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\\ {\mathrm{<span\; class="notranslate">\; Y</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\\ {\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\end{array}\right]{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

C _nr is the attitude matrix (direction cosine matrix) between F _n and F _r .

Comparing (6) and (7), and let the attitude matrix between F _b and F _r be C _br , we have

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; br</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\\ {\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; br</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nb</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; nr</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$

必存在。C _br is the desired attitude matrix between F _b and F _r , from which the description of the three unit vectors X _n , Y _n , Z _n of F _b in F _r can be obtained, that is, the attitude of the three-axis of the aircraft can be determined . Since X _n , Y _n , and Z _n are orthogonal to each other and not parallel, so

must exist.

If F _b is the aircraft body coordinate system, F _r = F _o is the orbit coordinate system, for the roll angle defined by the xyz rotation sequence When the pitch angle θ and yaw angle ψ are both small angles, the attitude matrix C _bo between F _b and F _o is

俯仰角θ及偏航角ψ。需要说明，由于滚动角

俯仰角θ及偏航角ψ约定为小角度，也常以Δθ、Δψ表示，称之为滚动、俯仰、偏航偏差角。Therefore, the rolling angle can be obtained by using formula (8)

Pitch angle θ and yaw angle ψ. Note that due to roll angle

The pitch angle θ and yaw angle ψ are agreed to be small angles, and are often expressed as Δθ, Δψ represent, called roll, pitch, yaw deviation angle.

②The recursive principle of posture

那么有Suppose the attitude quaternion of a certain probe of the star sensor at time t ₁ is Q ₁ , and the angular velocity of the star sensor at time t ₁ is ω ₁ , where Q ₁ =q ₁₀ ×i+q ₁₁ ×j+q ₁₂ ×k +q ₁₃ ,

then there is

$\left[\begin{array}{c}{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}\left[\begin{array}{cccc}\mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; x</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; the\; y</span>}}& {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; z</span>}}\\ <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; z</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; x</span>}}& {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; the\; y</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; the\; y</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; x</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; z</span>}}\\ <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; x</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; the\; y</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; z</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

At t′ ₁ (where t′ ₁ >t ₁ ), the attitude quaternion of the probe of the star sensor is Q′ ₁ (Q′ ₁ =q′ ₁₀ ×i+q′ ₁₁ ×j+q′ ₁₂ ×k+ q′ ₁₃ ) is:

$\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}^{<span\; class="notranslate">\; \&prime;</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; +</span>\left[\begin{array}{c}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 10</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}\\ {\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\end{array}\right]<span\; class="notranslate">\; \&times;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="t"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">t</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 12</span>}<span\; class="notranslate">\; )</span>$

Example 3:

The experimental method is as follows: as shown in Figure 2, put the three-probe star sensor outdoors, place the three-probe star sensor on the turntable of the earth, and adjust the turntable so that all three probes can observe enough stars. When randomly aligning with a certain area of the celestial sphere and the earth is relatively stationary, and rotates with the rotation of the earth, it will run for a long time. The three-probe star sensor automatically enters the star tracking mode after all-sky recognition. For the same calculation period, the three-probe star sensor first uses the star map of probe 1 to calculate the attitude, and then uses the star map of the three probes to adopt the method of the present invention. The method calculates attitude, the output adopts the three-axis attitude measurement value of probe 1 and the star map adopting three probes to adopt the method of the present invention to calculate attitude, and outputs these two groups of three-axis attitude measurement values, and then sends the calculated attitude to The upper computer, after receiving the attitude, the upper computer saves it in real time, and calculates the difference between the received attitude and the real attitude (ie attitude error) in real time. After receiving a certain number of attitudes (such as receiving about 1820 seconds of data frames), use MATLAB displays the attitude error curve and calculates the three-axis attitude accuracy. As shown in Figure 3, after calculation, the three-axis attitude errors calculated using only the star map of probe 1 are: yaw angle 1.1025" (3σ), pitch angle 1.0401" (3σ), and roll angle 5.8576" (3σ). As shown in Figure 4, the three-axis attitude error of adopting the method of the present invention to calculate the attitude of the star map of three probes is respectively: yaw angle 0.4561 " (3σ), pitch angle 0.3408 " (3σ), roll angle 0.3659 " ( 3σ).

According to the working principle of the star sensor, the accuracy of the roll angle of the star sensor using a probe is worse than the accuracy of the yaw angle and the pitch angle. From a theoretical analysis, the accuracy of the roll angle is about 5 times worse than the accuracy of the yaw angle and the pitch angle. times, as can be seen from Figure 3, the three-axis attitude errors calculated using only the star map of probe 1 are: yaw angle 1.1025″ (3σ), pitch angle 1.0401″ (3σ), roll angle 5.8576″ (3σ ), so the error of the roll angle is about 5 times worse than the accuracy of the yaw angle and the pitch angle. And adopt the three-axis attitude of the method of the present invention to calculate the attitude of the star map of the three probes, and do not directly roll the output of the three probes Instead, the yaw angle and pitch angle of the three probes are used for information fusion, so as to make up for the shortcomings of the poor roll angle accuracy of the single-probe star sensor. It can be seen from Figure 4 that the output three-axis attitude of the dual-probe star sensor is All meet the attitude error of 1″.

Example 4:

The following is an implementation of a dual-probe star sensor. Using the ESOQ2 algorithm, the attitude quaternions of the three probes can be calculated separately:

The attitude quaternion of probe 1: Q ₁ =q ₀ 1×i+q ₁₁ ×j+q ₂₁ ×k+q ₃₁ ;

The attitude quaternion of the probe 2: Q ₂ =q ₀₂ ×i+q ₁₂ ×j+q ₂₂ ×k+q ₃₂ ;

The attitude quaternion of the probe 3: Q ₃ =q ₀₃ ×i+q ₁₃ ×j+q ₂₃ ×k+q ₃₃ .

use $A\left(q\right)=\left[\begin{array}{ccc}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2+q_4^2& 2(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HSA00000748481400041.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+q_3^2+q_4^2\end{array}\right]$

The attitude matrices of the three probes can be calculated separately. According to the attitude matrices of the three probes, the optical axis directions of the three probes are calculated as follows: the optical axis direction of the probe 1: S ₁ =s ₀₁ ×i+s ₁₁ ×j+s ₂₁ ×k; the direction of the optical axis of probe 2: S ₂ =s ₀₂ ×i+s ₁₂ ×j+s ₂₂ ×k; the direction of the optical axis of probe 3: S ₃ =s ₀₃ ×i+s ₁₃ ×j+s ₂₃ ×k. The coordinates of the three optical axes pointing to the body image space coordinate system are (0, 0, 1), assuming that the attitude polarity of the three-probe star sensor is calculated to be consistent with the polarity of probe 1, then probe 2 and probe 3 The coordinates in the body image space are converted to the image space coordinates of probe 1, assuming that the relationship between probe 2 and the coordinate system of probe 1 body image space is R ₁₂ , and the relationship between probe 3 and the coordinate system of probe 1 body image space is R ₁₃ , then the coordinates of the probe 2 body image space coordinates in the probe 1 body image space coordinate system are: U ₁₂ = R ₁₂ (0, 0, 1), and the probe 3 body image space coordinates are in the probe 1 body image space The coordinates in the coordinate system of probe 1 are: U ₁₃ =R ₁₃ (0,0,1), the coordinates of the probe 1 body image space coordinates in the probe 1 body image space coordinate system are: U ₁ =(0,0,1 ), using U ₁₂ , U ₁₃ , U ₁ and S ₁ , S ₂ and S ₃ , the attitude of the three-probe star sensor can be calculated by formula (8).

Features and advantages of the present invention:

First: The data processing module periodically adjusts the time of the two probe modules, which makes up for the shortcoming of the increased time difference between the two imaging probe modules after the star sensor has been running for a long time;

Second: Make up for the shortcomings of poor attitude accuracy of the star sensor of the single imaging probe module;

Third: Even if a certain imaging probe module is effective, on the basis of ensuring the attitude accuracy, the other two imaging probe modules can still output the attitude, which improves the data reliability.

The main performance indicators of a single probe module:

Probe: 20°×20°

Area array: 2048×20484

Detection magnitude: 5.5Mv

Data update rate: 5Hz

The optical axes of the three probes point two by two orthogonally.

In order to verify the accuracy of the three-probe star sensor, an outfield star observation comparison was made with the accuracy of the output attitude of a certain probe of the star sensor (because the output attitude polarity of the three-probe star sensor is different from that of the star sensor of probe 1 The polarity is consistent, so during the experiment, the output three-probe attitude is compared with the attitude of probe 1).

Claims (1)

1. definite method of a probe star sensor attitude, it is characterized in that: step is following:

Step 1: read respectively

First obtains the attitude Q of probe ₁With the t time of exposure ₁

Second obtains the attitude Q of probe ₂With the t time of exposure ₂

The 3rd obtains the attitude Q of probe ₃With the t time of exposure ₃

Get t ₁, t ₂And t ₃Maximal value;

If t ₁Maximum; Calculate Δ t ₂=t ₁-t ₂, Δ t ₃=t ₁-t ₃

Step 2: utilize Q ₂, Δ t ₂And formula (11)

$\left[\begin{array}{c}{q}_{10}^{\&prime;}\\ q_{11}^{\&prime;}\\ q_{12}^{\&prime;}\\ q_{13}^{\&prime;}\end{array}\right]=\left[\begin{array}{c}{q}_{10}\\ q_{11}\\ q_{12}\\ q_{13}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_{10}\\ {\stackrel{\&CenterDot;}{q}}_{11}\\ {\stackrel{\&CenterDot;}{q}}_{12}\\ {\stackrel{\&CenterDot;}{q}}_{13}\end{array}\right]\&times;(t_1^{\&prime;}-t_1)---\left(11\right)$

Calculate second and obtain probe at t ₁Moment attitude Q ' ₂, utilize Q ₃, Δ t ₃And formula (11) calculating the 3rd is obtained probe at t ₁Moment attitude Q ' ₃

Step 3: utilize Q ₁Calculate first and obtain probe at t ₁Optical axis constantly points to S ₁, utilize Q ' ₂Calculate second and obtain probe at t ₁Optical axis constantly points to S ' ₂, utilize Q ' ₃Calculate the 3rd and obtain probe at t ₁Optical axis constantly points to S ' ₃

Step 4: first obtains the image space coordinate V that the probe optical axis points to ₁Be (0,0,1), utilize

Formula (5) $U_r=\left[\begin{array}{c}{U}_{\mathrm{Rx}}\\ U_{\mathrm{Ry}}\\ U_{\mathrm{Rz}}\end{array}\right],$ $V_r=\left[\begin{array}{c}{V}_{\mathrm{Rx}}\\ V_{\mathrm{Ry}}\\ V_{\mathrm{Rz}}\end{array}\right],$ $W_r=\left[\begin{array}{c}{W}_{\mathrm{Rx}}\\ W_{\mathrm{Ry}}\\ W_{\mathrm{Rz}}\end{array}\right]---\left(5\right)$

Obtain the image space coordinate V that the probe optical axis points to second ₂(0,0,1) is transformed into first and obtains the image space coordinate V ' of probe under the coordinate system ₂, utilize formula (5)

$U_r=\left[\begin{array}{c}{U}_{\mathrm{rx}}\\ U_{\mathrm{ry}}\\ U_{\mathrm{rz}}\end{array}\right],$ $V_r=\left[\begin{array}{c}{V}_{\mathrm{rx}}\\ V_{\mathrm{ry}}\\ V_{\mathrm{rz}}\end{array}\right],$ $W_r=\left[\begin{array}{c}{W}_{\mathrm{rx}}\\ W_{\mathrm{ry}}\\ W_{\mathrm{rz}}\end{array}\right]---\left(5\right)$

Obtain the image space coordinate V that the probe optical axis points to the 3rd ₃(0,0,1) is transformed into first and obtains the image space coordinate V ' of probe under the coordinate system ₃

Step 5: utilize V ₁, V ' ₂, V ' ₃And S ₁, S ' ₂, S ' ₃, utilize formula (8)

$\left\{\begin{array}{c}{C}_{\mathrm{nb}}{F}_b=C_{\mathrm{nr}}{F}_r\\ F_b=C_{\mathrm{br}}{F}_r\\ C_{\mathrm{br}}=C_{\mathrm{nb}}^{-1}{C}_{\mathrm{nr}}=C_{\mathrm{nb}}^T{C}_{\mathrm{nr}}\end{array}\right.---\left(8\right)$

Calculate the attitude Q of three probes, and output time t ₁With attitude Q;

If t ₂Maximum;

Calculate Δ t ₁=t ₂-t ₁, Δ t ₃=t ₂-t ₃

Step 6: utilize Q ₁, Δ t ₁And formula (11)

$\left[\begin{array}{c}{q}_{10}^{\&prime;}\\ q_{11}^{\&prime;}\\ q_{12}^{\&prime;}\\ q_{13}^{\&prime;}\end{array}\right]=\left[\begin{array}{c}{q}_{10}\\ q_{11}\\ q_{12}\\ q_{13}\end{array}\right]+\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_{10}\\ {\stackrel{\&CenterDot;}{q}}_{11}\\ {\stackrel{\&CenterDot;}{q}}_{12}\\ {\stackrel{\&CenterDot;}{q}}_{13}\end{array}\right]\&times;(t_1^{\&prime;}-t_1)---\left(11\right)$

Calculate first and obtain probe at t ₂Moment attitude Q ' ₁, utilize Q ₃, Δ t ₃And formula (11) calculating the 3rd is obtained probe at t ₂Moment attitude Q ' ₃

Step 7: utilize Q ' ₁Calculate first and obtain probe at t ₂Optical axis constantly points to S ' ₁, utilize Q ₂Calculate second and obtain probe at t ₂Optical axis constantly points to S ₂, utilize Q ' ₃Calculate the 3rd and obtain probe at t ₂Optical axis constantly points to S ' ₃

Step 8: first obtains the image space coordinate V that the probe optical axis points to ₁Be (0,0,1), utilize formula (5) to obtain the image space coordinate V that the probe optical axis points to second ₂(0,0,1) is transformed into first and obtains the image space coordinate V ' of probe under the coordinate system ₂, utilize formula (5) to obtain the image space coordinate V that the probe optical axis points to the 3rd ₃(0,0,1) is transformed into first and obtains the image space coordinate V ' of probe under the coordinate system ₃

Utilize V ₁, V ' ₂, V ' ₃And S ' ₁, S ₂, S ' ₃, the attitude Q that utilizes formula (8) calculating three to pop one's head in, and output time t ₂With attitude Q;

If t ₃Maximum;

Calculate Δ t ₁=t ₃-t ₁, Δ t ₂=t ₃-t ₂

Utilize Q ₁, Δ t ₁And formula (11) calculating first is obtained probe at t ₃Moment attitude Q ' ₁, utilize Q ₂, Δ t ₂And formula (11) calculating second is obtained probe at t ₃Moment attitude Q ' ₂

Utilize Q ' ₁Calculate first and obtain probe at t ₃Optical axis constantly points to S ' ₁, utilize Q ' ₂Calculate second and obtain probe at t ₃Optical axis constantly points to S ' ₂, utilize Q ₃Calculate the 3rd and obtain probe at t ₃Optical axis constantly points to S ₃

First obtains the image space coordinate V that the probe optical axis points to ₁Be (0,0,1), utilize formula (5) to obtain the image space coordinate V that the probe optical axis points to second ₂(0,0,1) is transformed into first and obtains the image space coordinate V ' of probe under the coordinate system ₂, utilize formula (5) to obtain the image space coordinate V that the probe optical axis points to the 3rd ₃(0,0,1) is transformed into first and obtains the image space coordinate V ' of probe under the coordinate system ₃

Step 9: utilize V ₁, V ' ₂, V ' ₃And S ' ₁, S ' ₂, S ₃, the attitude Q that utilizes formula (8) calculating three to pop one's head in, and output time t ₃With attitude Q.

Images