CN104635740A - Autonomous attitude maneuver control method of deep space probe - Google Patents

Abstract

The invention discloses an autonomous attitude maneuver control method of a deep space probe, which relates to the autonomous attitude maneuver control method and belongs to the technical field of spacecraft attitude control. The autonomous attitude maneuver control method comprises the following steps of carrying out random sampling on uniformly-distributed nodes of an attitude space with an ORRT (Optimized Rapidly Exploring Random Tree) algorithm as a path planning method, making a balance and choosing the optimum path to expand, carrying out incremental expansion in a safety space in a greedy expansion manner, respectively establishing a probe attitude maneuver dynamic constraint model, an actual engineering constraint model and a probe geometric constraint model under a probe body coordinate system to obtain path nodes which meet the constraint and generate a control moment for the nodes, thereby generating a probe attitude maneuver path and a required control moment, and implementing the purpose that probe maneuvers to a target attitude according to the generated probe attitude maneuver path and the required control moment. According to the autonomous attitude maneuver control method, the path planning time is shortened and the efficiency that the probe maneuvers to the target attitude from the initial attitude is improved under the condition that all kinds of complicated constraint conditions faced by the probe are met.

Description

A kind of deep space probe autonomous attitude maneuver control method

Technical field

The present invention relates to a kind of autonomous attitude maneuver control method, belong to technical field of spacecraft attitude control,

Background technology

Detector in orbit period need by a large amount of attitude maneuvers complete different attitude point between switch operating; Meanwhile, due to the diversity of detector task, the attitude maneuver process of some detector also exists certain spatial direction constraint.Secondly, the control inputs of detector also suffers restraints.This constraint is mainly caused by two factors: one is that the moment amplitude that topworks provides is limited; Two is after topworks's generating portion is damaged.3rd, because the range of some angular velocity sensor is limited, require that the angular velocity of detector must remain within the scope of certain, which forms angular speed constraint.In the face of above all multiple constraints, appearance control techniques must obtain the growth requirement that corresponding improvement could meet space mission.

For this problem, Mengali G, Quarta A A utilizes potential-energy function method to solve this problem in " Spacecraft Control with Constrained Fast Reorientation and Accurate Pointing " literary composition, this method calculates simple, less to resource requirement on detector, but the method is difficult to the problem processing bounded control input.

Cheng Xiaojun, Cui Hutao, Cui Pingyuan, Xu Rui utilizes pseudo-spectrometry to solve this problem in " Large angular autonomous attitude maneuver of deep spacecraft using pseudospectral method " literary composition, consider overall performance preferably, but because pseudo-spectrometry is by non-uniform node sliding-model control dynamics recursion, be difficult to the path constraint considered between node, easily cause hop to violate geometrical constraint.

Summary of the invention

The technical problem to be solved in the present invention is meeting under the various Complex Constraints conditions that detector faces, shortening the time selecting optimal programming path, improves detector from the motor-driven efficiency to targeted attitude of reference attitude.A kind of deep space probe autonomous attitude maneuver control method disclosed by the invention, meeting under the various Complex Constraints conditions that detector faces, can make detector motor-driven to targeted attitude from reference attitude rapidly.Described various Complex Constraints comprise Dynamic Constraints, Practical Project constraint, geometrical constraint.

The present invention is achieved through the following technical solutions:

A kind of deep space probe autonomous attitude maneuver control method disclosed by the invention, using ORRT (Optimized Rapidly Exploring Random Tree) algorithm as paths planning method, stochastic sampling is carried out to the Uniformly distributed node of configuration space, then carry out weighing preferentially extensions path, with greedy extended mode incremental expansion in safe space, detector attitude maneuver Dynamic Constraints model is set up respectively under detector body coordinate system, Practical Project restricted model and detector geometrical constrain model, be met the path node of constraint and generate the control moment of node, and then generate detector attitude maneuver path and required control moment, detector is realized motor-driven to targeted attitude according to generation detector attitude maneuver path and required control moment.The present invention not only considers the physical constraint faced in engineering, and fully meets Dynamic Constraints and geometrical constraint that detector faces, shortens the time selecting optimal programming path, improves detector from the motor-driven efficiency to targeted attitude of reference attitude.

A kind of deep space probe autonomous attitude maneuver control method disclosed by the invention, concrete implementation step is:

Step one: according to ephemeris moment in attitude maneuver moment at that time, determines the relevant bright expression r of celestial body under inertial system _i, then determine inertia be tied to detector body coordinate system under pose transformation matrix C _bI, therefore can indicate the position vector v of bright celestial body under detector body system _b;

