JP2021076426A - Inertial navigation device, movement control method, and movement control program - Google Patents

Abstract

To provide an inertial navigation device, a movement control method and a movement control program with which it is possible to execute the movement of a moving body by inertial navigation with higher accuracy.SOLUTION: An underwater vehicle 10 includes a moving vehicle body 20, an inertial navigation device 21 for moving the moving vehicle body, an inertia measuring device 22 for detecting the movement of the moving vehicle body by inertia, a coordinate calculation device 28 for calculating the coordinate of the position of the moving vehicle body based on absolute coordinates on earth, and a control device 30 for controlling the movement of the moving vehicle body by the inertial navigation device on the basis of the detection result of the inertia calculation device. The control device executes a correction process of calculating an error on the basis of the movement information calculated on the basis of the detection result of the inertia measuring device and the positions of start and end points of detection result acquired by the coordinate calculation device, calculating a correction value on the basis of the error and correcting the detection result of the inertia measuring device with the correction value.SELECTED DRAWING: Figure 1

Description

The present disclosure relates to an inertial navigation system, a movement control method, and a movement control program that move while performing position control by inertial navigation.

As one of the techniques for controlling the movement by using the inertial navigation method, there is a system that acquires an image of the surroundings, calculates a position error, and corrects it (for example, Patent Document 1).

JP-A-2019-504389

Like the system described in Patent Document 1, the position of the absolute coordinates (latitude and longitude) on the earth is calculated, and the position is corrected based on the calculation result, so that the moving body can be moved to an appropriate position. it can. However, it is necessary to acquire the position information based on the coordinates in order to correct the position, and there is a limit to the improvement of the accuracy of inertial navigation.

The present disclosure has been made in view of the above circumstances, and an object of the present invention is to provide an inertial navigation system, a movement control method, and a movement control program capable of performing movement of a moving body by inertial navigation with higher accuracy.

In order to solve the above-mentioned problems and achieve the object, at least one embodiment of the present disclosure detects the movement of the moving body main body, the inertial navigation device for moving the moving body main body, and the inertial movement of the moving body main body. Inertial measurement unit, a coordinate calculation device that calculates the coordinates of the position of the moving body body based on absolute coordinates on the sphere, and the moving body by the inertial navigation system based on the detection results of the inertial measurement unit. The control device includes a control device that controls the movement of the main body, and the control device includes movement information calculated based on the detection result of the inertial measurement unit, and a start point and an end point of the detection result acquired by the coordinate calculation device. Provided is an inertial navigation system that calculates an error based on the position of the above, calculates a correction value based on the error, and executes a correction process for correcting the detection result of the inertial measurement unit with the correction value.

Further, at least one embodiment of the present disclosure has an inertial step of acquiring the position of the start point of the moving body body based on the absolute coordinates on the earth and the movement of the moving body body from the start point to the end point. Calculated based on the steps of detecting with a measuring device and accumulating the detected information, the step of acquiring the position of the end point of the moving body body based on the absolute coordinates on the earth, and the detection result of the inertial measurement unit. A step of calculating an error based on the movement information and the positions of the start point and the end point in the absolute coordinates on the earth, and calculating a correction value based on the error, and the detection result of the inertial measurement unit. Provided is a movement control method including a step of controlling the movement of the moving body main body while performing correction processing for correction with the correction value.

Further, at least one embodiment of the present disclosure includes a step of acquiring the position of the starting point of the moving body main body based on the absolute coordinates on the earth by the control device of the moving body that can move by inertial navigation, and the moving body. A step of detecting the movement of the main body from the start point to the end point by an inertial measurement unit and accumulating the detected information, and a step of acquiring the position of the end point of the moving body main body based on absolute coordinates on the earth. , The error is calculated based on the movement information calculated based on the detection result of the inertial measurement unit, and the positions of the start point and the end point on the absolute coordinates on the earth, and the correction value is calculated based on the error. A movement control program for executing a process including a step of controlling the movement of the moving body main body while performing a correction process of correcting the detection result of the inertial measurement unit with the correction value is provided.

According to at least one embodiment of the present disclosure, the movement of a moving body by inertial navigation can be performed with higher accuracy.

