JP2021015351A - Guidance program, guidance method, and guidance device - Google Patents

Abstract

To provide a guidance program, a guidance method, and a guidance device capable of improving the movement performance while improving the energy consumption efficiency of a moving body.SOLUTION: From a tentative initial value of parameter that describes the control amount, including: a thrust direction time history indicating the time history in the direction in which a moving body outputs thrust, a switching time indicating the timing for switching the magnitude of the thrust of the moving body, and a terminal time indicating the time at the orbital termination of the moving body, or a provisional initial value of a parameter explaining a terminal state indicating the state of the moving body at the terminal time, a guidance program causes a computer mounted on the moving body to derive the switching time based on the switching function theory, and to predict a part or all of the thrust direction time history, the terminal time, and the terminal state based on the derived switching time and the initial value.SELECTED DRAWING: Figure 2

Description

The present invention relates to guidance programs, guidance methods, and guidance devices.

The missile guidance system called "direct guidance", which originated from the space shuttle, is an algorithm that outputs the orbit and control commands for precisely guiding to the target, that is, the optimum solution online in real time. Direct guidance is a method that already has a lot of flight records. Direct derivation involves (1) a prediction process that predicts the terminal state quantity analytically or numerically, and (2) a correction that corrects the assumed solution using the variational principle so that the predicted terminal state quantity becomes the desired value. It is realized by repeating the two operations of the process and making the assumed solution asymptotic to the optimum solution. Specifically, for example, it is obtained by numerically solving simultaneous nonlinear equations of the same number as the unknown for the unknown that explains the control amount and the terminal state.

After specifying the initial position and velocity and the terminal position and velocity, the objective function is to minimize the fuel consumption when the gravity is constant, the upper and lower limits of thrust or thrust acceleration are constant, and the influence of the atmosphere is negligible. The optimal solution at that time is theoretically known in the field of optimal control theory. Except in special cases, the magnitude of thrust is changed up to twice, such as maximum-minimum-maximum, within the upper and lower limits of the constraint. The timing of changing the magnitude of thrust is determined based on the value of a function called a switching function. The direction of thrust is determined based on a method called the bilinear tangent law.

Practically, direct induction has been used to find only the optimum thrust direction and end time that minimizes fuel consumption. For example, in the Space Shuttle, the magnitude of thrust is treated as a known time function, and direct guidance is used as a method of finding only the optimum thrust direction and end time. The direct guidance system, which adds the timing of changing the magnitude of thrust to this, has not been put into practical use.

Conventionally, it has been considered difficult to construct an inductive law by directly calculating the switching function described above from the viewpoint of numerical stability and the like. In relation to this, instead of directly calculating the switching function, by adding the change timing of the thrust magnitude to the optimization parameters and solving the optimization problem online, the optimum switching time, thrust direction, and termination Guidance rules for deriving the time are known (see Non-Patent Document 1).

P. Lu, "Propellant-Optimal Powered Descent Guidance," Journal of Guidance, Control, and Dynamics, vol. 41, no. 4, pp. 813--826, 2018.

For example, in guidance problems that require large position correction within a short period of time, such as high-precision landing on gravitational objects such as the moon and planets, incorporating thrust switching can reduce fuel consumption and large movement performance ( It is effective for high-precision induction performance). Therefore, there is a need for a direct guidance algorithm that simultaneously calculates the optimum thrust direction, thrust switching, and end time in real time.

However, in the conventional technique, some thrust switching times are obtained offline, the switching time is set as a fixed parameter, and the remaining switching time is set as an optimization parameter independent of other control variables to execute online optimization. It was necessary to make assumptions that the scope of application would be limited. As a result, if a suitable fixed parameter for the switching time cannot be found in advance, more fuel will be consumed and the movement performance may be deteriorated. Further, such a problem is not limited to a flying object such as a space shuttle, but is common to all moving objects to which the guidance law is applied.

The present invention has been made in consideration of such circumstances, and provides a guidance program, a guidance method, and a guidance device capable of improving the movement performance while improving the energy consumption efficiency of the moving body. That is one of the purposes.

In one aspect of the present invention, a computer mounted on a moving body has a thrust direction time history showing a time history in a direction in which the moving body outputs a thrust based on a switching function theory, and a magnitude of the thrust of the moving body. A tentative initial value of a parameter for explaining a control amount including a switching time indicating a switching timing and an end time indicating a time at the end of an orbit for navigating the moving body, or a provisional initial value of a parameter indicating the end time of the moving body. The switching time is derived from the provisional initial values of the parameters for explaining the termination state indicating the state, and the thrust direction time history, the termination time, and the termination time are based on the derived switching time and the initial value. It is a guidance program for predicting a part or all of the terminal states.

According to one aspect of the present invention, it is possible to improve the moving performance while improving the energy consumption efficiency of the moving body.

It is a figure which shows an example of the moving body M of an embodiment. It is a figure which shows an example of the structure of the navigation system 100 of an embodiment. It is a figure which shows typically the descent sequence at the time of landing a moving body M at a target point of a celestial body PL. It is a figure which shows an example of the structure of the guidance calculation unit 120 which concerns on embodiment. It is a flowchart which shows an example of the flow of a series of processing of TEG by the guidance calculation unit 120 which concerns on embodiment. It is a flowchart which shows the flow of a series of processing of the calculation process of the terminal state quantity by the induction calculation unit 120 which concerns on embodiment. It is a figure which shows an example of the switching function κ (t) of the pattern A. It is a figure which shows an example of the optimum switching of the thrust based on the switching function κ (t) of the pattern A. It is a figure which shows an example of the switching function κ (t) of the pattern B. It is a figure which shows an example of the optimum switching of the thrust based on the switching function κ (t) of the pattern B. It is a figure which shows an example of the switching function κ (t) of the pattern C. It is a figure which shows an example of the optimum switching of the thrust based on the switching function κ (t) of the pattern C. It is a figure which shows an example of the switching function κ (t) of the pattern D. It is a figure which shows an example of the optimum switching of the thrust based on the switching function κ (t) of the pattern D. It is a figure which shows an example of the switching function κ (t) of the pattern E. It is a figure which shows an example of the optimum switching of the thrust based on the switching function κ (t) of the pattern E.

