WO2020050084A1 - Storage medium having guidance control program stored thereon - Google Patents

Abstract

A non-transitory storage medium stores a guidance control program. When the guidance control program causes a computer mounted on a mobile body provided with a plurality of sensors to employ a Kalman filter to predict a future state quantity of the mobile body on the basis of detected values from each of a first sensor and a second sensor included in the plurality of sensors, the guidance control program causes the computer to derive, on the basis of a parameter of the Kalman filter, an index value indicating a degree of error in the state quantity of the mobile body, and causes the computer to solve an optimization problem in order to derive the state quantity of the mobile body which optimizes an evaluation function including the index value as an element.

Description

Storage medium storing guidance control program

The present invention relates to a storage medium storing a guidance control program.
Priority is claimed on Japanese Patent Application No. 2018-168193, filed on September 7, 2018, the content of which is incorporated herein by reference.

2. Description of the Related Art Conventionally, spacecraft that perform autonomous precision landing (APL) on gravity objects such as the moon and planets by image navigation are known. In the Powered Descent Landing (PDL) phase of landing a spacecraft on a gravitational celestial body, image navigation is performed by collating an image of the ground surface of the gravitational celestial body with terrain data obtained in advance. As well as generating a guided trajectory in real time based on the current location. On the other hand, a flying vehicle equipped with an inertial sensor and a positioning sensor based on a positioning satellite uses the detection value of each sensor as an observation value of a Kalman filter to perform complex navigation using the detection results of each sensor in a complex manner. Techniques are known (see Patent Document 1).

Japanese Patent Application Publication No. 2018-109530

誘導 However, the conventional technology has not been able to sufficiently optimize the guidance trajectory so as to reduce the navigation error.

One embodiment of the present invention provides a storage medium storing a guidance control program capable of optimizing a guidance trajectory so as to reduce a navigation error.

One embodiment of the present invention provides a computer mounted on a moving object including a plurality of sensors, based on the detection values of the first sensor and the second sensor included in the plurality of sensors, using a Kalman filter in the future. When predicting the state quantity of the moving body, an index value indicating the degree of the error of the state quantity of the moving body is derived based on the parameters of the Kalman filter, and the evaluation function including the index value as an element is optimal. A non-transitory computer-readable storage medium storing a guidance control program for deriving a state quantity of the moving object by solving an optimization problem.

According to one aspect of the present invention, the guidance trajectory can be optimized so that the navigation error is reduced.

It is a figure showing an example of a mobile of an embodiment. It is a figure showing an example of composition of a navigation system of an embodiment. 6 is a flowchart illustrating an example of a series of processes performed by a control unit according to the embodiment. FIG. 4 is a diagram illustrating an example of a relationship between a covariance matrix and a processing cycle. It is a conceptual diagram which shows typically the process which solves the convex optimization problem using the gradient information of an evaluation function.

Hereinafter, an embodiment of the guidance control program of the present invention will be described with reference to the drawings. The guidance control program according to the present embodiment is executed by, for example, a computer mounted on a moving object. The computer that executes the guidance control program performs various processes.

FIG. 1 is a diagram illustrating an example of the moving object M according to the embodiment. The moving object M may be, for example, a spacecraft (space explorer) that lands on a gravitational celestial body (hereinafter, simply referred to as a celestial body PL) such as a moon or a planet, and searches for the celestial body PL. A navigation device (computer) 100 that executes a guidance control program is mounted on a moving object M that is a spacecraft. The moving object M, which is a spacecraft, is equipped with a camera 10, an inertial measurement unit (IMU) 20, and a propulsion output device 30 in addition to the navigation device 100.

The camera 10 is provided, for example, below the moving body M and captures an image of the surface of the celestial body PL when the moving body M lands on the celestial body PL. The “lower side” is, for example, the side of the housing provided with legs that contact the surface of the celestial body PL. In other words, the camera 10 is provided on the direction in which the gravity of the celestial body PL acts on the moving body M (vertically downward). The camera 10 outputs the captured image to the navigation device 100. The camera 10 is an example of a “first sensor”. The camera 10 is an example of a “terrain detection sensor”.