FIG. 1 is a block diagram showing a main configuration of an underwater vehicle. FIG. 2 is a flowchart showing an example of a movement control method for the underwater vehicle. FIG. 3 is an explanatory diagram for explaining an example of a movement control method for the underwater vehicle. FIG. 4 is an explanatory diagram for explaining an example of a movement control method for the underwater vehicle. FIG. 5 is a flowchart showing an example of a movement control method for the underwater vehicle. FIG. 6 is an explanatory diagram for explaining an example of a movement control method for the underwater vehicle. FIG. 7 is a flowchart showing an example of a movement control method for the underwater vehicle. FIG. 8 is a flowchart showing an example of a movement control method for the underwater vehicle.

Hereinafter, embodiments according to the present disclosure will be described in detail with reference to the drawings. The present invention is not limited to this embodiment. In addition, the components in the embodiment include those that can be easily replaced by those skilled in the art, or those that are substantially the same. Furthermore, the components described below can be combined as appropriate. In the present embodiment, a case of an underwater navigating body moving underwater will be described as a preferable example of an inertial navigation system moving by an inertial navigation system (INS). The inertial navigation system may be an air vehicle moving in the air or a land moving body moving on land.

FIG. 1 is a block diagram showing a main configuration of an underwater vehicle. The underwater navigation body 10 includes a moving body main body 20, an inertial navigation device 21, an inertial measurement device 22, an altitude measurement device 26, a depth measurement device 27, a coordinate calculation device 28, and a control device 30. .. In addition to the above configuration, the underwater navigation body 10 has various configurations provided as an underwater navigation body. The mobile body 20 is an outer shell that houses each part of the underwater navigation body 10.

The inertial navigation system 21 is a device that generates a driving force, propels the moving body main body 20, and changes the attitude. The inertial navigation system 21 includes an upper and lower ladder 32, a left and right elevator 34, and a main thruster 36. The upper and lower rudder 32 has a control plate that projects in the vertical direction of the moving body main body 20, and rotates the control plate around an axis that extends in the vertical direction. The vertical rudder 32 changes the direction of the control plate along the axis to exert a force in the direction of rotation on the moving body main body 20 in the vertical direction. The left and right elevators 34 have control plates that project in the left-right direction of the moving body main body 20, and rotate the control plate around an axis that extends in the left-right direction. The left and right elevators 34 apply a force in the direction of rotation to the moving body main body 20 in the left-right direction by changing the direction of the control plate along the axis. That is, the left and right elevators 34 rotate in the direction in which the moving body main body 20 is floated and in the direction in which it is settled. The main thruster 36 is a propeller and a jet propulsion machine, and exerts a force for propelling the moving body main body 20 in the propulsion direction. The inertial navigation system 21 can change the attitude of the moving body main body 20 by controlling the directions of the upper and lower ladders 32 and the left and right elevators 34 while propelling the inertial navigation system 21 by the main thruster 36.

The inertial measurement unit 22 detects the inertial force acting on the moving body main body 20, and detects the change in the posture and the movement of the moving body main body 20. The inertial measurement unit 22 of the present embodiment includes a gyro sensor 24 and an acceleration sensor 25. The gyro sensor 24 detects the angle (posture), angular velocity, and angular acceleration of the moving body body 20. The gyro sensor 24 can detect an angle with respect to the geomagnetism and detects an angle with respect to absolute coordinates. The acceleration sensor 25 calculates the acceleration in the three-axis direction and detects the speed applied to the moving body main body 20 in the three-dimensional direction.

The altitude measuring device 26 detects height information, that is, the height (distance in the vertical direction) from the reference position. The underwater navigation body 10 is an underwater navigation body. Therefore, the altitude measuring device 26 measures the height of the moving body main body 20 from the seabed. The depth measuring device 27 measures the distance and depth of the moving body main body 20 from the sea surface (water surface). Further, the altitude measuring device 26 and the depth measuring device 27 may be included in the inertial measurement device 22. The altitude measuring device 26 does not necessarily have to be provided.