Hereinafter, embodiments of the guidance program, guidance method, and guidance device of the present invention will be described with reference to the drawings. The guidance device in the present embodiment is a device that performs various processes by executing a guidance program by a computer mounted on the mobile body.

FIG. 1 is a diagram showing an example of the mobile body M of the embodiment. The mobile body M may be, for example, a spacecraft (space probe) that lands on a gravitational celestial body such as the moon or a planet (hereinafter, simply referred to as a celestial body PL) and searches for the celestial body PL. A navigation device 100 using a guidance device is mounted on the mobile body M, which is a spacecraft. Further, in addition to the navigation device 100, the moving body M, which is a spacecraft, is equipped with a camera 10, an inertial measurement unit (IMU) 20, a radar 30, and a propulsion force output device 40. ..

The camera 10 is provided, for example, below the moving body M, and when the moving body M lands on the celestial body PL, the camera 10 images the ground surface of the celestial body PL. The "lower side" is, for example, the housing side provided with legs in contact with the ground surface of the celestial body PL. In other words, the camera 10 is provided on the side (vertically downward) in which the gravity of the celestial body PL acts on the moving body M. The camera 10 outputs the captured image to the navigation device 100.

The inertial measurement unit 20 includes, for example, a triaxial acceleration sensor and a triaxial gyro sensor. The inertial measurement unit 20 outputs the detected values detected by these sensors to the navigation system 100. The values detected by the inertial measurement unit 20 include, for example, accelerations and / or angular velocities in the horizontal, vertical, and depth directions, and velocities (rates) of pitch, roll, and yaw axes.

The radar 30 is provided, for example, under the moving body M, and when the moving body M lands on the celestial body PL, it irradiates the ground surface of the celestial body PL with radio waves and receives the reflected waves of the irradiated radio waves. The value detected by the radar 30 includes, for example, the altitude and speed of the moving body M relative to the ground surface. The radar 30 is, for example, a Doppler radar.

The propulsion output device 40 includes, for example, a reaction wheel, a control moment gyro, a thruster for trajectory control and attitude control (for example, a thruster using chemical fuel), a thrust vector controller (TVC), and the like. For example, the thrust output device 40 thrusters by driving an actuator such as a reaction wheel or a control moment gyro to change the direction of the aircraft with respect to the ground surface, or by changing the thruster injection direction with respect to the aircraft by TVC. The output direction of the thrust (propulsive force) generated by is changed to an arbitrary direction.

By executing the guidance program, the navigation device 100 uses, for example, an image of the moving body M, which is a spacecraft, captured by the camera 10, the detection result of the inertial measurement unit 20, and the detection result of the radar 30. And let it sail. Hereinafter, navigation using the image captured by the camera 10 will be referred to as "image navigation", navigation using the detection result of the inertial measurement unit 20 will be referred to as "inertial navigation", and navigation using the detection result of the radar 30 will be referred to as "inertial navigation". It will be referred to as "radar navigation". The moving body M is not limited to a spacecraft, and may be another moving body such as a UAV (Unmanned Aerial Vehicle) flying on the earth. Hereinafter, as an example, the mobile body M will be described as being a spacecraft.

FIG. 2 is a diagram showing an example of the configuration of the navigation device 100 of the embodiment. The navigation device 100 includes, for example, a communication unit 102, a control unit 110, and a storage unit 130.

The communication unit 102 wirelessly communicates with a monitoring device on the earth using radio waves in a frequency band such as that used in a telemeter line or the like. The monitoring device on the earth can also control the position of the moving body M to the target position of the celestial body PL by remotely controlling (remotely guiding) the navigation device 100 via the communication unit 102, for example.

The control unit 110 includes, for example, an acquisition unit 112, an image navigation calculation unit 114, an inertial navigation calculation unit 116, a radar navigation calculation unit 118, a guidance calculation unit 120, and a control calculation unit 122.

The components of the control unit 110 are realized, for example, by a processor such as a CPU (Central Processing Unit) or a GPU (Graphics Processing Unit) executing an induction program stored in the storage unit 130. Further, some or all of the components of the control unit 110 may be realized by hardware such as LSI (Large Scale Integration), ASIC (Application Specific Integrated Circuit), or FPGA (Field-Programmable Gate Array). However, it may be realized by the collaboration of software and hardware. Further, the guidance program referred to by the processor may be stored in the storage unit 130 in advance, or is stored in a removable storage medium such as a DVD or a CD-ROM, and the storage medium is stored in the navigation device 100. It may be installed in the storage unit 130 from the storage medium by being attached to the drive device.

The storage unit 130 is realized by, for example, a storage device such as an HDD (Hard Disc Drive), a flash memory, an EEPROM (Electrically Erasable Programmable Read Only Memory), a ROM (Read Only Memory), and a RAM (Random Access Memory). In addition to various programs such as firmware and application programs (including guidance programs), the storage unit 130 stores terrain data 132, reference trajectory information 134, switching pattern information 136, and the like.

The terrain data 132 is, for example, data in which the three-dimensional shape of the ground surface of the celestial body PL landed on the moving body M is modeled. The model showing the three-dimensional shape of the ground surface of the celestial body PL includes visually conspicuous features (feature points) such as craters and rocks that form irregularities on the ground surface of the celestial body PL.

The reference orbit information 134 is information that defines a reference orbit (nominal orbit). The reference orbit is the state quantity such as the position, velocity, and posture that the moving body M should take when guiding the moving body M to the celestial body PL, the direction in which the thrust is output (thrust direction), and the timing at which the thrust is output. The control amount such as the indicated time (thrust switching time) is, for example, information specified for each time interval or distance interval, or a mathematical formula that analytically expresses the state quantity or control quantity and a parameter value that expresses the reference trajectory. It is a set.

The switching pattern information 136 is information that defines patterns of a plurality of switching functions described later.