The inertia measurement device 20 includes, for example, a three-axis acceleration sensor formed of MEMS (Micro Electro Mechanical Systems) or an optical fiber, and a three-axis gyro sensor. The inertial measurement device 20 outputs detection values detected by these sensors to the navigation device 100. The values detected by the inertial measurement device 20 include, for example, the accelerations and / or angular velocities in the horizontal, vertical, and depth directions, and the velocities (rates) of the pitch, roll, and yaw axes. The inertial measurement device 20 is an example of a “second sensor”.

The propulsion output device 30 includes, for example, a gimbal actuator and an ion engine. For example, the thrust output device 30 changes the output direction of the thrust generated by the ion engine to an arbitrary direction by driving the gimbal actuator.

By executing the guidance control program, the navigation device 100 causes the moving object M, which is a spacecraft, to navigate, for example, using the image captured by the camera 10 and the detection result of the inertial measurement device 20. Hereinafter, the navigation using the image captured by the camera 10 will be referred to as “image navigation”, and the navigation using the detection result of the inertial measurement device 20 will be referred to as “inertial navigation”. The mobile object M is not limited to a spacecraft, but an UAV (Unmanned Aerial Vehicle) that flies in an environment where GNSS (Global Navigation Satellite System) cannot be used, and an AUV (Autonomous Vehicle) that propells underwater to search for resources on the sea floor. Other moving objects such as Underwater Vehicles may be used. Hereinafter, as an example, a description will be given assuming that the moving object M is a spacecraft.

FIG. 2 is a diagram illustrating an example of the configuration of the navigation device 100 according to 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 used for a telemeter line, for example. The monitoring device on the earth, for example, remote-controls (remotely guides) the navigation device 100 via the communication unit 102, so as to go straight to a straight line connecting the celestial body PL to be landed and the earth (straight direction of radio wave). The position of the moving object M in the direction in which the moving object M moves is controlled. That is, the monitoring device on the earth controls the position of the moving body M in the horizontal direction perpendicular to the vertical direction based on the gravity of the celestial body PL.

The control unit 110 includes, for example, an acquisition unit 112, an image navigation calculation processing unit 114, an inertial navigation calculation processing unit 116, a parameter determination unit 118, a prediction unit 120, and an airframe control 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 a guidance control program stored in the storage unit 130. 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), or software. And hardware may cooperate. The guidance control program referred to by the processor may be stored in the storage unit 130 in advance, or may be stored in a removable storage medium such as a DVD or a CD-ROM. It may be installed in the storage unit 130 from a storage medium by being mounted on the device.

The storage unit 130 is realized by a storage device such as a hard disk drive (HDD), a flash memory, an electrically erasable programmable read only memory (EEPROM), a read only memory (ROM), and a random access memory (RAM). The storage unit 130 stores terrain data 132, reference trajectory information 134, filter parameter information 136, and the like, in addition to various programs such as firmware and application programs (including a guidance control program).

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

The reference trajectory information 134 is information defining a reference trajectory (nominal trajectory). The reference trajectory refers to a state quantity such as a position, a speed, an acceleration, and a posture to be taken by the moving object M when the moving object M is remotely guided to the celestial body PL from the ground monitoring device, at a certain time interval or at a distance interval. This is the information specified for each. The state quantity indicated by the reference trajectory information 134 is an example of “reference state quantity”.

The filter parameter information 136 is information in which each parameter of a Kalman filter equation described later is defined.

The acquisition unit 112 acquires an image from the camera 10 and acquires detection values such as acceleration and speed from the inertial measurement device 20. The acquisition unit 112 periodically acquires the reference trajectory information 134 from the monitoring device on the ground via the communication unit 102, and stores the reference trajectory information 134 in the storage unit 130, so that the reference trajectory information 134 stored in the storage unit 130 is obtained. To update.

The image navigation calculation processing 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 to derive the state quantity of the moving object M. For example, the image navigation calculation processing unit 114 performs image processing on the image of the camera 10 to extract feature points such as rocks and craters, and includes the extracted feature points and the three-dimensional model indicated by the terrain data 132. The position of the moving body M in the horizontal direction perpendicular to the vertical direction based on the gravity of the celestial body PL is derived as a state quantity by pattern matching with the feature points to be obtained.