The coordinate calculation device 28 calculates the coordinates of the position of the moving body 20 with respect to the absolute coordinates on the earth. The coordinate calculation device 28 acquires information from an object whose absolute coordinate position on the earth is known, calculates the relative position between the object and the moving body body 20 based on the acquired information, and calculates the relative position of the moving body body 20. Calculate the position. Absolute coordinates on the earth are coordinate systems that refer to one position on the earth and also specify the axis. Absolute coordinates on the earth are, for example, latitude and longitude. As the coordinate calculation device 28, a device that receives a signal output from a satellite and detects position coordinates can be used in a global positioning satellite system (GNSS: Global Navigation Satellite System). As a specific configuration example, there is a positioning device for detecting a position by using a positioning system such as a Global Positioning System (GPS). The underwater vehicle 10 of the present embodiment detects the position information of the absolute coordinates by the coordinate calculation device 28 when the underwater vehicle 10 ascends to a water depth where the signal output from the satellite can be received.

The control device 30 controls the operation of each part of the underwater vehicle 10. The control device 30 stores an arithmetic circuit such as a CPU (Central Processing Unit) and a memory such as a ROM (Read Only Memory) and a RAM (Random Access Memory). It includes a device, a calculation unit that performs various processes related to movement control, and a storage unit that stores various data. The control device 30 stores the movement control program 40, the movement plan data 42, and the measurement storage data 44 in the storage unit. The movement control program 40 is a program that controls the movement of the underwater vehicle 10. The movement plan data 42 is a movement route set for the underwater vehicle 10. The movement plan data 42 is data of a three-dimensional movement route. The travel route includes information on the starting point, the location information of the final destination, and information on the route passing from the starting point to the final destination. Further, the movement path includes the positions of the start point and the end point as points for acquiring the position (information on the position (coordinates) on the absolute coordinates) by the coordinate calculation device 28. The measurement storage data 44 is various data acquired and accumulated by the processing of the movement control program 40, such as the information detected by the inertial measurement unit 22, the information detected by the coordinate calculation device 28, and the calculation result calculated by the control device 30. is there. The movement control program 40 and the movement plan data 42 are supplied to the control device 30 by various communication functions and stored before the start of the movement.

Next, with reference to FIGS. 2 to 4, the operation of each part of the underwater vehicle 10, that is, an example of the movement control method of the underwater vehicle 10 will be described. FIG. 2 is a flowchart showing an example of a movement control method for the underwater vehicle. The underwater vehicle 10 executes the process shown in FIG. 2 by controlling the operation of each part by the control device 30. Further, FIG. 2 is an example of processing, and a part of each processing shown in FIG. 2 may be executed in parallel or may be executed as separate processing.

The underwater vehicle 10 acquires movement plan data (step S12). The underwater vehicle 10 moves to the start point based on the acquired movement plan data, acquires position information (information on the position of the moving body main body 20) by the coordinate calculation device 28, and specifies the position of the start point. (Step S14). The underwater vehicle 10 stores the position of the absolute coordinates acquired by the coordinate calculation device 28 as the position of the starting point in the measurement accumulation data 44.

The underwater vehicle 10 moves with the inertial navigation system 21 based on the movement plan data 42 while acquiring the measurement result of the inertial measurement unit 22 (step S16). The underwater vehicle 10 calculates the movement locus from the start point based on the result measured by the inertial measurement unit 22, so that the movement locus calculated by the inertial navigation follows the movement route set in the movement plan data 42. Move to. Further, the underwater vehicle 10 stores the calculated measurement result in the measurement accumulation data 44.

The underwater vehicle 10 determines whether or not the movement has been completed to the end point of the movement plan data (step S18). The underwater vehicle 10 calculates the current position based on the integrated result of the measurement results of the inertial measurement unit 22, and determines whether the calculated position is the end point. When it is determined that the movement of the underwater vehicle 10 has not been completed up to the end point of the movement plan data (No in step S18), the underwater vehicle 10 returns to step S16 and continues the movement based on the movement plan data.

When it is determined that the underwater vehicle 10 has completed the movement to the end point of the movement plan data (Yes in step S18), the coordinate calculation device 28 acquires the position information and specifies the position of the end point (step S20). ..

The underwater vehicle 10 calculates an error (deviation amount) based on the position information, that is, the position information of the start point and the end point of the moving body main body 20 on the absolute coordinates, and the measurement result of the inertial measurement unit 22. (Step S22). The underwater vehicle 10 compares the information of the start point and the end point of the absolute coordinates with the position information and the information of the start point and the end point calculated from the measurement result of the inertial measurement unit 22. Since the start point is the same point, the amount of deviation in the position of the end point is calculated as an error.