The acquisition unit 112 acquires an image from the camera 10, acquires detection values such as acceleration and velocity from the inertial measurement unit 20, and acquires detection values such as the distance from the radar 30 to the ground surface of the celestial body PL. Further, the acquisition unit 112 acquires the reference orbit information 134 from the monitoring device on the ground via the communication unit 102, and stores the reference orbit information 134 in the storage unit 130 to store the reference orbit information 134 stored in the storage unit 130. Can be updated.

The image navigation calculation unit 114 collates (compares) the image acquired from the camera 10 by the acquisition unit 112 with the terrain data 132 stored in the storage unit 130, and derives the state quantity of the moving body M. For example, the image navigation calculation unit 114 performs image processing on the image of the camera 10, extracts feature points such as rocks and craters, and includes the extracted feature points and the three-dimensional model shown by the terrain data 132. By pattern matching with the feature points, the three-dimensional position of the moving body M with respect to the celestial body PL is derived as a state quantity.

The inertial navigation calculation unit 116 starts inertial navigation using the detection result of the inertial measurement unit 20 after receiving the start signal from the ground monitoring device or based on the start time preset in the storage unit 130, for example. Then, a state quantity that quantitatively indicates the state of the moving body M is derived. The "state quantity" includes, for example, position, velocity, quaternion from the airframe coordinate system to the inertial coordinate system, and the like. The machine coordinate system is, for example, a coordinate system fixed to the moving body M with the center of gravity of the moving body M as the origin of the coordinates. The inertial coordinate system is, for example, a coordinate system fixed to outer space or a celestial body. Further, the "state quantity" may include a sensor bias value with respect to the acceleration or the angular velocity detected by the inertial measurement unit 20.

For example, when the altitude of the moving body M with respect to the ground surface of the celestial body PL becomes equal to or lower than a predetermined altitude, the radar navigation calculation unit 118 determines the state of the moving body M by radar navigation using the detection result of the radar 30. Derivation of the state quantity shown quantitatively. This state quantity includes, for example, the distance of the celestial body PL from the ground surface, that is, the relative position of the moving body M with respect to the celestial body PL, the relative velocity, and the like.

Generally, when remote control is performed by wireless communication with a monitoring device on the earth, the order of the distance between the earth and the celestial body PL may be 100 million kilometers or more, and it is converted into the round-trip propagation time of radio waves for several tens of minutes. It may take more than that. Therefore, when the altitude of the moving body M with respect to the ground surface of the celestial body PL is lowered and the moving body M is landed on the celestial body PL, remote control based on a command from the ground may not be able to cope with it. Therefore, in order to perform autonomous navigation, the control unit 110 performs image navigation using the image of the camera 10, inertial navigation using the detection value of the inertial measurement unit 20, radar navigation using the detection value of the radar 30, and the like. The state quantity of the moving body M is derived by using it in combination.

The guidance calculation unit 120 uses a direct guidance method (also referred to as a direct guidance law) to reach a target point on the ground surface of the celestial body PL (for example, a point with strong scientific interest or a point with excellent habitability). Predict the trajectory of the moving body M. Specifically, the guidance calculation unit 120 calculates the state amount of the moving body M at the end of the orbit (hereinafter referred to as the terminal state amount) from the current state amount of the moving body M (hereinafter referred to as the initial state amount). It is derived by sequentially (online) searching for solutions that satisfy the conditions for flying with the minimum fuel consumption. The orbital end may be, for example, a position several kilometers above the target point of the celestial body PL, or may be the target point of the celestial body PL itself, and can be arbitrarily given. Hereinafter, as an example, the end of the orbit will be described as being above the target point of the celestial body PL.

The control calculation unit 122 controls the propulsion force output device 40 based on the difference amount between the state amount derived by the image navigation calculation unit 114 and the trajectory (state amount) predicted by the guidance calculation unit 120. Image navigation is performed to control states such as the position, speed, acceleration, and posture of the moving body M. Further, the control calculation unit 122 controls the propulsion force output device 40 based on the difference amount between the state amount derived by the inertial navigation calculation unit 116 and the trajectory (state amount) predicted by the guidance calculation unit 120. Inertial navigation is performed to control the position, speed, posture, and other states of the moving body M. Further, the control calculation unit 122 controls the propulsion force output device 40 based on the difference amount between the state quantity derived by the radar navigation calculation unit 118 and the trajectory (state quantity) predicted by the guidance calculation unit 120. Then, radar navigation is performed to control states such as the position, speed, and attitude of the moving body M. As a result, the control calculation unit 122 pinpoints the moving body M at the target point of the celestial body PL.

FIG. 3 is a diagram schematically showing a descent sequence when the moving body M is landed at the target point of the celestial body PL. The descent sequence includes, for example, a PD (Powered Descent) phase and a VD (Vertical Descent) phase. In the PD phase, the moving body M is lowered by combining image navigation and inertial navigation by image matching using the camera 10 in a complex manner. Under the PD phase, the control calculation unit 122 may temporarily shut off the throttle at the time of image matching to stop the output of thrust. In the VD phase, image navigation by image matching using the camera 10, inertial navigation using the detection value of the inertial measurement unit 20, and radar navigation using the detection value of the radar 30 are combined in a complex manner to perform PD. The moving body M is vertically lowered to the target point of the celestial body PL while reducing the vertical position error generated in the phase.

For example, in the first section from the point P1 to the point P2, the second section from the point P2 to the point P3, and the third section from the point P3 to the point P4, image navigation is performed according to the PD phase. When the altitude of the moving body M becomes equal to or lower than the predetermined altitude at the point P4, image navigation, inertial navigation, and radar navigation are performed according to the VD phase.

Of the first to third sections of the PD phase, especially in the third section, in order to smoothly transition to the VD phase, it is necessary to correct a position error of several kilometers in a short time (for example, about 100 seconds). Therefore, in the present embodiment, the guidance calculation unit 120 uses TEG (Throttled Explicit Guidance), which is a kind of direct guidance method, to derive the state quantity of the moving body M before the transition to the VD phase, that is, the terminal state quantity.