For example, when the altitude of the moving object M with respect to the ground surface of the celestial body PL (for example, the altitude of the ground surface is set to zero) falls below a predetermined altitude, the inertial navigation calculation processing unit 116 detects the inertial measurement device 20 A state quantity that quantitatively indicates the state of the moving object M is derived by inertial navigation using the result. The “state quantity” includes, for example, a position, a speed, a quaternion from the body coordinate system to the inertial coordinate system, and the like. The machine body coordinate system is a coordinate system in which the center of gravity of the moving body M is the origin of the coordinates, and the machine axis direction of the moving body M is one axis. The inertial coordinate system is a coordinate system in which the vertical direction based on the gravity of the celestial body PL is a certain axis. The “state quantity” may include a sensor bias value for the acceleration or the angular velocity detected by the inertial measurement device 20.

Generally, in the case of remote control 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, and it is more than tens of minutes in terms of the round-trip propagation time of radio waves. May hang. For this reason, when lowering the height of the moving body M with respect to the ground surface of the celestial body PL and landing the moving body M on the celestial body PL, remote control based on a command from the ground may not be sufficient. Therefore, the inertial navigation calculation processing unit 116 derives the state quantity of the moving object M by the inertial navigation using the detection value of the inertial measurement device 20 to perform the autonomous navigation.

The 決定 parameter determining unit 118 refers to the reference trajectory information 134 and determines values of parameters included in various equations of the Kalman filter. The parameters include, for example, a state transition matrix of a Kalman filter described later, a covariance matrix of process noise of the Kalman filter, a covariance matrix of observation noise of the Kalman filter, and the like. After determining the value of the parameter, the parameter determination unit 118 stores the parameter value in the storage unit 130 as the filter parameter information 136.

When the state quantity of the moving object M is derived by the inertial navigation operation processing unit 116, the prediction unit 120 performs the first processing (prediction processing) using the Kalman filter in which the parameters are defined by the filter parameter information 136 (the prediction processing). Of the state quantity error (navigation error) of the moving object M is reduced in the second processing (update processing) using the Kalman filter.

For example, when the current processing cycle in the Kalman filter is k, the first processing includes, in the current cycle k, processing for predicting the state quantity of the moving object M in the next processing cycle k + 1, and in the current cycle k, the next processing. And a process of estimating an error of the state quantity of the moving object M in the processing cycle k + 1. More specifically, in the first processing, the state quantity (state quantity vector described later) of the moving object M estimated to be obtained in the current cycle k, the control input (control vector described later) in the current cycle k, and For estimating the state quantity (estimated value) of the moving object M estimated to be obtained in the next cycle k + 1 based on the above, and the degree of error included in the estimated value of the current cycle k, or the accuracy of the estimated value Of the error included in the estimated value of the next cycle k + 1 derived in the current cycle k, based on the covariance (pre-estimated covariance) indicating the current cycle k and the covariance indicating the degree of the process noise of the Kalman filter in the current cycle k. A process of deriving a covariance (a posteriori estimated covariance) indicating the degree or accuracy of the estimated value.

The second processing derives a Kalman gain based on the covariance of the state quantity of the moving object M estimated as the posterior estimation covariance in the first processing, and based on the derived Kalman gain and a certain observation value, Based on the process of updating the state quantity of the moving object M estimated in the first process, the derived Kalman gain, and the covariance of the state amount of the moving object M estimated as the posterior estimated covariance in the first process. And updating the covariance of the state quantity of the moving object M. The observation value may be, for example, a difference between the state quantity of the moving body M derived by the inertial navigation calculation processing unit 116 and the state quantity of the moving body M derived by the image navigation calculation processing unit 114. That is, the observation value may be a difference between the position of the moving object M obtained by the inertial navigation and the position of the moving object M obtained by the image navigation. In the second process, by using the position difference as an observation value, in addition to the position, the velocity, acceleration, or the like, which is a differential value of the position, is estimated as a state quantity.

Here, when performing the first processing and the second processing by using the Kalman filter, the prediction unit 120 solves an optimization problem (optimal control problem) exemplified as Expressions (1) to (6), so that the future An optimal solution of the state quantity of the moving object M is derived. Hereinafter, a letter with (→) represents a vector or a matrix.