The underwater vehicle 10 calculates a correction value based on an error (deviation amount) (step S24). The underwater vehicle 10 calculates a correction value for correcting the measurement error of the inertial measurement unit 22 based on the calculated error (deviation amount). The underwater vehicle 10 calculates a correction value for at least one of the initial azimuth angle error, the initial attitude angle error, and the velocity component detection error of the inertial measurement unit 22. The initial azimuth error is an error in the horizontal direction with respect to the reference position of the absolute coordinates set in the inertial measurement unit 22, for example, the geomagnetism. The initial posture angle error is an error between the reference position set in the inertial measurement unit 22 and the posture of the moving body main body 20. The velocity component detection error is an error between the velocity in each direction detected from the acceleration of the inertial measurement unit 22 and the actual velocity in each direction of the moving body main body 20.

The underwater vehicle 10 moves with the inertial navigation system 21 based on the movement plan data while correcting the measurement result of the inertial measurement unit 22 with the correction value (step S26). That is, the underwater vehicle 10 calculates the position where the measurement error of the inertial measurement unit 22 is corrected by correcting the measurement result of the inertial measurement unit 22 with the correction value, and based on the information of the corrected current position, the underwater vehicle 10 calculates the position where the measurement error of the inertial measurement unit 22 is corrected. It moves by the inertial navigation device 21.

Next, an example of the processing shown in FIG. 2 will be described with reference to FIGS. 3 and 4. FIG. 3 is an explanatory diagram for explaining an example of a movement control method for the underwater vehicle. FIG. 4 is an explanatory diagram for explaining an example of a movement control method for the underwater vehicle. FIG. 3 schematically shows the movement of the underwater vehicle 10 in the depth direction (altitude direction). FIG. 4 shows an example of the horizontal movement of the underwater vehicle 10.

As shown in FIG. 3, the underwater navigator 10 subsides from the vicinity of the sea surface 60 and navigates in the vicinity of the seabed 62. The underwater vehicle 10 moves on the route set in the movement plan data 42. The section 64 shown in FIG. 3 is a section of the primary cruising, and is a section moved from step S14 to step S20 in FIG. The section 66 shown in FIG. 3 is a section to be moved in the process of step S26. The underwater vehicle 10 acquires absolute position information with the coordinate calculation device 28 while being at the position P of the first section 70 of the section 64. Then, in the section 72 after the section 70, the route 80 is moved based on the movement plan data 42 while calculating the position based on the inertial measurement unit 22. Here, the movement route set in the movement plan data 42 is a route that moves from the position P to the position Pa as shown in FIG. Due to a measurement error of the inertial measurement unit 22, the underwater vehicle 10 moves to a position Pb deviated from the position Pa when the path 80 is moved by inertial navigation.

When the underwater vehicle 10 moves to the position Pb along the route 80, it determines that it has moved to the end point and moves to the vicinity of the sea level 60. After that, while at the position Pb in the section 76, the coordinate calculation device 28 acquires the absolute position information.

The underwater vehicle 10 calculates an error based on the relationship between the start point P and the end point Pa on the movement plan and the start point P and the end point Pb moved by inertial navigation. For example, as shown in FIG. 4, the angle φ1 formed by the line segment connecting the start point P and the end point Pa and the line segment connecting the start point P and the end point Pb is calculated. The underwater vehicle 10 calculates the initial azimuth angle error, initial attitude angle error, and speed component detection error of the inertial measurement unit 22 based on the angle φ1 formed, and based on the calculation results, the initial azimuth angle error and initial attitude. Determine the amount of correction for angle error and velocity component detection error. Although the movement path is schematically shown by a straight line in FIG. 4, the actual movement path is bent. Therefore, the formed angle φ1 which is an index of the displacement of the position is a component including an error of the angle and an error of the velocity in each direction. The underwater vehicle 10 calculates the correction amount of the speed component based on the speed component detection error.