According to detector self mounting characteristics, determine the position vector r of sensor under detector body system _b.

Step 2: set up detector attitude maneuver Dynamic Constraints model, set up the Practical Project restricted model faced in engineering.Set up the geometrical constrain model that detector faces.Concrete point three steps:

The first step: set up detector attitude maneuver Dynamic Constraints model.

Attitude kinematics and kinetics equation are expressed as follows:

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{Q\ω}=\frac{1}{2}\mathrm{\Ω\ω}---\left(1\right)$

Jω＝T-ω×Jω (2)

Wherein, q=[q ₀, q ₁, q ₂, q ₃] ^t, meet normalization constraint || q|| ₂=1, represent 2-norm.ω=[ω ₁, ω ₂, ω ₃] ^tthe expression of angular velocity under body series of spacecraft relative inertness system, and

$Q=\left[\begin{array}{ccc}-q_1& {-q}_2& -q_3\\ q_0& {-q}_3& q_2\\ q_3& q_0& {-q}_1\\ -q_2& q_1& q_0\end{array}\right],\mathrm{\Ω}=\begin{array}{c}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_1& -{\mathrm{\ω}}_2& -{\mathrm{\ω}}_3\\ {\mathrm{\ω}}_1& 0& {\mathrm{\ω}}_3& -{\mathrm{\ω}}_2\\ {\mathrm{\ω}}_2& -{\mathrm{\ω}}_3& 0& {\mathrm{\ω}}_1\\ {\mathrm{\ω}}_3& {\mathrm{\ω}}_2& -{\mathrm{\ω}}_1& 0\end{array}\right]---\left(3\right)\end{array}$

J=diag (J ₁, J ₂, J ₃) represent the inertial matrix that spacecraft opposing body is, u=[T ₁, T ₂, T ₃] ^tfor the component of control moment under body series.

Second step: set up the Practical Project restricted model faced in engineering.Described Practical Project constraint comprises control moment bounded and angular velocity bounded.

Due in Practical Project, control moment bounded is expressed as:

|T _i|≤γ _Ti(4)

In attitude maneuver process, because measuring sensor exists measurement range or the normal work in order to some instrument, need by angular velocity amplitude limit within the specific limits, angular velocity bounded is expressed as:

|ω _i|≤γ _ωi(5)

3rd step: set up geometrical constrain model.

$r_B^T{v}_B\≤\cos \mathrm{\θ}---\left(6\right)$

Wherein, r _brepresent the direction vector of sensitive element under body series, v _bfor the direction vector of high light celestial body under body coordinate is in order to avoid spacecraft is in mobile process, high light celestial body light enters in the visual field of optical sensing element, must ensure that the angle between the direction of visual lines vector of this type of Sensitive Apparatus and high light celestial body direction vector can not lower than threshold value θ.

Convert (6) formula to hypercomplex number representation

v _B＝C _BIr _I＝r _I-2 q ^T qr _I+2 q q ^Tr _I+2q ₀([r _I×] q) (7)

Wherein, r _irepresent the direction vector component under inertial system of detector to high light celestial body, C _bIrepresent the attitude cosine matrix of spacecraft.

Wherein, q=[q ₁, q ₂, q ₃] ^tfor hypercomplex number vector section.[r _i×] be multiplication cross matrix, concrete form is

$[r_I\×]=\left[\begin{array}{ccc}0& -r_{I3}& r_{I2}\\ r_{I3}& 0& {-r}_{I1}\\ -r_{I2}& r_{I1}& 0\end{array}\right]---\left(8\right)$

Following form is identical.(6) are expressed as compacter form, obtain the quadratic constraints form in (9) formula.

q ^TAq≤0 (9)

Wherein,

$A=\left[\begin{array}{cc}{r}_B^T{r}_I-\cos {\mathrm{\θ}}_1& -r_B^T[r_I\×]\\ -r_B^T[r_I\×]& {2r}_I{r}_B^T-(r_B^T{r}_I+{\cos \mathrm{\θ}}_1)I_3\end{array}\right]---\left(10\right)$

Step 3: the various Complex Constraints proposed in conjunction with above-mentioned steps two, utilize ORRT planning algorithm to draw autonomous attitude maneuver control method.Described various Complex Constraints comprise Dynamic Constraints, Practical Project constraint, geometrical constraint.