With TEG, the optimum solution of the throttle control timing that adjusts the thrust can be explicitly (explicitly) obtained, and the fuel can be optimized by switching the output timing of the thrust based on the optimum solution. It is a direct guidance system. The TEG includes an initialization process for initializing the seven unknowns, which will be described later, and a prediction / correction process for converging the seven unknowns so that the termination state error is small (for example, approaching zero).

FIG. 4 is a diagram showing an example of the configuration of the guidance calculation unit 120 according to the embodiment. The induction calculation unit 120 includes, for example, an initialization unit 120A, a derivation unit 120B, a correction unit 120C, and a prediction unit 120D. Hereinafter, the TEG processing of these components will be described in detail using a flowchart.

FIG. 5 is a flowchart showing an example of a flow of a series of TEG processes by the induction calculation unit 120 according to the embodiment. The processing of this flowchart may be repeated at a predetermined cycle, or may be executed at an arbitrary timing.

[Initialization process]
First, the initialization unit 120A sets a provisional initial value of an unknown parameter (hereinafter, referred to as an unknown number) explaining the control amount or the terminal state, and the terminal state amount and the target that can be obtained from the parameter. The error from the end state quantity (hereinafter referred to as the end state error) is calculated (step S100). Formula (1) represents an initial value of an unknown (hereinafter referred to as an initial unknown), formula (2) represents a termination state error, and formula (3) represents an initialized termination state error. As will be described later, the termination state error is expressed by the distance between multidimensional vectors, that is, the Euclidean norm. Hereinafter, the alphabet with (→) is used to represent a vector. The "T" symbol in the upper right corner of the letter means transpose of a vector or matrix.

The initial unknown u ₀ (→) is the constant ν _R (→) related to the contingent variable of the position of the moving body M, the constant ν _V (→) related to the contingent variable of the velocity of the moving body M, and the time at the end of the orbit (hereinafter). , Called the end time) t _f and is represented by a multidimensional vector. The constants ν _R (→) and ν _V (→) are represented by three-dimensional vectors, respectively. That is, the initial unknown number u ₀ (→) is a 7-dimensional vector containing seven unknowns as elements.

For example, the initialization unit 120A initializes the initial unknown number u ₀ (→) by replacing it with a predetermined numerical value (for example, a value representing a reference trajectory), and ends state error h (u ₀ (→)) (→). ) Is calculated. That is, the initialization unit 120A sets the values of the seven unknowns (three-dimensional constant ν _R (→), three-dimensional constant ν _V (→), and one-dimensional end time t _f ) as provisional values ( The termination state error h (u ₀ (→)) (→) is calculated as a so-called preset value). Alternatively, the initialization unit 120A in the case of those TEG run is second or later, and as an initialization, the initial state quantity _{u 0} (→) the previous TEG runtime final solution _{u k} (→), The termination state error h (u ₀ (→)) (→) may be calculated. As described above, the initial unknown number u ₀ (→) and the end state error h (u ₀ (→)) (→) are not necessarily guaranteed to be the optimum values, and are arbitrary values for which it is not certain whether they are the optimum values. The seven initialized unknowns, that is, the initial unknowns u ₀ (→), are an example of “provisional initial values of parameters”.

In equation (2), w _R (→), w _V (→), and _WH represent the weighting factors for position, velocity, and Hamiltonian termination state error, respectively. The termination state error h (u (→)) (→) is the termination time with the target position vector R _f (→) at the termination time t _f , the unknowns u (→), and the switching time (t1 and t2) as explanatory variables. the difference between the position vector R _(t f) _(→) in t _f, and multiplied by the weighting coefficient vector _{w R} (→), the target speed vector _{V f} at the end time _{t f} (→), unknown u ( →) and the difference from the speed vector V (t _f ) (→) at the end time t _f with the switching time (t1 and t2) as explanatory variables multiplied by the weighting coefficient vector w _V (→) and the end It is defined as a total of 7-dimensional vector obtained by multiplying the Hamiltonian of time by the weighting factor _WH .