x (→) represents a state quantity vector indicating the state quantity of the moving body M, u (→) represents a control vector of the moving body M, f represents a nonlinear dynamics function, and g represents a state constraint equation. , H represents a control constraint equation, F (→) represents a state transition matrix of a Kalman filter, H (→) represents an observation matrix of a Kalman filter, and K (→) represents the second processing described above. P (→) represents the covariance matrix of the Kalman filter, Q (→) represents the covariance matrix of the process noise of the Kalman filter, and R (→ ) Represents the covariance matrix of the observation noise of the Kalman filter, and k represents each processing cycle (step) of the Kalman filter.

Formula (1) represents a formula that formulates a minimization problem of a certain evaluation function J, and formulas (2) to (6) represent formulas to be satisfied in solving formula (1). Evaluation function J shown in Equation (1) represents a certain processing cycle k _f of the observation matrix G (→), the trace value of a composite mapping and covariance matrix P (→) _kf processing cycle k _f. The processing cycle k _f, which is the final processing time of the Kalman filter, which is calculated based on the derived track instructed from the ground of the monitoring device (final processing cycle). In other words, processing cycle k _f is the final processing period when the moving object M is landing on celestial PL. Observation matrix G (→) is a matrix-diagonal elements are zero, the final treatment time covariance matrix P in k _f (→) trace value with a weighted specific diagonal elements of _kf (vs. This is a matrix for calculating the sum of angular components. Therefore, the evaluation function J is a trace value in which a specific diagonal component of the covariance matrix P (→) _kf is weighted.

Equation (2) is a state equation represented by a nonlinear dynamics function having a state quantity vector x (→) and a control vector u (→) as variables, and equation (3) is a constraint equation of the equation. Expression (4) represents an inequality constraint condition expression.

Equation (5) represents the process of deriving the posterior estimated covariance among the two processes included in the above-described first process. That is, Equation (5) is obtained by calculating the covariance matrix (prior estimated covariance) P (→) _{k−1 | k−1 of the} error included in the estimated value of the previous cycle _k−1 and the process of the Kalman filter in the current cycle k. Deriving a covariance matrix of the error (post posteriori covariance) P (→) _{k | k−1} based on the noise covariance matrix Q (→) _k and the error included in the estimated value of the current period k. ing. k | k-1 means estimating the covariance matrix of the current cycle k which is one time ahead in the previous cycle k-1.

Equation (6) represents a process of updating the covariance of the state quantity of the moving object M among the two processes included in the above-described second process. That is, as shown in Expression (5), Expression (6) expresses the current period k based on the covariance matrix P (→) _{k | k-1} of the current period k estimated in the previous period k-1. By deriving the covariance matrix P (→) _{k | k} again, the covariance matrix P (→) _{k | k−1} estimated at the previous cycle k−1 is converted to a more reliable covariance matrix P (→) _{k | k} is updated. The above-mentioned Kalman gain is obtained by calculating the right-hand side term P (→) _{k | k−1} H (→) ^T (H (→) P (→) _{k | k−1} H (→) ^T + R (→) in Expression (6). _k ) ^-1 . The process of updating the covariance is performed at a predetermined processing cycle K (→). a k> _{K f.}

The prediction unit 120 solves the optimization problem by finding the time history of the control vector u (→) that minimizes the evaluation function J shown in Expression (1). In other words, the prediction unit 120, control vector from a set of obtained in repeating the processing of the Kalman filter from k ₁ to k _f k _f-number of the control vector u (→), the evaluation function J is minimized Solve the optimization problem by searching for u (→).

In order to solve the optimization problem, a calculation method is required in which the evaluation function J is discretely linearized with respect to the control vector u (→) under a certain condition. Therefore, in the present embodiment, the prediction unit 120 applies the approximation formula shown in Expression (7) to the first processing and the second processing, and calculates the discrete covariance matrix P (→) _k which is one element of the evaluation function J. Perform linearization. P (−) in Expression (7) represents a covariance matrix derived by a Kalman filter based on the amount of movement to be taken by the moving object M determined as the reference trajectory. The variance matrix P (-) representing the navigation accuracy when the moving object M moves on the reference trajectory is derived by virtually operating the Kalman filter used when the moving object M actually moves on the reference trajectory. You. “Virtually operate” means that the calculation result of the Kalman filter is not used for control of the mobile unit M. (-) Indicates an overline (bar).

Approximate expression shown in Equation (7), the reference trajectory (x (→), u (→)) in the vicinity of, the covariance matrix P _{(→) k} of each processing cycle, P _{(→) k} to tr (P (→) _k ) represents an assumption that the result is the same as or very close to the result of differentiation, and is satisfied only when the conditional expression shown in Expression (8) is satisfied. In other words, the approximation formula shown in Expression (7) is obtained by calculating the covariance matrix P (→) _{k with} respect to the trace value (tr (P (→) _k (−))) of the covariance matrix P (→) _k (−). Assuming that the covariance matrix P (→) _k approximates the value obtained by multiplying the covariance matrix P (→) _k (−) by the division value of the trace value (tr (P (→) _k )). I have. The covariance matrix P (→) _k shown in equation (7) is an example of “first covariance matrix”, and the covariance matrix P (→) _k (−) shown in equation (7) is It is an example of “covariance matrix”.

Equation (9) represents the linear form of the covariance matrix P (→) _k . The prediction unit 120 derives the time history of the control vector u (→) that minimizes the evaluation function J according to Expression (9) and Expressions (2) to (4) that have been linearized discretely. [K ⁻ , K ⁺ ] in Expression (9) indicates that the period is the processing period of Expression (6), that is, the processing period of the second processing (the processing of updating the covariance matrix), and each tr ( P (→)) is expressed in a linear form that is established in the vicinity of the reference trajectory (x (→), u (→)) used as a reference for linearization.

Specifically, when the prediction unit 120 derives the control vector u (→) that minimizes the evaluation function J according to Expression (9), the prediction unit 120 substitutes the derived control vector u (→) into Expression (2). , And repeats discretization and linearization using Expression (7) again based on the derived state quantity vector x (→) and control vector u (→). Do.

Thus, the optimization problem of searching for the time history of the control vector u (→) in which the evaluation function J is minimized is discretized and linearized around the reference trajectory (x (→), u (→)). Thus, the linearized optimization problem can be solved as a convex optimization problem. The convex optimization problem is a minimization problem of a convex function on a convex set, and can be optimized more easily than a general optimization problem, and the local minimum matches the global minimum Has properties. By linearizing the optimization problem and treating it as a convex optimization problem in this way, even when a relatively simple calculation based on the trace value (scalar value) of the covariance matrix is performed, Even if an initial solution that does not hold is given, the solution of the calculation can be made to converge and the optimal solution (x (→), u (→)) can be obtained.

The body control unit 122 controls the propulsion output device 30 based on the optimal solution (x (→), u (→)) derived by the prediction unit 120 using the Kalman filter, and thereby the position and speed of the moving body M. , Acceleration, posture, etc., are controlled to be the desired states indicated by the optimal solutions (x (→), u (→)).

FIG. 3 is a flowchart illustrating an example of a series of processes performed by the control unit 110 according to the embodiment. The processing of this flowchart is performed, for example, in a descent phase in which the distance (altitude) between the ground surface of the celestial body PL and the moving body M is equal to or less than a predetermined distance (for example, about 40 [m]).

First, the parameter determination unit 118 refers to the reference trajectory information 134 and obtains (x (→), u (→)) of the reference trajectory that is an initial value (step S100). Next, the parameter determination unit 118 determines the parameters of the Kalman filter (F (→), Q (→), R (→)) based on (x (→), u (→)) of the reference trajectory as the initial value. Is determined (step S102).

Next, the prediction unit 120 calculates the Kalman filter including the parameters determined by the parameter determination unit 118 until the processing cycle k of the Kalman filter becomes the processing cycle K (→) for starting the update processing of the predetermined covariance matrix. Is used to repeatedly derive the covariance matrix P (→) _k (step S104).

FIG. 4 is a diagram illustrating an example of the relationship between the covariance matrix P (→) _k and the processing cycle k. As shown, the covariance matrix P (→) _k converges to a constant value as the process is repeated, given the initial value P (→) ₀ . At this time, the covariance matrix P (→) _{k serving as} a convergence value is uniquely determined by the parameters of the Kalman filter.

Returning to the description of FIG. 3, next, the prediction unit 120 acquires (derives) gradient information around the reference trajectory (x (→), u (→)) given as the reference trajectory (step S106). The gradient information is information on the gradient of the evaluation function J. The gradient of the evaluation function J is represented by, for example, x (→) or u (→) of the trace value of the covariance matrix P (→) _k , which is one element of the evaluation function J, which is represented by the left-hand side equation of Expression (7). Is the partial differential amount at. For example, the prediction unit 120 partially differentiates the covariance matrix P (→) k with x (→) or u (→) in order to obtain gradient information. The prediction unit 120 can discretize and linearize (ie, convexize) the trace value (tr (P (→) k)) of the covariance matrix P (→) _k using the partial differentiation result. Discretization and linearization means that when the evaluation function J is written in a linear form of x (→) or u (→) at each time k approximately discretized, x (→) or u (→) This partial differential value is used as such a coefficient.

Next, the prediction unit 120 solves a convex optimization problem, which is one of the optimization problems, so that the trace value (tr (P (→) _k )) takes the minimum value to obtain the optimal solution (x (→), u (→)) is derived (step S108).

FIG. 5 is a conceptual diagram schematically showing a process for solving a convex optimization problem using gradient information of the evaluation function J. The illustrated example shows that the optimization problem is transformed into a convex optimization problem using the gradient information of the evaluation function J. When the convex optimization problem, which is one of the optimization problems, is solved by using the gradient information of the evaluation function J as in the example shown, the prediction unit 120 calculates the trace value of the covariance matrix P (→) _k (Tr (P (→) _k )) is made convex. As a result, even when the optimal solution is locally searched, the optimal solution having the minimum trace value (tr (P (→) _k )) is not derived as a local solution, and the original solution is obtained by convex optimization. One of the local solutions to the problem can be solved quickly.

Next, the prediction unit 120 derives (x (→), u (→)) (x (−) in the figure) given as the reference trajectory and derives the optimal solution (x (→), u (→)). ) (X in the figure), it is determined whether or not the norm (Euclidean norm) of the difference is equal to or smaller than a threshold value Δ (step S110).