The underwater vehicle 10 calculates the correction amount while moving from the position Pb to the position Pb'in the section 74. After calculating the correction amount and moving to the position Pb', the underwater vehicle 10 is in the state of being at the position Pb'of the section 76, and the coordinate calculation device 28 is used as the position information of the moving body main body 20 based on the absolute coordinates. Get the position. After that, in the section 78 after the section 76, the route 82 is moved based on the movement plan data 42 while calculating the position based on the calculation result and the correction value of the inertial measurement unit 22. Here, the movement route set in the movement plan data 42 is a route that moves from the position Pb'to the position Pc as shown in FIG. The underwater vehicle 10 moves to a position Pd deviated from the position Pc when the path 82 is moved by inertial navigation due to a measurement error of the inertial measurement unit 22. The underwater vehicle 10 corrects the calculation result of the inertial measurement unit 22 based on the correction value and calculates the position, so that the line segment connecting the start point Pb'and the end point Pc and the start point Pb' The angle φ2 formed by the line segment connecting the end point Pd can be made smaller than the angle φ1 formed before the correction.

From the above, the underwater vehicle 10 sets a correction value for correcting an error caused by the measurement of the inertial measuring device 22 based on the deviation between the position calculated by the inertial measuring device 22 and the position detected by the coordinate calculating device 28. By setting, the movement accuracy of inertial navigation using the inertial measuring device 22 can be improved. That is, it is possible to reduce the deviation between the movement plan data 42 and the movement path of the inertial navigation using the inertial measurement unit 22 of the underwater navigation body 10.

Here, when the underwater vehicle 10 has a deviation (drift characteristic) when the power of the gyro sensor 24 or the like is turned on as the inertial measurement device 22, and the drift characteristic does not change when the power is input, FIG. By performing the correction measures shown in (1), the accuracy of inertial navigation after calculating the correction value can be improved.

Further, by using the underwater vehicle 10 for inertial navigation as described in the present disclosure, it is possible to improve the accuracy of the position control of the underwater navigation in which it is difficult to acquire the position information.

Further, it is preferable that the underwater vehicle 10 executes the correction process shown in FIG. 2 a plurality of times. As a result, the accuracy of the correction value can be improved by executing the correction process a plurality of times, and the difference between the detection result of the inertial measurement unit 22 and the actual movement locus can be reduced. Further, when the underwater vehicle 10 executes the correction process a plurality of times, it is preferable that the distance that the moving body main body moves in the correction process is longer than the distance that the moving body main body moves in the previous correction process. By increasing the distance of the correction process to be executed later, the correction value can be calculated with a higher resolution, and thus the accuracy of the correction value can be further improved.

Further, when the underwater vehicle 10 executes the correction process a plurality of times, the position detected by the coordinate calculation device, the position information calculated by accumulating the detection results of the inertial measurement unit, and the elapsed time of each are used. , It is preferable to perform inertial approximation and calculate the correction value.

For example, the position (INS position) calculated based on the measurement result of the inertial measurement unit 22 is Pi (k), the position (GNSS position) detected by the coordinate calculation device 28 is Pr (k), and the time is T (k). To do. Here, k = 1, 2, 3, ..., N, which is the ascent timing. Assuming that the current ascent is k and the previous ascent is k-1, the error d (k) this time is d (k) = Pr (k) -Pi (k), and the inclination error ae (k) of the error occurrence. ) = (Pr (k) -Pr (k-1))-(Pi (k) -Pi (k-1)) / (T (k) -T (k-1)).

As a result, the position at time t after the ascent this time can be corrected by Pi_rev (t) = Pi (t) + ae (k) * (t-T (k)) + d (k). The control device 30 stores the INS position, the GNSS position, the time, the error d (k), and the tilt error ae (k) in n times of ascent.

In this way, by averaging the slope ae and the error d of the past n times at the specified N times, the appearance of the error is gradually refined and the influence of random factors such as tidal current is reduced. be able to. Further, the N times may be all points in the past or the latest N times (for example, 3 times).