Terminal state error h (u (→)) ( →) is the above-mentioned constant ν _R ^T (→) and ν _V ^T (→), and the end time _{t f,} is a function of the switching times t1 and t2 to be described later .. However, by performing the process of step S200 described later, the switching times t1 and t2 can be regarded as a function of the unknown variable u (→). Therefore, the error h (u (→)) terminal state with the S200 when calculating the (→), the terminal state error h (u (→)) ( →) is a constant ν _R ^T (→) and [nu _V ^T ( It can also be regarded as a function of →) and the end time t _f , that is, a function of u (→).

Further, it is known that the Hamiltonian H (t) must be zero along the time t as a necessary condition for the controlled variable explained by the unknown number u (→) to be the minimum fuel consumption solution. Therefore, the target value of the Hamiltonian at the end time t _f is also zero, and the part related to the Hamiltonian in the mathematical formula (2) is an expression considering that the target value is zero.

Next, the initialization unit 120A determines whether or not the termination state error h (u ₀ (→)) (→) satisfies the termination condition (step S102). The termination condition is that the termination state error h (u ₀ (→)) (→) is sufficiently small, for example, that it is less than a certain threshold value ε _h (for example, a value larger than zero and sufficiently close to zero). is there.

When the end state error h (u ₀ (→)) (→) satisfies the end condition, the processing of this flowchart ends. If the end state error h (u ₀ (→)) (→) does not satisfy the end condition, the process proceeds to the repetition of the prediction / correction process described later.

[Prediction / correction process]
Deriving unit 120B is terminated state error _{h (u k (→))} (→) if does not satisfy the termination condition, in those iteration _{k, u} k (→) for terminating state error h _{(u k} (→)) (→) to derive the sensitivity matrix _{S k} of the termination state error _{h (u k (→))} (→) to reduce, to search for the best direction to be corrected u _k (→) (step S104) .. Equation (4) represents the sensitivity matrix _{S k.} Terminal state error h (u k _(→)) (→) for prediction (derivation) method, will be described separately with sub-flowchart.

The sensitivity matrix _Sk (→) is represented by the Jacobian matrix. u k _(→) represents seven of the unknowns in the k-th iteration process. For example, the derivation unit 120B, by solving a linear equation shown in Equation (5), the terminal state error h _{(u k} (→)) (→) to reduce, best of to be modified _{u k} (→) Search for directions.

For example, the derivation unit 120B normalizes the seven unknowns and the search direction in order to ensure numerical stability when solving the linear equation shown in the equation (5). The left side of the equation (6) represents the seven normalized unknowns, and the left side of the equation (7) represents the normalized search direction.

_{D i u k,} in _i and equation (7) in equation (6) represents the i-th element of each _{u k} (→) and _{d k} (→). Similarly, u _i ^* and d _i ^* represents the i-th element of the normalized vector. Further, u ^ _i represents the i-th element of the constant vector u ^ (→) used for normalization. Here, the arguments i are 1, 2, ..., 7. By using the normalized unknowns, the normalized sensitivity matrix _Sk ^* can be expressed as in equations (8) and (9).

First, derivation unit 120B in accordance with equation (8), terminal state error h a (u k _(→)) (→), by partially differentiating the normalized termination state quantity u ^*, the normalized sensitivity Calculate the matrix _Sk ^* . Next, the derivation unit 120B calculates the normalized search directions _{dk, i} ^* by solving the mathematical formula (9) which is a linear equation. Then, the derivation unit 120B calculates the unnormalized search directions _{dk and i} according to the above-mentioned mathematical formula (7).

Specifically, the derivation unit 120B calculates each i-th column of the normalized sensitivity matrix _{Sk, i} ^* based on the mathematical formula (10).

Ε _u in the equation (10) is a sufficiently small scalar value, and δu _i (→) is a sufficiently small deviation vector. Each element δu _{i, j} included in the deviation vector δu _i (→) is defined by the mathematical formula (11). The arguments j are 1, 2, ..., 7.

Before equation by deriving section 120B (10) is calculated, the prediction unit 120D has a switching time t1 and t2 will be described later, and _{(u k (→) + δu} i (→)), u k (→) each leave calculated for, in a subsequent, and _{h (u k (→) +} δu i (→)) (→), h (u k (→)) (→) and to calculate the. The calculation method of the switching times t1 and t2 will be described later.

Next, the correction unit 120C includes, (k + 1) th for processing, k-th seven unknowns included in the terminal state quantity u _{k (→)} in the iterative process, the search direction d calculated by the derivation unit 120B _k (→) is corrected based on the corrected terminal state quantity _{u k} (→), is updated to (k + 1) weight terminal state in th iteration _{u k + 1} (→) (step S106). The formula (12) represents an update formula for the state quantity.

Α _k in the mathematical formula (12) represents the scaling coefficient in the kth iteration process. The scaling coefficient α _k takes a value of 0 <α _k ≦ 1. When the terminal state quantity u _k is sufficiently close to the optimum solution, it is known to be alpha _{k =} 1. For example, as shown in the equation (13), the correction unit 120C determines the scaling coefficient α _k so that the Euclidean norm of the termination state error is reduced in the manner of the attenuation Newton method. Attenuation Newton's method is a general method for solving simultaneous nonlinear equations, and it is possible to find a globally converged solution. The product of the scaling coefficient α _k and the search direction d _k is an example of the “correction amount”.

For example, in the attenuation Newton's method, the correction unit 120C starts the calculation from α _{k, 0} = 1 and reduces the value of α _{k, 0} until the mathematical expression (14) is satisfied. Ρ in the mathematical formula (14) is a scalar value having a value of 0 <ρ _< 1. As a result, the scaling coefficient α _k that reduces the Euclidean norm of the termination state error can be _obtained at higher speed.

The correction unit 120C ends the update of the scaling coefficient α _k when the relationship shown in the mathematical formula (15) is satisfied. End condition of equation (15), the search direction d _k calculated is particularly useful when not a good way to reduce the terminal state error.

The correction unit 120C uses the scaling coefficient α _k to update the terminal state quantity uk _{+ 1} (→) of the mathematical expression (12), which is a state quantity update formula, and ends the state quantity update process.

Next, the correction unit 120C determines whether or not the end state error h (uk _{+ 1} (→)) (→) satisfies the end condition, or whether the number of iterations k + 1 satisfies the end condition (step S108). ).

For example, when the Euclidean norm of the termination state error h (uk _{+ 1} (→)) (→) is less than the threshold value ε _h , the correction unit 120C sets the updated unknown number uk _{+ 1} (→) to the control calculation unit 122. Output and end the processing of this flowchart. Further, when the number of iterations k + 1 exceeds the predetermined number of times k _max , the correction unit 120C may output the updated unknown number uk _{+ 1} (→) to the control calculation unit 122 and end the processing of this flowchart. ..

On the other hand, when the Euclidean norm of the termination state error h (uk _{+ 1} (→)) (→) is equal to or greater than the threshold value ε _h , or when the number of iterations k + 1 is equal to or less than the predetermined number k _max , the correction unit 120C sets S104. The process is returned, and the calculation process of the search direction and the update process of the end state amount are repeated.

[Calculation process of terminal state quantity]
Hereinafter, a series of processing flows of the terminal state quantity calculation processing by the induction calculation unit 120 will be described with reference to a flowchart. FIG. 6 is a flowchart showing a series of processing flows of the calculation processing of the terminal state quantity by the induction calculation unit 120 according to the embodiment.

First, in the derivation unit 120B, when the initial state quantity u ₀ (→) is initialized, the moving body M outputs thrust between the initial time 0 and the end time t _f based on the switching function theory. The switching time representing the power timing is derived (step S200). The second switching time t ₂ represents a time in the future of the first switching time t ₁ .

For example, the derivation unit 120B has a temporary first switching time t ₁ ^* and a temporary second switching time t based on the mathematical formulas (16) and (17) and the switching function κ (t) described later. ₂ ^* and is derived.