When the prediction unit 120 determines that the norm is equal to or smaller than the threshold Δ, the airframe control unit 122 proceeds based on the optimal solution (x (→), u (→)) derived by the prediction unit 120 using the Kalman filter. The power output device 30 is controlled (step S112). Thus, the processing of this flowchart ends.

When determining that the norm exceeds the threshold value Δ, the prediction unit 120 derives the reference trajectory (x (→), u (→)) given as the initial value as the optimal trajectory (x (→ ( →), u (→)) (step S114), and the process returns to S102. In the process of S114, the prediction unit 120 determines a state quantity that can be taken as an optimal solution such that the state quantity x (→) obtained as the optimal solution does not greatly deviate from the state quantity x (→) (−) indicated by the reference trajectory. The upper and lower limits of x (→) may be changed.

In response to this, the parameter determination unit 118 determines the Kalman filter parameters (F (→), Q (→), R (→)) based on the updated trajectory (x (→), u (→)). Determine the value again. As described above, when the moving object M lands on the celestial body PL by repeatedly solving the convex optimization problem until the deviation from the reference trajectory or the trajectory obtained as the optimal solution in the previous process is equal to or smaller than the threshold value Δ. It is possible to sequentially derive the future state quantity of the moving object M in which the navigation error of the autonomous navigation to be performed becomes small. As a result, it is possible to optimize the guidance trajectory represented by the future state quantity of the moving object M that is continuous in time series so that the navigation error is reduced.

According to the above-described embodiment, the navigation device 100, which is a computer mounted on a moving body M including a plurality of sensors such as the camera 10 and the inertial measurement device 20, supplies the detection values of the camera 10 and the inertial measurement device 20 with each other. When predicting the future state quantity of the moving object M using the Kalman filter based on the Kalman filter, based on the Kalman filter parameters (F (→), Q (→), R (→)), A covariance matrix P (→) _k which is an index value indicating the degree of error of the state quantity is derived, and the trace value (tr (P (→) _k )) of the covariance matrix P (→) _k is included as an element. The state quantity x (→) of the moving object at which the evaluation function J is optimal is derived by solving an optimization problem (for example, a convex optimization problem). This makes it possible to optimize the guidance trajectory when landing the moving body M on the celestial body PL so that the navigation error is reduced.

[Modification]
Hereinafter, a modified example of the above-described embodiment will be described. In the above-described embodiment, the moving body M is a spacecraft, and the sensors mounted on the moving body M are the camera 10 and the inertial measurement device 20. However, the present invention is not limited to this. For example, when the moving object M is the above-described UAV, the sensors mounted on the moving object M include radar and LIDAR (Laser (Light) Imaging Detection and Ranging) instead of or in addition to the camera 10. May be. When the moving body M is the above-described AUV, the sensor mounted on the moving body M may include a sonar instead of or in addition to the camera 10. Radar, LIDAR, and sonar are other examples of "terrain detection sensors."

In the above-described embodiment, the navigation device 100 is described as deriving the state quantity x (→) of the moving object at which the evaluation function J is optimal by solving a convex optimization problem which is one of the optimization problems. However, it is not limited to this. For example, the prediction unit 120 of the navigation apparatus 100 calculates the Newton's method based on the partial differential amount of x (→) or u (→) of the trace value of the covariance matrix P (→) _k which is the gradient information of the evaluation function J. By solving another optimization problem such as the steepest descent method or the steepest descent method, the state quantity x (→) of the moving object at which the evaluation function J is optimal may be derived. In other words, the prediction unit 120, the covariance matrix P _(→) k _(-) Trace value _{(tr (P (→) k} (-))) covariance matrix for P _(→) trace value of _k (tr ( P (→) _k )) is multiplied by the covariance matrix P (→) _k (−), under the assumption that the covariance matrix P (→) _k approximates By solving another optimization problem such as the steep descent method, the state quantity x (→) of the moving object at which the evaluation function J is optimal may be derived.

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

DESCRIPTION OF SYMBOLS 10 ... Camera, 20 ... Inertial measurement device, 100 ... Navigation device, 102 ... Communication part, 110 ... Control part, 112 ... Acquisition part, 114 ... Image navigation calculation processing part, 116 ... Inertial navigation calculation processing part, 118 ... Parameter determination Unit, 120: prediction unit, 122: airframe control unit, 130: storage unit, M: mobile unit

Claims (5)

A computer mounted on a moving object equipped with a plurality of sensors,
Based on the respective detection values of the first sensor and the second sensor included in the plurality of sensors, when predicting the future state amount of the moving object using a Kalman filter, based on the parameters of the Kalman filter, Deriving an index value indicating the degree of error in the state quantity of the moving object,
The index value, the state quantity of the moving body in which the evaluation function included as an element is optimal is derived by solving an optimization problem,
A non-transitory computer-readable storage medium storing a guidance control program. On the computer,
If the norm of the difference between the state quantity of the moving body derived by solving the optimization problem and a preset reference state quantity exceeds a threshold, the parameter is changed based on the state quantity of the moving body. Let
Based on the Kalman filter including the changed parameter, to derive the index value,
A storage medium storing the guidance control program according to claim 1. On the computer,
Until the norm becomes equal to or less than a threshold, the parameter is changed based on the state quantity of the moving object, and the index value is derived based on the Kalman filter including the changed parameter.
A storage medium storing the guidance control program according to claim 2. On the computer,
The parameter is determined based on the state quantity of the derived moving body, and based on the Kalman filter including the determined parameter, a first covariance matrix is derived as the index value,
A parameter of the Kalman filter is determined based on a previously set reference state quantity, and based on the Kalman filter including the determined parameter, a second covariance matrix is derived as the index value,
Assuming that the first covariance matrix approximates a value obtained by multiplying the second covariance matrix by a value obtained by dividing the trace value of the first covariance matrix by the trace value of the second covariance matrix, Solve the problem
A storage medium storing the guidance control program according to claim 1. The first sensor is a terrain detection sensor that detects terrain, the second sensor is an inertial measurement device,
On the computer,
Based on the detection result by the terrain detection sensor, to derive the state quantity of the moving body,
The state quantity of the moving object measured by the inertial measurement device, and the difference between the state quantity of the moving object derived based on the image as the observed value of the Kalman filter, the future state of the moving object To predict the amount,
A storage medium storing the guidance control program according to claim 1.

Images