ORRT is the greedy expansion process in consistent attitude sample space, comprises random targets node sample, adjacent node calculates and new node is expanded.First carry out initialization, then generate planning space, effective metric function must be chosen before this:

$\mathrm{\ρ}={\underset{\‾}{q}}_e^T{\underset{\‾}{q}}_e+{\mathrm{\ω}}_e^T{\mathrm{\ω}}_e---\left(11\right)$

Wherein q _ebe the arrow portion of deviation between two attitude quaternions, ω _eit is the deviation between two angular velocity.

Then in planning space, generate M random node, be then optimized screening according to evaluation function f (q, w), find out nearest node and expand.

$f(q,w)={\underset{\‾}{q}}_e^{T}a{\underset{\‾}{q}}_e+{\mathrm{\ω}}_e^T{\mathrm{b\ω}}_e---\left(12\right)$

Wherein, a and b is weighting matrix.

Expand after optimization nearest node, now should consider kinetic part.

State equation (1) and (2) are carried out single order Euler linearization,

q(k+1)＝q(k)+△T(0.5Q(k)ω(k+1))

(13)

Jω(k+1)＝Jω(k)+△T(u(k+1)-ω(k)×Jω(k))

And then be expressed as compacter linear restriction expression formula

FX＝E (14)

Wherein,

X＝[u(k+1) ^T,ω(k+1) ^T,q(k+1) ^T] ^T(15)

$F=\left[\begin{array}{ccc}-{\mathrm{\ΔTI}}_{3\×3}& J& 0_{3\×4}\\ 0_{4\×3}& -0.5\mathrm{\ΔTQ}\left(k\right)& I_{4\×4}\end{array}\right]---\left(16\right)$

$E=\left[\begin{array}{c}\mathrm{J\ω}\left(k\right)-\mathrm{\ΔT}[\mathrm{\ω}\left(k\right)\×]\mathrm{J\ω}\left(k\right)\\ q\left(k\right)\end{array}\right]---\left(17\right)$

$Q\left(k\right)=\left[\begin{array}{ccc}-q_1\left(k\right)& -q_2\left(k\right)& -q_3\left(k\right)\\ q_0\left(k\right)& -q_3\left(k\right)& q_2\left(k\right)\\ q_3\left(k\right)& q_0\left(k\right)& -q_1\left(k\right)\\ -q_2\left(k\right)& q_1\left(k\right)& q_0\left(k\right)\end{array}\right]---\left(18\right)$

Node new after calculating expansion thus, then the constraint judging whether to meet step 2 is carried out, if do not meet, re-start search, judge whether again to arrive targeted attitude if meet, if arrive targeted attitude, stop search and return whole result, if do not arrive targeted attitude to proceed search.The path node of constraint can be met after search terminates and generate the control moment of node.Satisfied constraint refers to meet the various Complex Constraints such as Dynamic Constraints, Practical Project constraint, geometrical constraint.

Step 4: utilize the attitude maneuver method designed, realize from reference attitude motor-driven to targeted attitude.Under the reference attitude providing detector and targeted attitude condition, the path node meeting constraint and the control moment generating node is cooked up by above-mentioned steps one, two, three, detector attitude maneuver path and required control moment can be generated, realize detector according to generation detector attitude maneuver path and required control moment motor-driven to targeted attitude.

Beneficial effect:

1, the present invention utilizes the Uniformly distributed node of ORRT algorithm to configuration space to carry out stochastic sampling, make search can spread all over whole configuration space, with greedy extended mode incremental expansion in safe space, improve global search speed, evaluation function is adopted to weigh preferentially extensions path, improve the searching efficiency of secure path node, shorten the path planning time.

2, the attitude maneuver path that the present invention cooks up not only considers the physical constraint faced in engineering, and fully meet the Dynamic Constraints and geometrical constraint that detector faces, avoid detector sensitive element in attitude maneuver process to be damaged by high light celestial body, guarantee topworks does not exceed the maximum execution moment that it bears, and improves the stability in detector attitude maneuver.

Accompanying drawing explanation

Geometrical constraint schematic diagram suffered by Fig. 1 detector attitude maneuver;

Fig. 2 is the path planning process figure of a kind of deep space probe autonomous attitude maneuver of the present invention control method;

Detector attitude maneuver path examples figure under Fig. 3 celestial coordinate system;

The control moment instance graph of Fig. 4 plane-generating.

Embodiment

In order to better objects and advantages of the present invention are described, below in conjunction with accompanying drawing and example, summary of the invention is described further.

Embodiment 1:

A kind of deep space probe autonomous attitude maneuver control method disclosed in this example, specific implementation step is as follows:

Step one: according to ephemeris moment in attitude maneuver moment at that time, determine that the vector of the sun under inertial system is r _i=[1 0 0] ^t, then determine that the pose transformation matrix that inertia is tied under detector body coordinate system is $C_{\mathrm{BI}}=\left[\begin{array}{ccc}0.4602& -0.6354& 0.6201\\ -0.8786& -0.4261& 0.2156\\ 0.1272& -0.6440& -0.7543\end{array}\right],$ Therefore the position vector v of the sun under detector body system can be indicated _b=C _bIr;

According to detector self mounting characteristics, determine the position vector of sensor under detector body system.In order to verification algorithm validity, if value has 8 sensors, their parameter is as following table:

Step 2: the problem indicating topworks's boundedness quantitatively.

Due in Practical Project, control moment bounded, there is constraint in its dynamics:

|T _i|≤1Nm

In attitude maneuver process, because measuring sensor exists measurement range or the normal work in order to some instrument, need by angular velocity amplitude limit within the specific limits,

|ω _i|≤0.05rad/s

Quadratic constraints form by (9) formula: q ^taq≤0, can obtain A ₁, A ₂, A ₃, A ₄, A ₅, A ₆, A ₇, A ₈eight geometrical constraints.

Step 3: the various Complex Constraints proposed in conjunction with above step 2, utilize ORRT planning algorithm to draw autonomous attitude maneuver control method.

First carry out initialization, then generate planning space, then in planning space, generate 10 random nodes, be then optimized screening according to evaluation function f (q, w), find out nearest node and expand.

$f(q,w)={\underset{\‾}{q}}_e^{T}a{\underset{\‾}{q}}_e+{\mathrm{\ω}}_e^T{\mathrm{b\ω}}_e$

Wherein, $a=\left[\begin{array}{ccc}1& & \\ & 1& \\ & & 1\end{array}\right],b\left[\begin{array}{ccc}1.3& & \\ & 1.3& \\ & & 1.3\end{array}\right].$

Expand after optimization nearest node, now should consider kinetic part.

State equation (1) and (2) are carried out single order Euler linearization,

q(k+1)＝q(k)+△T(0.5Q(k)ω(k+1))

Jω(k+1)＝Jω(k)+△T(u(k+1)-ω(k)×Jω(k))

And then be expressed as compacter linear restriction expression formula

FX＝E

Wherein,

X＝[u(k+1) ^T,ω(k+1) ^T,q(k+1) ^T] ^T

$F=\left[\begin{array}{ccc}-{\mathrm{\ΔTI}}_{3\×3}& J& 0_{3\×4}\\ 0_{4\×3}& -0.5\mathrm{\ΔTQ}\left(k\right)& I_{4\×4}\end{array}\right]$

$E=\left[\begin{array}{c}\mathrm{J\ω}\left(k\right)-\mathrm{\ΔT}[\mathrm{\ω}\left(k\right)\×]\mathrm{J\ω}\left(k\right)\\ q\left(k\right)\end{array}\right]$

$Q\left(k\right)=\left[\begin{array}{ccc}-q_1\left(k\right)& -q_2\left(k\right)& -q_3\left(k\right)\\ q_0\left(k\right)& -q_3\left(k\right)& q_2\left(k\right)\\ q_3\left(k\right)& q_0\left(k\right)& -q_1\left(k\right)\\ -q_2\left(k\right)& q_1\left(k\right)& q_0\left(k\right)\end{array}\right]$

Node new after calculating expansion thus, then the constraint judging whether to meet step 2 is carried out, if do not meet, re-start search, judge whether again to arrive targeted attitude if meet, if arrive targeted attitude, stop search and return whole result, if do not arrive targeted attitude to proceed search.The path node of constraint can be met after search terminates qwith the control moment u generating node.Satisfied constraint refers to meet the various Complex Constraints such as Dynamic Constraints, Practical Project constraint, geometrical constraint.Detail flowchart as shown in Figure 2.

Step 4: utilize the attitude maneuver method designed, realize from reference attitude motor-driven to targeted attitude.Provide the reference attitude of detector q(0)=[-0.4746-0.4746 0.4746 0.5695] ^tand targeted attitude q(t)=[1 00 0] ^t, the path node meeting constraint is cooked up by above-mentioned steps one, two, three qwith the control moment u generating node, path node is represented under celestial coordinate system and obtains detector attitude maneuver path as shown in Figure 3, required control moment as shown in Figure 4.