For example, the derivation unit 120B sets the first switching time t ₁ to 0 when the time t ₁ ^* is less than 0, and sets the first switching time t ₁ ^* to 0 or more and the final time t _f or less when the time t ₁ ^* is less than 0. When t ₁ is t ₁ ^* and the time t ₁ ^* exceeds the final time t _f , the first switching time t ₁ is t _f . Similarly, the derivation unit 120B sets the second switching time t ₂ to 0 when the time t ₂ ^* is less than 0, and sets the second switching when the time t ₂ ^* is 0 or more and the final time t _f or less. When the time t ₂ is t ₂ ^* and the time t ₂ ^* exceeds the final time t _f , the second switching time t _{2 is set} to t _f .

[Conditions where the upper and lower limits of thrust acceleration are constant]
The switching function κ (t) is expressed by the mathematical formula (18) when the upper and lower limits of the thrust acceleration are constant.

Ν _R (→) in the equation (18) represents a constant related to the concomitant variable of the position of the moving body M as described above, and ν _V (→) represents a constant related to the contingent variable of the velocity of the moving body M. .. Further, t _go represents the time obtained by subtracting the current time t from the end time t _f (hereinafter, referred to as the remaining flight time). That is, the remaining flight time t _go is as shown in the following mathematical formula (19).

The equation when the value of the switching function κ (t) shown in the equation (18) is zero is a quadratic equation with respect to the time t. There are five patterns A, B, C, D, and E depending on the shape of the switching function κ (t). Each of these patterns is stored in the storage unit 130 as switching pattern information 136.

For example, the derivation unit 120B solves a quadratic equation of κ (t) = 0 and derives the solution of the equation at times t ₁ ^* and t ₂ ^* . The solution of the quadratic equation may be a multiple solution. For example, if there is no solution to the quadratic equation, the derivation unit 120B may return a value satisfying t ₁ ^* = t ₂ ^* . For example, it is preferable to return the axis of κ (t) as in the formula (20). Is. To solve a quadratic equation in which the switching function κ (t) is 0, for example, in addition to deriving the switching time as the objective variable when there is a solution, when there is no solution. Even so, it involves deriving the switching time as the objective variable.

FIG. 7 is a diagram showing an example of the switching function κ (t) of the pattern A, and FIG. 8 is a diagram showing an example of optimum switching of thrust based on the switching function κ (t) of the pattern A. FIG. 9 is a diagram showing an example of the switching function κ (t) of the pattern B, and FIG. 10 is a diagram showing an example of the optimum switching of thrust based on the switching function κ (t) of the pattern B. FIG. 11 is a diagram showing an example of the switching function κ (t) of the pattern C, and FIG. 12 is a diagram showing an example of optimum switching of thrust based on the switching function κ (t) of the pattern C. FIG. 13 is a diagram showing an example of the switching function κ (t) of the pattern D, and FIG. 14 is a diagram showing an example of optimum switching of thrust based on the switching function κ (t) of the pattern D. FIG. 15 is a diagram showing an example of the switching function κ (t) of the pattern E, and FIG. 16 is a diagram showing an example of optimum switching of thrust based on the switching function κ (t) of the pattern E. The switching function κ (t) of either pattern is a downwardly convex function. For example, the derivation unit 120B derives the switching time indicating the output timing of the thrust derived from the switching function κ (t) as the objective variable.

For example, in the case of the switching function κ (t) of the pattern E, t ₁ ^* ≠ t ₂ ^* , and the first switching time t ₁ and the second switching time t ₂ are different times from each other. In this case, the thrust is to fly at maximum thrust or maximum thrust acceleration from time 0, switches to a minimum thrust or minimum thrust acceleration at time t _1, the maximum thrust or maximum thrust acceleration a subsequent time t ₂ timing.

[Conditions where the upper and lower limits of thrust are constant]
Further, the switching function κ (t) can be defined other than the condition that the upper limit and the lower limit of the thrust acceleration are constant. For example, the switching function κ (t) may be represented by a function in which the upper and lower limits of thrust are constant. Equation (21) represents a switching function κ (t) when the upper and lower limits of thrust acceleration are constant.

In the equation, c represents the effective exhaust velocity of the thruster (engine), M (t) represents the mass of the moving body M at time t, and λ _M (t) represents the concomitant variable of the mass. The switching function κ (t) shown in the equation (21) is found to have a maximum of one inflection point by time differentiation. Therefore, there are at most two solutions of the equation κ (t) = 0 for t. However, the switching function κ (t) shown in the equation (21) is different from the switching function κ (t) shown in the equation (18), and the equation κ (t) = 0 is not a quadratic equation with respect to the time t.

Therefore, the derivation unit 120B treats κ (t) = 0 as a root solution problem of the quadratic equation by approximating M (t) λ _M (t) = K (constant). The derivation unit 120B solves the equation κ (t) = 0 for t, and derives the solution of the equation at times t ₁ ^* and t ₂ ^* . The solution of the quadratic equation may be a multiple solution. For example, if there is no solution to the quadratic equation, the derivation unit 120B may return a value satisfying t ₁ ^* = t ₂ ^* . For example, it returns the axis of κ (t) as in the above equation (20). Is suitable. To solve a quadratic equation in which the switching function κ (t) is 0, for example, in addition to deriving the switching time as the objective variable when there is a solution, when there is no solution. Even so, it involves deriving the switching time as the objective variable.

The value of the constant K is known to be λ _M (t _f ) = 1, for example, using this, K = M (t _f ) λ _M (t _f ) = M (t _f ). As described above, the terminal mass of the nominal orbit may be determined as a guide, or it may be adjusted to a value other than the nominal terminal mass.

Prediction unit 120D, when the first switching time _{t 1} and the second switching time _{t 2} is derived by the derivation unit 120B, and their time _t 1, _{t 2,} the unknowns _{u k} (→) comprising seven unknowns based on the termination state error _{h (u k (→))} (→) predicting (step S202).

[Conditions where the upper and lower limits of thrust acceleration are constant]
Formulas (22) and (23) represent prediction formulas for predicting the terminal state regarding the position velocity under the condition that the upper limit and the lower limit of the thrust acceleration are constant.