Here, the underwater vehicle 10 may be a weighted average (weighted average) when calculating the average when the data of the correction processing of a plurality of times is used. That is, in the case of an error d (k), the error d (k), (k = 1, 2, 3, ..., N) is obtained by using the weight w (k) as follows.
^{_{d mean = (Σ N K =}} 1 (w (k) · d (k)) / Σ N K = 1 w (k)

As a result, by increasing the weight as the weight data of the latest levitation data is increased, it is possible to calculate the correction amount with an emphasis on the method of generating the latest error. For example, the weight w (k) is 10, 9, 8, ... From the latest. Further, by setting the weight to increase the weight at the place where the elapsed time is long, it is possible to calculate the correction amount with an emphasis on the error generated during long-time (long-distance) navigation. Further, in the present embodiment, although the correction formula is shown as an example of the linear formula, it may be a quadratic formula.

Hereinafter, an example of processing will be described with reference to FIG. FIG. 5 is a flowchart showing an example of a movement control method for the underwater vehicle. The underwater vehicle 10 executes a correction process (step S40). Here, the correction process is the process from step S12 to step S22 in FIG. 2, that is, from the start point to the end point while controlling the movement based on the result measured by the inertial measurement unit 22 based on the movement plan data. This is a process in which the coordinate calculation device 28 detects the position of the absolute coordinates on the earth at the start point and the end point.

The underwater vehicle 10 acquires information on past correction processing (step S42). That is, the underwater vehicle 10 acquires the position information of the absolute coordinates of the correction process acquired before step S40 and the position information calculated by inertial navigation.

The underwater vehicle 10 executes a weighting process (step S44). The underwater vehicle 10 performs a preset weighting process on the information calculated by each correction process. Next, the underwater vehicle 10 calculates a correction value based on the weighting (step S46).

The underwater vehicle 10 moves with the inertial navigation system 21 based on the movement plan data while correcting the measurement result of the inertial measurement unit 22 with the correction value (step S48). That is, the underwater vehicle 10 calculates the position where the measurement error of the inertial measurement unit 22 is corrected by correcting the measurement result of the inertial measurement unit 22 with the correction value, and based on the information of the corrected current position, the underwater vehicle 10 calculates the position where the measurement error of the inertial measurement unit 22 is corrected. It moves by the inertial navigation device 21.

Here, it is preferable that the underwater vehicle 10 sets a recommended ascent region, and the position detection by the coordinate calculation device 28 is executed in the recommended ascent region. Here, the ascending recommended area is an area set by the operator when the underwater vehicle 10 is determined to be able to ascend. The recommended ascent region may be a plurality of divided regions or a single region.

FIG. 6 is an explanatory diagram for explaining an example of a movement control method for the underwater vehicle. FIG. 6 is a diagram schematically illustrating a recommended ascent region. In FIG. 6, a port 110 is provided on the coastline 100. In addition, a ship 120 is sailing in the sea. The port 110 and the ship 120 are facilities related to the underwater vehicle 10. The underwater vehicle 10 can set a predetermined distance range 101 from the coastline 100, a range 112 near the port 110, and a fixed distance range 122 with respect to the ship 120 as levitation recommended areas.

An example of the movement control method using the recommended ascent region will be described with reference to FIGS. 7 and 8. FIG. 7 is a flowchart showing an example of a movement control method for the underwater vehicle. The underwater vehicle 10 acquires position information with a coordinate calculation device (step S50). The underwater vehicle 10 determines whether the current position is the recommended ascent region (step S52).

When the underwater vehicle 10 determines that the current position is not the recommended ascent region (No in step S52), the underwater vehicle 10 moves to the recommended ascent region (step S54) and proceeds to step S56. The movement in step S54 may be performed while acquiring the position information in the vicinity of the sea surface, but it is preferable to move by inertial navigation while acquiring the information of the inertial measurement unit 22 in the water. When the underwater vehicle 10 determines that the current position is the recommended ascent region (Yes in step S52), the underwater vehicle 10 proceeds to step S56.

The underwater vehicle 10 is in the recommended ascent region, and then the correction process is executed (step S56). The underwater vehicle 10 executes the correction process based on the movement plan data in which the start point and the end point are set as arbitrary positions in the levitation recommended area. Next, the underwater vehicle 10 calculates a correction value based on the information calculated by the correction process (step S57). The correction value is preferably calculated by a calculation method based on the results of the above-mentioned plurality of correction processes.

The underwater vehicle 10 executes the correction process, calculates the correction value, and then determines whether the correction process has been executed a predetermined number of times (step S58). The predetermined number of times (threshold number of times) is a preset number of times. When the underwater vehicle 10 determines that the number of times the correction number has been executed is less than the predetermined number (No in step S58), it returns to step S52 and executes the correction process again.

When it is determined that the number of times the underwater navigation body 10 has executed the correction number of times is equal to or greater than the predetermined number of times (Yes in step S58), the correction value is determined, and the measurement result of the inertial measurement unit 22 is corrected by the correction value. , The inertial navigation system 21 moves based on the movement plan data (step S60). That is, the underwater vehicle 10 calculates the position where the measurement error of the inertial measurement unit 22 is corrected by correcting the measurement result of the inertial measurement unit 22 with the correction value, and based on the information of the corrected current position, the underwater vehicle 10 calculates the position where the measurement error of the inertial measurement unit 22 is corrected. It moves by the inertial navigation device 21.

By repeating the correction process a number of times specified in the recommended ascent region as in the present embodiment, the accuracy of movement by inertial navigation can be improved. Further, by ascending in the recommended ascent region, the correction value for the inertial measurement unit 22 can be calculated while maintaining high position detection accuracy in the coordinate calculation device 28 of the surface navigation body 10. The accuracy of correction can be increased. As a result, the accuracy of movement by inertial navigation can be improved.

In the process of FIG. 7, the correction process is executed a predetermined number of times, but it may be determined whether to end the correction process based on the change in the correction value or the error. FIG. 8 is a flowchart showing an example of a movement control method for the underwater vehicle. Of the processes of FIG. 8, the same steps as those of FIG. 7 are performed in the same step.

The underwater vehicle 10 acquires position information with the coordinate calculation device 28 (step S50). The underwater vehicle 10 determines whether the current position is the recommended ascent region (step S52).

When the underwater vehicle 10 determines that the current position is not the recommended ascent region (No in step S52), the underwater vehicle 10 moves to the recommended ascent region (step S54) and proceeds to step S56. The movement in step S54 may be performed while acquiring the position information in the vicinity of the sea surface, but it is preferable to move by inertial navigation while acquiring the information of the inertial measurement unit 22 in the water. When the underwater vehicle 10 determines that the current position is the recommended ascent region (Yes in step S52), the underwater vehicle 10 proceeds to step S56.

The underwater vehicle 10 is in the recommended ascent region, and then the correction process is executed (step S56). The underwater vehicle 10 executes the correction process based on the movement plan data in which the start point and the end point are set as arbitrary positions in the levitation recommended area. Next, the underwater vehicle 10 calculates a correction value based on the information calculated by the correction process (step S57). The correction value is preferably calculated by a calculation method based on the results of the above-mentioned plurality of correction processes.

The underwater vehicle 10 executes the correction process, calculates the correction value, and then determines whether the difference between the correction value and the error (deviation) is equal to or less than the threshold value (step S72). That is, in the underwater vehicle 10, the error between the position of the end point detected by the detection result of the inertial measurement unit 22 (the position calculated by using the correction value) and the position of the end point measured by the coordinate calculation device 28 ( It is determined whether the deviation) is equal to or less than the threshold value.

When the underwater vehicle 10 determines that the difference between the correction value and the error (deviation) is larger than the threshold value (No in step S72), that is, the position of the end point detected by the detection result of the inertial measurement unit 22. If the error from the position of the end point measured by the coordinate calculation device 28 is large, the process returns to step S52 and the correction process is executed again.

When the underwater vehicle 10 determines that the difference between the correction value and the error (deviation) is equal to or less than the threshold value (Yes in step S72), the correction value is determined, and the measurement result of the inertial measurement unit 22 is used as the correction value. While making corrections, the inertial navigation system 21 moves based on the movement plan data (step S60). That is, the underwater vehicle 10 calculates the position where the measurement error of the inertial measurement unit 22 is corrected by correcting the measurement result of the inertial measurement unit 22 with the correction value, and based on the information of the corrected current position, the underwater vehicle 10 calculates the position where the measurement error of the inertial measurement unit 22 is corrected. It moves by the inertial navigation device 21.

As in the present embodiment, the accuracy of movement by inertial navigation can be improved by repeating the correction process until the error (deviation) is within the permissible range. Further, by ascending in the recommended ascent region, the correction value for the inertial measurement unit 22 can be calculated while maintaining high position detection accuracy in the coordinate calculation device 28 of the surface navigation body 10. The accuracy of correction can be increased. As a result, the accuracy of movement by inertial navigation can be improved.

The underwater vehicle 10 need only be able to detect absolute coordinates on the earth with the coordinate calculation device 28, and is not limited to detecting the position using a satellite such as GNSS, and is relative to an object fixed on the earth. The position may be specified by communication, video, or the like. For example, the coordinate calculation device 28 may specify a plurality of structures such as a lighthouse and detect absolute coordinates based on the relative position with the building. Further, the coordinate calculation device 28 may communicate with a communication device whose absolute coordinates on the earth are known to specify the absolute coordinates on the earth.

The embodiments and modifications are presented as examples and are not intended to limit the scope of the invention. The embodiments and modifications can be implemented in various other forms, and various omissions, replacements, and changes can be made without departing from the gist of the invention. The embodiments and modifications are included in the scope and gist of the invention, as well as in the scope of the invention described in the scope of patent claims and the equivalent scope thereof.

10 Underwater vehicle 20 Mobile body 21 Inertial navigation system 22 Inertial navigation system 24 Gyro sensor 25 Accelerometer 26 Altitude measurement device 27 Depth measurement device 28 Coordinate calculation device 30 Control device 32 Vertical ladder 34 Left and right elevator 36 Main thruster 40 Movement control Program 42 Movement plan data 44 Measurement accumulation data

Claims (13)

With the mobile body
An inertial navigation system that moves the moving body body,
An inertial measurement unit that detects movement due to the inertia of the moving body body,
A coordinate calculation device that calculates the coordinates of the position of the moving body based on the absolute coordinates on the earth, and
A control device that controls the movement of the moving body body by the inertial navigation system based on the detection result of the inertial measurement unit is included.
The control device calculates an error based on the movement information calculated based on the detection result of the inertial measurement unit and the positions of the start point and the end point of the detection result acquired by the coordinate calculation device, and makes an error. An inertial navigation system that calculates a correction value based on the above and executes a correction process for correcting the detection result of the inertial measurement unit with the correction value. The inertial navigation system according to claim 1, wherein the correction value includes at least one of an initial azimuth angle error and an initial attitude angle error of the inertial measurement unit. The inertial navigation system according to claim 1 or 2, wherein the correction value includes a speed component detection error of the inertial measurement unit. The inertial navigation system according to any one of claims 1 to 3, wherein the control device executes the correction process a plurality of times. The inertial navigation system according to claim 4, wherein the control device makes the distance that the moving body main body moves in the correction process longer than the distance that the moving body main body moves in the previous correction process. The inertial navigation system according to claim 4 or 5, wherein the control device weights the movement information calculated by the correction process and maximizes the weight of the movement information calculated by the latest correction process. The inertial navigation system according to any one of claims 1 to 6, wherein the mobile body is capable of navigating underwater. The inertial navigation system according to claim 7, wherein the control device is acquired by the coordinate calculation device when the moving body main body is arranged within a predetermined distance from the sea surface. The control device contains information on the ascending area.
The inertial navigation system according to any one of claims 4 to 8, wherein the region that floats by the correction process is a region that can float. The inertial navigation system according to claim 9, wherein the control device repeats the correction process in the ascending region until the error becomes equal to or less than a threshold value. The coordinate calculation device acquires information from an object whose absolute coordinate position on the earth is known, calculates the relative position between the object and the moving body body based on the acquired information, and calculates the relative position of the moving object. The inertial navigation system according to any one of claims 1 to 10, which calculates the position of the main body. The step of getting the position of the starting point of the moving body based on the absolute coordinates on the earth, and
A step of detecting the movement of the moving body body from the start point to the end point by an inertial measurement unit and accumulating the detected information.
The step of getting the position of the end point of the moving body based on the absolute coordinates on the earth, and
An error is calculated based on the movement information calculated based on the detection result of the inertial measurement unit and the positions of the start point and the end point in the absolute coordinates on the earth, and the correction value is calculated based on the error. Steps and
A movement control method including a step of controlling the movement of the moving body main body while performing correction processing for correcting the detection result of the inertial measurement unit with the correction value. The step of acquiring the position of the starting point of the moving body based on the absolute coordinates on the earth to the control device of the moving body that can move by inertial navigation,
A step of detecting the movement of the moving body body from the start point to the end point by an inertial measurement unit and accumulating the detected information.
The step of getting the position of the end point of the moving body based on the absolute coordinates on the earth, and
An error is calculated based on the movement information calculated based on the detection result of the inertial measurement unit and the positions of the start point and the end point in the absolute coordinates on the earth, and the correction value is calculated based on the error. Steps and
A movement control program that executes a process including a step of controlling the movement of the moving body main body while performing a correction process of correcting the detection result of the inertial measurement unit with the correction value.

Images