Scope is not only confined to embodiment, embodiment for explaining the present invention, all changes with the present invention under same principle and design condition or revise all within protection domain disclosed by the invention.

Claims (2)

1. a deep space probe autonomous attitude maneuver control method, it is characterized in that: using ORRT (Optimized Rapidly Exploring Random Tree) algorithm as paths planning method, stochastic sampling is carried out to the Uniformly distributed node of configuration space, then carry out weighing preferentially extensions path, with greedy extended mode incremental expansion in safe space, detector attitude maneuver Dynamic Constraints model is set up respectively under detector body coordinate system, Practical Project restricted model and detector geometrical constrain model, be met the path node of constraint and generate the control moment of node, and then generate detector attitude maneuver path and required control moment, detector is realized motor-driven to targeted attitude according to generation detector attitude maneuver path and required control moment.

2. a kind of deep space probe autonomous attitude maneuver control method as claimed in claim 1, is characterized in that: specific implementation step is as follows,

Step one: according to ephemeris moment in attitude maneuver moment at that time, determines the relevant bright expression r of celestial body under inertial system _i, then determine inertia be tied to detector body coordinate system under pose transformation matrix C _bI, therefore can indicate the position vector v of bright celestial body under detector body system _b;

According to detector self mounting characteristics, determine the position vector r of sensor under detector body system _b;

Step 2: set up detector attitude maneuver Dynamic Constraints model, set up the Practical Project restricted model faced in engineering; Set up the geometrical constrain model that detector faces; Concrete point three steps:

The first step: set up detector attitude maneuver Dynamic Constraints model;

Attitude kinematics and kinetics equation are expressed as follows:

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{Q\ω}=\frac{1}{2}\mathrm{\Ω\ω}---\left(1\right)$

Jω＝T-ω×Jω (2)

Wherein, q=[q ₀, q ₁, q ₂, q ₃] ^t, meet normalization constraint || q|| ₂=1, represent 2-norm; ω=[ω ₁, ω ₂, ω ₃] ^tthe expression of angular velocity under body series of spacecraft relative inertness system, and

$Q=\left[\begin{array}{ccc}-q_1& -q_2& -q_3\\ q_0& -q_3& q_2\\ q_3& q_0& -q_1\\ -q_2& q_1& q_0\end{array}\right],\mathrm{\Ω}=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_1& -{\mathrm{\ω}}_2& -{\mathrm{\ω}}_3\\ {\mathrm{\ω}}_1& 0& {\mathrm{\ω}}_3& -{\mathrm{\ω}}_2\\ {\mathrm{\ω}}_2& -{\mathrm{\ω}}_3& 0& {\mathrm{\ω}}_1\\ {\mathrm{\ω}}_3& {\mathrm{\ω}}_2& -{\mathrm{\ω}}_1& 0\end{array}\right]---\left(3\right)$

J=diag (J ₁, J ₂, J ₃) represent the inertial matrix that spacecraft opposing body is, u=[T ₁, T ₂, T ₃] ^tfor the component of control moment under body series;

Second step: set up the Practical Project restricted model faced in engineering; Described Practical Project constraint comprises control moment bounded and angular velocity bounded;

Due in Practical Project, control moment bounded is expressed as:

|T _i|≤γ _Ti(4)

In attitude maneuver process, because measuring sensor exists measurement range or the normal work in order to some instrument, need by angular velocity amplitude limit within the specific limits, angular velocity bounded is expressed as:

|ω _i|≤γ _ωi(5)

3rd step: set up geometrical constrain model;

$r_B^T{v}_B\≤\cos \mathrm{\θ}---\left(6\right)$

Wherein, r _brepresent the direction vector of sensitive element under body series, v _bfor the direction vector of high light celestial body under body coordinate is in order to avoid spacecraft is in mobile process, high light celestial body light enters in the visual field of optical sensing element, must ensure that the angle between the direction of visual lines vector of this type of Sensitive Apparatus and high light celestial body direction vector can not lower than threshold value θ;

Convert (6) formula to hypercomplex number representation

$v_B=C_{\mathrm{BI}}{r}_I=r_I-2{\underset{\‾}{q}}^T\underset{\‾}{q}{r}_I+2{\underset{\‾}{\mathrm{qq}}}^T{r}_I+{2q}_0\left(\right[r_I\×\left]\underset{\‾}{q}\right)---\left(7\right)$