In the equation, V (t _f ) (→) represents the state quantity of the velocity of the moving body M at the terminal time t _f , and R (t _f ) represents the state quantity of the position of the moving body M at the terminal time t _f . Represents. V (t _f ) (→) and R (t _f ) (→) are represented by three-dimensional vectors, respectively. Therefore, the terminal state quantity is represented by a seven-dimensional vector including a three-dimensional velocity state function, a three-dimensional position state function, and a one-dimensional Hamiltonian. Further, g _m (→) represents a gravity vector, and l (t) (→) represents a thrust direction unit vector. Further, a _max represents the maximum acceleration. The thrust direction unit vector l (t) (→) collected in chronological order is an example of the “thrust direction time history”.

The above V (t _f ) (→) and R (t _f ) (→) shall be in accordance with mathematical formulas (24) to (27).

Note that R ₀ (→) represents the initial position vector, and V ₀ (→) represents the initial velocity vector. Further, a _min represents the minimum acceleration.

The thrust direction unit vector l (t) (→) follows the bilinear tangent law, which is the optimal solution derived from the optimal control theory. That is, the thrust direction unit vector l (t) (→) is as shown in the following mathematical formula (28).

As can be seen from the mathematical formula (28), when the unknowns ν _R (→), ν _V (→), and t _f are given, the thrust direction unit vector l (t) (→) is determined for an arbitrary time t.

The integral term relating to the thrust direction unit vector l (t) (→) appearing in the mathematical formulas (22) to (27) may be calculated analytically or numerically.

Formula (29) represents a prediction formula for predicting the terminal state of the Hamiltonian under the condition that the upper and lower limits of the thrust acceleration are constant.

However, a _{(t f)} is a thrust acceleration at end time _{t f,} is determined to either _{a min} or _{a max} by the sign of the switching function at the end time _{t f.}

[Conditions where the upper and lower limits of thrust are constant]
Formulas (30) and (31) represent prediction formulas for predicting the end state quantity under the condition that the upper limit and the lower limit of the thrust acceleration are constant.

However, T _max is the maximum thrust. Further, the above V (t _f ) (→) and R (t _f ) (→) shall follow mathematical formulas (32) to (35).

However, T _min is the minimum thrust. The integral term relating to the thrust direction unit vector l (t) (→) and the mass M (t) of the moving body M may be calculated analytically or numerically.

Equation (36) represents a prediction equation for predicting the termination state for the Hamiltonian under conditions where the upper and lower limits of thrust are constant.

However, m (t _f ) (.) Is the fuel consumption per unit time at the end time t _f . The fuel consumption m (t) (.) Per unit time at time t and the thrust T (t) have a relationship shown in the mathematical formula (37) using the effective exhaust speed c.

Fuel consumption in the ending time _{t f m (t f) (} ·) is the sign of the switching function in the terminal time tf, the fuel consumption per unit time corresponding to a thrust lower or upper limit _{m min} (·) or _{m max} It is decided to be either (・).

According to the embodiment described above, the navigation device 100 mounted on the moving body M starts the first switching time t ₁ and the second switching from the initialized initial unknown number u ₀ (→) based on the switching function theory. After obtaining the time t ₂ , the terminal state quantity error h (u ₀ (→)) is further regarded as a function of the first switching time t ₁ and the second switching time t ₂ and the initial unknown number u ₀ (→). )) Predict (→). When the end state error h (u ₀ (→)) (→) satisfies the end condition, the guidance process ends. If not, move to iterative prediction / correction process.

Navigation device 100 predicts the terminal state error _{h (u k (→))} (→) in the k th iteration, the sensitivity matrix _{S k} of the terminal state error _{h (u k (→))} (→) derived, the terminal state error _{h (u k (→))} (→) to search for the best direction _{d k, i} of _{u k} (→) to be modified to reduce.

Navigation apparatus 100 is unknown _{u k} (→) the search direction _{d k} in the k-th iteration _is corrected based on _i, the corrected unknowns, (k + 1) times the amount the terminal state in the first iteration _{u k +} 1 ( → Update as.

The navigation device 100 repeats the update of the scaling coefficient α _k until the relationship shown in the equation (15) is satisfied, and ends the update of the scaling coefficient α _{k when} the relationship shown in the equation (15) is satisfied. The navigation device 100 updates the terminal state quantity uk _{+ 1} (→) using the updated scaling coefficient α _k .

Then, when the updated unknown number uk _{+ 1} (→) satisfies the end condition, the navigation device 100 derives from the unknown number uk _{+ 1} (→), switching times t1, t2, thrust direction l (t) (→), By controlling the propulsive force output device 40 based on the control amount such as the end time t _f and the state amount such as the position, speed, and attitude, the moving body M is accurately moved to the target orbital end. Can be done. As a result, the moving performance can be improved while improving the energy consumption efficiency of the moving body M.

For example, assuming a lander with a landing weight of about 200 kg, about 20 kg of fuel may be consumed in the final phase of descent to the celestial body PL as in the above-mentioned VD phase. When TEG is used, the fuel consumption efficiency of about 3.6% of the 20 kg of fuel is improved. That is, about 0.72 kg of fuel can be saved. The weight of the payload that can be mounted on a lander with a landing weight of 200 kg is typically several kg or less, and 0.72 kg is about 10 to 20%, so the load weight is also increased. Very effective.

[Modification example]
Hereinafter, a modified example of the above-described embodiment will be described. In the above-described embodiment, the mobile body M is a spacecraft, and the sensors mounted on the mobile body M are the camera 10, the inertial measurement unit 20, and the radar 30, but the present invention is not limited to this. For example, when the moving body M is the above-mentioned UAV, the sensor mounted on the moving body M is replaced with or in addition to the camera 10 and the radar 30 by LIDAR (Laser (Light) Imaging Detection and Ranging). May be included.

Also, some of the termination state quantity constraints on position and velocity may be removed. For example, it may be desired to generate an orbit with relaxed conditions because the orbit between two points is physically difficult. In this case, when the constraint of a part of the end state quantity is not always necessary (for example, the circular orbit input does not require the constraint of the down range), a part of the constraint of the end state quantity may be removed. When removing the constraint of the terminal state quantity, the constant ν (unknown number) related to the contingent variable associated with the terminal state quantity to be excluded may be set to zero, and then the corresponding unknown number and the conditional equation may be excluded from the nonlinear simultaneous equations.

Further, in the above-described embodiment, the thrust acceleration or the upper and lower limits of the thrust are set to be constant, but the present invention is not limited to this. For example, the TEG may be performed with the thrust acceleration or the upper and lower limits of the thrust as a "target" instead of a "constraint". As a result, for example, even if it is physically impossible to generate an orbit when treated as a constraint, a more practical induction such as generating a physically established orbit by relaxing the upper and lower limit constraints of thrust (acceleration). Will be able to.

When solving simultaneous nonlinear equations of termination state error h (u (→)) (→) is treated as a “constraint”, f = ρ under the constraint condition of h (u (→)) (→) = 0. _min (T _min −T _min , t) ² + ρ _max (T _max −T _max , t) Minimize ² (in the case of thrust upper and lower limit constraints), or f = ρ _min (a _min −a _min , t) ) ² + ρ _max (a _max −a _max , t) Minimizing ² (in the case of the upper and lower limit constraints of thrust acceleration) can be treated as a “target”, and this is solved as a nonlinear programming problem. ρ is a weight parameter, T _t is a thrust, and a _t is a target value of thrust acceleration.

At this time, the change when changing the treatment from "constraint" to "target" is, for example, in the TEG processing flow, after adding the parameters (two) related to the upper and lower limits of the thrust (acceleration) to the unknown. The point is to replace "solving non-linear simultaneous equations (for example, decay Newton's method)" with "solving equation-constrained optimization problems (for example, Lagrange's undetermined multiplier method)".

Although the embodiments for carrying out the present invention have been described above using the embodiments, the present invention is not limited to these embodiments, and various modifications and substitutions are made without departing from the gist of the present invention. Can be added.

10 ... Camera, 20 ... Inertial measurement unit, 30 ... Radar, 40 ... Propulsion output device, 100 ... Navigation device, 102 ... Communication unit, 110 ... Control unit, 112 ... Acquisition unit, 114 ... Image navigation calculation unit, 116 ... Inertial navigation calculation unit, 118 ... Radar navigation calculation unit, 120 ... Guidance calculation unit, 120A
... initialization unit, 120B ... derivation unit, 120C ... correction unit, 120D ... prediction unit, 122 ... control calculation unit, 130 ... storage unit, M ... mobile body

Claims (11)

For computers mounted on mobiles
Based on the switching function theory, the thrust direction time history indicating the time history in the direction in which the moving body outputs the thrust, the switching time indicating the timing for switching the magnitude of the thrust of the moving body, and the moving body are navigated. From the tentative initial value of the parameter explaining the control amount including the end time indicating the time at the end of the orbit, or the tentative initial value of the parameter explaining the end state indicating the state of the moving body at the end time. , The switching time is derived,
Based on the derived switching time and the initial value, a part or all of the thrust direction time history, the end time, and the end state is predicted.
Guidance program. The parameters include constants related to the contingent variables of the equation of motion of the moving body and provisional initial values when each of the end time is unknown.
The guidance program according to claim 1. On the computer
By solving the equation in which the switching function with the parameter as the explanatory variable becomes zero, the switching time is derived as the objective variable.
The guidance program according to claim 1 or 2. On the computer
Under the condition that the upper and lower limits of the acceleration of the thrust are constant.
By solving the equation in which the switching function of the quadratic equation becomes zero, the switching time is derived.
The guidance program according to claim 3. On the computer
Under the condition that the upper and lower limits of the thrust are constant,
The switching time is derived by solving an equation in which the switching function approximates a quadratic equation becomes zero.
The guidance program according to claim 3 or 4. On the computer
When the switching time, which is the solution of the equation in which the switching function becomes zero, is a time before the first time, the switching time is changed to the same time as the first time.
When the switching time, which is the solution of the equation in which the switching function becomes zero, is the time after the second time, the switching time is changed to the same time as the second time.
When the switching time, which is the solution of the equation in which the switching function becomes zero, is after the first time and before the second time, the switching time is not changed.
The guidance program according to any one of claims 3 to 5. On the computer
The error between the predicted terminal state and the target terminal state is derived, and the error is derived.
The parameter is updated based on the derived error.
The guidance program according to any one of claims 1 to 6. On the computer
When the error is equal to or greater than the threshold value, the correction amount is derived using the sensitivity matrix based on the error.
The parameter is updated by the sum of the derived correction amount and the parameter.
The guidance program according to claim 7. On the computer
By partially differentiating the error with the parameter, the sensitivity matrix is derived.
The correction amount is derived using the derived sensitivity matrix.
The guidance program according to claim 8. The computer mounted on the mobile body
Based on the switching function theory, the thrust direction time history indicating the time history in the direction in which the moving body outputs the thrust, the switching time indicating the timing for switching the magnitude of the thrust of the moving body, and the moving body are navigated. From the tentative initial value of the parameter explaining the control amount including the end time indicating the time at the end of the orbit, or the tentative initial value of the parameter explaining the end state indicating the state of the moving body at the end time. , Derived the switching time,
Based on the derived switching time and the initial value, a part or all of the thrust direction time history, the end time, and the end state is predicted.
Guidance method. A guidance device mounted on a moving body
Based on the switching function theory, the thrust direction time history indicating the time history in the direction in which the moving body outputs the thrust, the switching time indicating the timing for switching the magnitude of the thrust of the moving body, and the moving body are navigated. From the tentative initial value of the parameter explaining the control amount including the end time indicating the time at the end of the orbit, or the tentative initial value of the parameter explaining the end state indicating the state of the moving body at the end time. , The derivation unit that derives the switching time,
A prediction unit that predicts a part or all of the thrust direction time history, the end time, and the end state based on the switching time and the initial value derived by the derivation unit.
Guidance device.

Images