Wherein, r _irepresent the direction vector component under inertial system of detector to high light celestial body, C _bIrepresent the attitude cosine matrix of spacecraft;

Wherein, q=[q ₁, q ₂, q ₃] ^tfor hypercomplex number vector section; [r _i×] be multiplication cross matrix, concrete form is

$[r_I\×]=\left[\begin{array}{ccc}0& -r_{I3}& r_{I2}\\ r_{I3}& 0& -r_{I1}\\ -r_{I2}& r_{I1}& 0\end{array}\right]---\left(8\right)$

Following form is identical; (6) are expressed as compacter form, obtain the quadratic constraints form in (9) formula;

q ^TAq≤0 (9)

Wherein,

$A=\left[\begin{array}{cc}{r}_B^T{r}_I-\cos {\mathrm{\θ}}_1& -r_B^T[r_I\×]\\ -r_B^T[r_I\×]& {2r}_I{r}_B^T-(r_B^T{r}_I+\cos {\mathrm{\θ}}_1)I_3\end{array}\right]---\left(10\right)$

Step 3: the various Complex Constraints proposed in conjunction with above-mentioned steps two, utilize ORRT planning algorithm to draw autonomous attitude maneuver control method;

ORRT is the greedy expansion process in consistent attitude sample space, comprises random targets node sample, adjacent node calculates and new node is expanded; First carry out initialization, then generate planning space, effective metric function must be chosen before this:

$\mathrm{\ρ}={\underset{\‾}{q}}_e^T{\underset{\‾}{q}}_e+{\mathrm{\ω}}_e^T{\mathrm{\ω}}_e---\left(11\right)$

Wherein q _ebe the arrow portion of deviation between two attitude quaternions, ω _eit is the deviation between two angular velocity;

Then in planning space, generate M random node, be then optimized screening according to evaluation function f (q, w), find out nearest node and expand;

$f(q,w)={\underset{\‾}{q}}_e^{T}a{\underset{\‾}{q}}_e+{\mathrm{\ω}}_e^{T}b{\mathrm{\ω}}_e---\left(12\right)$

Wherein, a and b is weighting matrix;

Expand after optimization nearest node, now should consider kinetic part;

State equation (1) and (2) are carried out single order Euler linearization,

q(k+1)＝q(k)+△T(0.5Q(k)ω(k+1)) (13)

Jω(k+1)＝Jω(k)+△T(u(k+1)-ω(k)×Jω(k))

And then be expressed as compacter linear restriction expression formula

FX＝Ε (14)

Wherein,

X＝[u(k+1) ^T,ω(k+1) ^T,q(k+1) ^T] ^T(15)

$F=\left[\begin{array}{ccc}-\mathrm{\Δ}{\mathrm{TI}}_{3\×3}& J& 0_{3\×4}\\ 0_{4\×3}& -0.5\mathrm{\ΔTQ}\left(k\right)& I_{4\×4}\end{array}\right]---\left(16\right)$

$E=\left[\begin{array}{c}\mathrm{J\ω}\left(k\right)-\mathrm{\ΔT}[\mathrm{\ω}\left(k\right)\×]\mathrm{J\ω}\left(k\right)\\ q\left(k\right)\end{array}\right]---\left(17\right)$

$Q\left(k\right)=\left[\begin{array}{ccc}-q_1\left(k\right)& -q_2\left(k\right)& -q_3\left(k\right)\\ q_0\left(k\right)& -q_3\left(k\right)& q_2\left(k\right)\\ q_3\left(k\right)& q_0\left(k\right)& -q_1\left(k\right)\\ -q_2\left(k\right)& q_1\left(k\right)& q_0\left(k\right)\end{array}\right]---\left(18\right)$

Node new after calculating expansion thus, then the constraint judging whether to meet step 2 is carried out, if do not meet, re-start search, judge whether again to arrive targeted attitude if meet, if arrive targeted attitude, stop search and return whole result, if do not arrive targeted attitude to proceed search; The path node of constraint can be met after search terminates and generate the control moment of node; Satisfied constraint refers to meet the various Complex Constraints such as Dynamic Constraints, Practical Project constraint, geometrical constraint;

Step 4: utilize the attitude maneuver method designed, realize from reference attitude motor-driven to targeted attitude; Under the reference attitude providing detector and targeted attitude condition, the path node meeting constraint and the control moment generating node is cooked up by above-mentioned steps one, two, three, detector attitude maneuver path and required control moment can be generated, realize detector according to generation detector attitude maneuver path and required control moment motor-driven to targeted attitude.