JP2019207181A - Azimuth angle calibration system and azimuth angle calibration method - Google Patents

Abstract

To eliminate the need for movement to a point where an angle error becomes a specific quantity.SOLUTION: An azimuth angle calibration system 1 comprises an arithmetic unit 4 for acquiring azimuth angle information on the basis of the information of an azimuth angle detector of an underwater moving body 2, and an estimation system 5. The estimation system 5 includes a steering device 6 of the underwater moving body 2, a measuring device 7, an azimuth angle estimation device 8, and a determiner 9. The azimuth angle estimation device 8 performs an update process of updating, by an iterative least square method, the inclination and dispersion from reference direction of movement locus of the underwater moving body 2 on the basis of the movement start position of the underwater moving body that moves in a movement direction indicated by the steering device 6 and information pertaining to the moved position, and further performs a computation process of computing the angle of inclination to the reference direction of movement locus and a process of estimating the azimuth angle of the underwater moving body 2 on the basis of the computation result and the history of moved positions of the underwater moving body 2. When the dispersion decreases to a set value or below, the determiner 9 sends azimuth angle estimation information to the arithmetic unit 4 for the purpose of calibration.SELECTED DRAWING: Figure 1

Description

  The present invention relates to an azimuth angle calibration system and an azimuth angle calibration method for calibrating azimuth angle information of an underwater moving body calculated by an arithmetic device provided in the underwater moving body.

  One technique for moving an underwater mobile body such as an underwater robot is a technique in which a pilot remotely controls the underwater mobile body using a control device.

  This type of underwater vehicle control device changes the direction of movement of the underwater vehicle, such as forward, backward, leftward, and rightward, with reference to the front / rear / left / right directions of the underwater vehicle to be operated. The type of instruction is common.

  When performing an operation of moving the underwater moving body in a certain target direction (azimuth) using the control device, information on the azimuth, which is the nose direction of the underwater moving body, is obtained. It is necessary to instruct the moving direction of the underwater moving body with the control device according to the difference in angle.

  Therefore, the underwater moving body is equipped with a device that detects the azimuth angle of its own aircraft. As an apparatus for detecting an azimuth angle, an apparatus in which an arithmetic unit receives an angular velocity sensor value output from a gyro sensor and calculates the azimuth angle by integrating the angular velocity sensor value is often used. Yes.

  However, in this type of azimuth angle detection device, since the sensor value of the gyro sensor includes an error, integration error accumulates over time due to the integration of the sensor value including the error. An error also occurs in the detection result of the azimuth angle.

  A calibration method for calibrating the zero point offset of the gyro has been conventionally proposed (see, for example, Patent Document 1).

  This is because when the moving object moves from the first point to the second point where the position or angle error is a specific amount, dead reckoning using the outputs of the acceleration sensor and the gyro sensor is used at the second point. The obtained position or angle is estimated, a difference from a specific amount is calculated, and the sensor is calibrated using the difference.

JP 2012-215547 A

  By the way, the method disclosed in Patent Document 1 requires that the moving body move from the first point to the second point where the position or angle error is a specific amount.

  However, underwater vehicles such as underwater robots are easily affected by the movement of surrounding water, so when moving by remote control, move from the movement start position to a point where the angle error is a specific amount with high accuracy. That itself is difficult. Therefore, it is difficult to use the technique disclosed in Patent Document 1 for calibrating azimuth information of a remotely operated underwater vehicle.

  Therefore, the present invention provides an azimuth angle calibration system and an azimuth angle calibration method that can calibrate azimuth angle information of an underwater vehicle without requiring movement of the underwater vehicle to a point where the angle error is a specific amount. Is to provide.

  In order to solve the above problems, the present invention provides an azimuth angle detection device provided in an underwater vehicle, an arithmetic device that acquires azimuth angle information based on information received from the azimuth angle detection device, and an estimation system. The estimation system includes: a control device for the underwater mobile body; a measurement device for measuring data used for detecting the position of the underwater mobile body; an azimuth angle estimation device; The estimation device has a function of receiving information on a movement direction of the underwater moving body instructed by the maneuvering device, and a position of the underwater moving body estimated based on data measured by the measuring device. A function of receiving the information of the underwater mobile body, a function of acquiring the position information of the underwater mobile body at the start of movement based on the information on the position of the underwater mobile body and starting the movement according to an instruction from the control device, Step In addition, an update process is performed in which the function of acquiring the position information of the underwater moving body, the inclination from the reference direction of the movement trajectory of the underwater moving body, and the variance are sequentially updated by the least square method. Based on a function, a function of calculating an angle of inclination with respect to a reference direction of the movement trajectory, a history of positions where the underwater moving object has moved, and a result of the calculating process. And a function for performing processing for estimating the azimuth angle of the azimuth angle, and the decision unit is configured to receive the azimuth angle estimation information and the variance result from the azimuth angle estimation device, and the variance is set. A function of sending the azimuth angle estimation information to the arithmetic device when the value is equal to or less than a value, or a function of sending the azimuth angle estimation information to the arithmetic device when the variance change rate is equal to or less than a set value; The arithmetic unit comprises A function of receiving the estimate of the azimuth estimated by the estimation system, using the estimated information of the azimuth, and azimuth calibration system has a configuration in which the function to calibrate the azimuth angle information.

  The measurement device may be configured as an imaging sonar.

  The steering device may be configured to have a function of instructing the moving body in any one of forward, backward, leftward, and rightward directions by the azimuth angle estimation process.

  In addition, the underwater mobile body includes an arithmetic device that acquires azimuth angle information based on information received from an azimuth angle detection device provided in the underwater mobile body, and an estimation system, and the estimation system moves according to instructions from a control device. An azimuth angle estimation device that estimates the azimuth angle of the vehicle, and a determiner, wherein the azimuth angle estimation device receives information on the movement direction instructed to the underwater vehicle from the control device; A process of receiving information on the position of the underwater moving body estimated based on data measured by a measuring device that measures data used for position detection; and the movement of the underwater moving body that starts moving according to an instruction from the control device A process for obtaining information on the position at the start of movement, a process for obtaining information on the position at which the underwater mobile object has moved for each set step, and a reference for the movement trajectory of the underwater mobile object Update processing for sequentially updating the inclination and variance from the direction by the least square method, calculation processing for calculating the angle of the inclination relative to the reference direction of the movement locus, and the position of the position where the underwater moving body has moved A history and a process of estimating the azimuth angle of the underwater moving body based on the result of the arithmetic processing are performed, and the estimator estimates the azimuth angle when the variance is equal to or less than a set value. A process of sending information to the computing device, or a process of sending the azimuth angle estimation information to the computing device when the rate of change of the variance is less than or equal to a set value; It is set as the azimuth angle calibration method which performs the process which receives the estimation information of the said azimuth angle estimated, and the process which calibrates the said azimuth angle information using the said azimuth angle estimation information.

  According to the azimuth angle calibration system and the azimuth angle calibration method of the present invention, it is possible to calibrate the azimuth angle information of the underwater vehicle without requiring the underwater vehicle to move to a point where the angle error becomes a specific amount. it can.

1 is a schematic diagram illustrating an embodiment of an azimuth calibration system. It is a figure for demonstrating the azimuth angle estimation process implemented with the estimation system in an azimuth angle calibration system.

  Hereinafter, embodiments for carrying out the present invention will be described with reference to the drawings.

  FIG. 1 is a schematic diagram illustrating an embodiment of an azimuth calibration system. FIG. 2 is a diagram showing an outline of position information of the underwater moving body used in the estimation system in the azimuth calibration system.

  The azimuth angle calibration system of the present embodiment is denoted by reference numeral 1 in FIG. 1, and includes a gyro sensor 3 as an azimuth angle detection device provided in the underwater moving body 2, and an arithmetic device 4 that receives sensor values from the gyro sensor 3. The configuration includes an estimation system 5 that estimates the azimuth angle of the underwater vehicle 2.

  The azimuth angle of the underwater vehicle 2 indicates the nose direction of the underwater vehicle 2 as an angle from a certain reference azimuth. In the present embodiment, the azimuth serving as a reference for indicating the azimuth angle of the underwater vehicle 2 will be described as the positive direction of the x axis in the xy orthogonal coordinate system shown in FIG. 2 (the right direction in FIG. 2). Note that the xy orthogonal coordinate system shown in FIG. 2 is set in the use area of the underwater vehicle 2, and may be a coordinate system fixed to the earth such as latitude and longitude, or the underwater vehicle 2 The coordinate system set according to the shape of the used area and the structure to be the used area may be used.

  The gyro sensor 3 has a function of detecting the angular velocity of the underwater moving body 2 and sending the angular velocity detection result to the arithmetic device 4 as a sensor value.

  The arithmetic device 4 has a function of acquiring information related to the azimuth angle of the underwater moving body 2 (hereinafter referred to as azimuth angle information) by integrating sensor values received from the gyro sensor 3.

  Further, the arithmetic device 4 uses the acquired azimuth angle information of the underwater vehicle 2 as a propulsion or steering control device (not shown) of the underwater vehicle 2, a control device 6 for remotely operating the underwater vehicle 2, A function of sending the azimuth information of the underwater vehicle 2 to various devices is provided.

Further, the arithmetic unit 4 has a function of receiving the estimate of the azimuth angle [psi _N estimated by the estimation system 5 as described below, using the estimated information of the received azimuth [psi _N, the sensor value of the gyro sensor 3 A function for calibrating the azimuth angle information acquired based on this is provided.

  The estimation system 5 includes an underwater vehicle 2, a control device 6 for the underwater vehicle 2, a measurement device 7 that measures data used for detecting the position of the underwater vehicle 2, an azimuth angle estimation device 8, and a determiner 9 It is set as the structure provided with these.

  The underwater vehicle 2 is a propulsion device (not shown) that includes a propulsion device capable of generating a thrust including a front-rear direction component and a thrust including a left-right direction component, and at least maintaining an azimuth angle. , It is possible to move forward, backward, left and right.

  If the propulsion device provided in the underwater vehicle 2 can achieve forward, backward, leftward, and rightward movements while maintaining the azimuth angle of the underwater vehicle 2, the type, number, and arrangement of the underwater vehicle are as follows: It may be set arbitrarily.

  Moreover, the underwater vehicle 2 may be configured to include a rudder (not shown) used for changing the direction of the aircraft, that is, the azimuth angle.

  The control device 6 is a device that gives a movement instruction to the underwater moving body 2 in accordance with an operation of a pilot (not shown). The control device 6 has a function capable of instructing the underwater moving body 2 to move forward, backward, leftward, and rightward at least while maintaining the azimuth angle. If this function is provided, the control device 6 may be of any configuration such as a configuration having a controller such as a pad type or a joystick type, a configuration having a display device such as a VR goggles, or the like. .

  In the following description, the forward, backward, leftward, and rightward movements while maintaining the azimuth angle of the underwater mobile body 2 are simply forward, backward, leftward, and rightward movements of the underwater mobile body 2 for convenience of explanation. This is called moving.

  When the control device 6 and the underwater mobile body 2 give instructions for forward, backward, leftward, and rightward movement from the control device 6 to the underwater mobile body 2, the underwater mobile body 2 moves in the instructed direction from the movement start position. It is adjusted to move forward, backward, left, and right. At this time, if the environment of the use area of the underwater moving body 2 is an environment in which there is a disturbance affecting the underwater moving body 2, the underwater moving body 2 moves while correcting the influence of the disturbance received on its own aircraft. The control device 6 and the underwater vehicle 2 are adjusted so as to move forward, reverse, left, and right from the start position.

  The control device 6 and the underwater vehicle 2 may be connected by wire, or may exchange information such as a movement instruction by means other than wired connection.

  In this embodiment, the measuring device 7 uses an imaging sonar (also referred to as an acoustic camera) which is a kind of active sonar.

  The measuring device 7 is installed in the use area of the underwater moving body 2. At this time, the measurement device 7 performs measurement so that the measurement range includes from the position where the operation of the underwater moving body 2 starts to the target position of the movement of the underwater moving body 2 in the use area of the underwater moving body 2. The arrangement and orientation of the measurement range of the device 7 are set. Thereby, the coordinates of the installation position of the measuring device 7 in the xy orthogonal coordinate system set in the use area of the underwater vehicle 2 are clarified. Further, based on the specification of the measurement device 7, the measurement range of the measurement device 7 in the xy orthogonal coordinate system is also clarified.

  The imaging sonar as the measuring device 7 can obtain an image overlooking the measurement range, and can obtain an image of the underwater moving body 2 that is an object that exists in the measurement range and generates an echo in the image. .

  Therefore, the measuring device 7 in the present embodiment outputs information on the image overlooking the measurement range as image data including information on the position of the underwater moving body 2. Therefore, the estimation system 5 of the present embodiment is configured to include an image processing unit 10 as a position estimation device that receives image data including information on the position of the underwater moving body 2 from the measurement device 7. The function of estimating the position of the underwater moving body 2 by performing image processing of image data received from the measuring device 7 is provided.

  In the image data obtained by the measuring device 7, the image of the underwater moving body 2 that generates an echo has a luminance value different from the surroundings. By the way, the image data also includes an image of a fixed object other than the underwater moving body 2 that generates an echo existing in the measurement range of the measurement device 7.

  Therefore, the image processing unit 10 performs background difference processing based on a plurality of image data received from the measuring device 7 at a certain time difference, and only pixel information of the image of the underwater moving body 2 that is a moving object is obtained from the entire image. To extract.

  Next, the image processing unit 10 obtains the position of the underwater moving body 2 as pixel position information by obtaining the luminance gravity center of the extracted image of the underwater moving body 2.

  As described above, the installation position and measurement range of the measurement device 7 in the xy orthogonal coordinate system are clear. Therefore, the information is stored in the image processing unit 10 in advance.

  Thereby, the image processing unit 10 can convert the position information of the pixel indicating the position of the underwater moving body 2 into the position coordinate information in the xy orthogonal coordinate system, and the position coordinates in the xy orthogonal coordinate system after the conversion Is estimated as the coordinates of the position of the underwater vehicle 2.

  The image processing unit 10 has a function of sending information on the position of the underwater vehicle 2 estimated by the above processing to the azimuth angle estimation device 8.

  Note that the image processing unit 10 may have a configuration including an arithmetic unit dedicated to image processing, or may be implemented as one of the functions in the azimuth angle estimation device 8 or other devices having a calculation function. .

  Next, functions of the azimuth angle estimation device 8 will be described, and an azimuth angle calibration method performed by the azimuth angle calibration system of the present embodiment will be described.

  The azimuth angle estimation device 8 has a function of performing azimuth angle estimation processing based on information on the estimated position of the underwater moving body 2 received from the image processing unit 10.

  When performing this azimuth angle estimation process, the azimuth angle estimation apparatus 8 performs an initialization process in advance on the following parameters required for calculation of the estimation process.

For example, it is assumed that the gradient φ _N−1 = 0, the variance P _N−1 = 10 ³ , and the forgetting factor ρ = 0.1.

Note that the value of the inclination φ _N−1 is intended to give an initial value, and therefore may be an arbitrary value other than 0 as long as numerical calculation of each expression described later is established. The variance P _N−1 is generally set to a value as large as possible when the sequential least square method described later is performed.

The forgetting factor ρ may be determined in the range of 0 to 1. At this time, as the value of the forgetting factor ρ is closer to 1, it becomes stronger against noise in the calculation of the slope φ _N and the variance P _N described later, but the influence of the past steps becomes larger. On the other hand, the closer the value of the forgetting factor ρ is to 0, the smaller the influence of past steps, but the weaker against noise. Therefore, the value of the forgetting factor ρ may be set appropriately in consideration of the balance between the influence of past steps and the resistance to noise.

  In this state, when starting the azimuth angle estimation process, the control device 6 performs a process of instructing any one of forward movement, reverse movement, left movement, and right movement of the underwater moving body 2. Along with this processing, the azimuth estimation device 8 has a function of receiving, from the control device 6, information related to the moving direction of the underwater moving body 2 instructed by the control device 6.

  At this time, for example, information on a fixed object existing around the underwater moving body 2 can be obtained from the image data of the measuring device 7 or the like. For this reason, as the moving direction of the underwater moving body 2, a direction that does not cause interference with surrounding fixed objects even if the underwater moving body 2 is moved from the current position may be selected.

  The direction of movement may be instructed by manually operating the control device 6 by an operator (not shown). Further, the control device 6 is provided with a start switch and a program for azimuth angle estimation processing, and when the start switch is operated, the control device 6 moves forward, backward, leftward and rightward of the underwater vehicle 2. Alternatively, a direction in which interference with peripheral fixed objects does not occur even when the underwater vehicle 2 is moved from the current position may be selected, and an instruction to move in that direction may be issued. The start switch may be a physical switch or a switch operated on the screen.

  Thereby, the underwater vehicle 2 starts moving in the direction instructed by the control device 6 from the current position.

The azimuth estimation device 8 detects the position of the underwater moving body 2 at the start of movement based on the information received from the image processing unit 10, and sets the position as coordinates (x ₀ , y ₀ ).

Next, the azimuth estimation device 8 determines the position of the underwater vehicle 2 estimated by the image processing unit 10 based on the image data output from the measurement device 7 for each set step after the start of the azimuth estimation processing. The information is received, and the position is set as coordinates (x _N , y _N ). N indicates the number of steps after the start of the azimuth angle estimation process. Note that the period of the step may be constant or may not be constant.

Next, the azimuth estimation device 8 performs a process of updating the inclination φ _N of the movement trajectory and the variance P _N by the successive least square method using the following two equations.

  According to this process, as shown in FIG. 2, even if the underwater mobile body 2 is affected by disturbances and other various influences and the actual movement trajectory 11 of the underwater mobile body 2 meanders, the underwater mobile body The trajectory of movement 2 can be estimated as a straight line 12 as shown by a two-dot chain line in FIG.

When the slope φ _N and the variance P _N are updated, the azimuth estimation device 8 uses the following formula to set the positive direction of the x-axis as the reference (0 [rad]) with respect to the slope φ _N and counterclockwise Processing for calculating an angle θ _N [rad] with the direction being positive is performed.

Note that tan ⁻¹ can estimate the angle θ _N only in the range of −π / 2 [rad] to π / 2 [rad]. This range is the range of the first quadrant and the fourth quadrant with reference to a certain position in the xy orthogonal coordinate system of FIG. Therefore, the estimated angle theta _N actually is underwater vehicle 2 has moved, it is necessary to convert from 0 [rad] in the direction of 2π [rad].

Therefore, the azimuth estimation device 8 uses the coordinates (x ₀ , y ₀ ) of the position at the start of movement in the azimuth estimation process and the coordinates (x _N , y _N ) of the position being estimated to move underwater. The quadrant 2 is moved from the coordinates (x ₀ , y ₀ ) to which quadrant, and the angle is corrected.

Further, when correcting the angle, the azimuth angle estimation device 8 is one of the forward, backward, leftward, and rightward movement directions of the underwater moving body 2 indicated by the control device 6 at the start of the azimuth angle estimation process. Based on whether or not the azimuth angle ψ _N [rad] of the underwater vehicle 2 is calculated.

Specifically, the quadrant where the underwater vehicle 2 has moved is identified based on the first condition “x _N −x ₀ > 0” based on the history of the position where the underwater vehicle 2 has moved, and “y _N The second condition “−y ₀ > 0” is used, and the determination is made according to whether each condition is true or false. When both conditions are true, it can be determined that the underwater vehicle 2 has moved to the first quadrant. Further, when the first condition is true and the second condition is false, it can be specified that the underwater vehicle 2 has moved to the fourth quadrant. When the first condition is false and the second condition is true, it can be specified that the underwater vehicle 2 has moved to the second quadrant. If both conditions are false, it can be determined that the underwater vehicle 2 has moved to the third quadrant.

Therefore, when the moving direction of the underwater vehicle 2 instructed by the control device 6 at the start of the azimuth angle estimation process is forward, the azimuth angle estimating device 8 follows the following Table 1 and the azimuth angle ψ of the underwater vehicle 2 _N [rad] is calculated. In the table, T is true, and F is a false determination result (the same applies to Tables 2, 3, and 4 described later).

When the moving direction of the underwater vehicle 2 indicated by the control device 6 at the start of the azimuth angle estimation process is reverse, the azimuth angle estimation device 8 follows the following Table 2 and the azimuth angle ψ _N of the underwater vehicle 2 Processing to calculate [rad] is performed.

When the moving direction of the underwater vehicle 2 instructed by the control device 6 at the start of the azimuth angle estimation process is to the left, the azimuth angle estimating device 8 follows the following Table 3 and the azimuth angle ψ of the underwater vehicle 2 _N [rad] is calculated.

When the moving direction of the underwater vehicle 2 indicated by the control device 6 at the start of the azimuth angle estimation process is rightward movement, the azimuth angle estimation device 8 follows the following Table 4 and the azimuth angle ψ of the underwater vehicle 2 _N [rad] is calculated.

Note that the azimuth angle ψ _N of the underwater vehicle 2 is calculated according to the movement direction of the underwater vehicle 2 indicated by the control device 6 and the determination results of the first condition and the second condition shown in each table. The relationship with the equation may be stored in the azimuth estimation device 8.

Meanwhile, if the moving direction of the underwater vehicle 2 along the y-axis direction, because it can not calculate the tan ^-1 phi _N, can not be determined angle θ _{N [rad]} in operation. However, since the value of phi _N is large, it is seen that the angle θ _{N [rad]} is near [pi / 2.

Therefore, the azimuth angle-estimating device 8 has a function of a threshold for the value of phi _N is set, when the value of phi _N exceeds the threshold, the angle θ _{N [rad]} be regarded as [pi / 2 A function to perform processing is provided. Note that the threshold value in this case is appropriately set according to the estimation accuracy of the position of the underwater vehicle 2 and the accuracy required for estimation of the azimuth angle.

Furthermore, the azimuth estimation device 8 calculates (y _N −y ₀ ) based on the history of the position where the underwater vehicle 2 has moved, and if the calculation result is a positive value, the underwater vehicle 2 If the movement direction is estimated to be the positive direction of the y-axis, and the calculation result is a negative value, the movement direction of the underwater moving body 2 is estimated to be the negative direction of the y-axis. Yes.

The azimuth estimation device 8 substitutes the movement direction of the underwater vehicle 2 thus obtained for θ _N in the equations when the first and second conditions in the first to fourth tables are both true. Thus, the azimuth angle ψ _N [rad] of the underwater moving object is calculated.

Thus, the azimuth angle-estimating device 8 can determine the azimuth angle [psi _N of underwater vehicle 2.

When the azimuth angle estimation device 8 finds the azimuth angle ψ _N of the underwater vehicle 2, the result is used as estimation information of the azimuth angle ψ _N and the result of the variance P _N obtained by the sequential least square method at the same number of steps. , Processing to be sent to the determiner 9 is performed.

After that, the azimuth estimation device 8 sets φ _N−1 = φ _N for the inclination and P _N−1 = P _N for the variance, advances one step, and outputs the image data output by the measurement device 7. Information on the position of the underwater moving body 2 estimated based on the position is received from the image processing unit 10 and the azimuth angle ψ _N of the underwater moving body 2 is calculated from the processing using the position as coordinates (x _N , y _N ). Repeat until the process.

Thus, as the number of steps proceeds, the value of dispersion P _N obtained sequentially by the least square method, gradually decreases.

Determination unit 9 from the azimuth estimation device 8, and the estimate of the azimuth angle [psi _N, receives the results of dispersion P _N, the dispersion P _N of which, or less than threshold value square root is set Distributed P _N It has a function of performing a process of determining whether or not.

  At this time, the threshold is set according to the accuracy of the azimuth angle information required for the arithmetic device 4. Specifically, when the accuracy of calibration of the azimuth angle information desired by the arithmetic device 4 is ± α [rad], the threshold is set to α [rad].

Moreover, determination unit 9, by the determination process, the square root of the variance P _N is the threshold alpha [rad] determination result that less is obtained, received from the dispersion P _N at the same time as the azimuth angle estimating apparatus 8 orientation the estimate of the angular [psi _N, and a function of sending to the computing device 4.

Note that the estimation system 5 estimates the azimuth angle ψ _N of the underwater vehicle 2 based on the position information of the underwater vehicle 2 detected by the measuring device 7 provided outside the underwater vehicle 2. It is not affected by errors caused by accumulation of integration errors with the passage of time, which has been caused by the method of calculating the azimuth angle by integrating the sensor values of the gyro sensor.

Thus, the arithmetic unit 4, by using the estimate of the azimuth angle [psi _N receive from determiner 9 estimation system 5, for azimuth information based on the sensor value of the gyro sensor 3 is calibrated, the accuracy of the azimuth angle information Falls within the range of ± α [rad].

Therefore, the azimuth angle calibration system 1 according to the present embodiment performs the azimuth angle calibration method according to the above-described procedure, so that the arithmetic device 4 obtains the azimuth obtained by the arithmetic device 4 based on the sensor value of the gyro sensor 3. even the error has occurred on the corner information, using the estimate of the azimuth angle [psi _N arithmetic unit 4 receives from the estimation system 5, it is possible to calibrate the azimuth information to a state that satisfies the accuracy desired.

At this time, the estimation system 5, based on information on the position of the underwater vehicle 2 to move in response to the instruction of the moving direction from the steering apparatus 6, it is possible to estimate the azimuth angle [psi _N of underwater vehicle 2.

  Therefore, in the azimuth angle calibration system and the azimuth angle calibration method of the present embodiment, a process of moving the underwater vehicle 2 to a point where the angle error is a specific amount is not required.

The object to which the azimuth angle calibration system and the azimuth angle calibration method of the present embodiment are applied is exemplified by the arithmetic unit 4 that acquires azimuth angle information based on the sensor value of the gyro sensor 3. On the other hand, the azimuth angle calibration system and the azimuth angle calibration method of the present disclosure include, for example, an azimuth angle detection device of a type using a magnetic azimuth meter, an azimuth angle detection device of a system using GPS information, and other gyro sensors 3. Of course, the azimuth angle may be calibrated with respect to the arithmetic device 4 that acquires azimuth angle information based on information from other azimuth angle detection devices. In this case as well, even if there is an error in the azimuth angle information acquired by the calculation device 4, the estimated accuracy of the azimuth angle ψ _N received by the calculation device 4 from the estimation system 5 is used to satisfy the desired accuracy. The effect that the azimuth angle information can be calibrated can be obtained.

  In the said embodiment, when performing the azimuth angle estimation process in the estimation system 5, the four directions of forward, reverse, left movement, and right movement were shown as the movement directions instructed to the underwater vehicle 2 by the control device 6. On the other hand, as an application example of the azimuth angle estimation process performed by the estimation system 5, an arbitrary moving direction may be instructed to the underwater moving body 2 by the control device 6.

  In this case, when the moving direction is instructed to the underwater moving body 2 by the control device 6, the angle to the instructed moving direction is set to, for example, an angle β (= 0 in the counterclockwise direction with reference to the nose direction. ˜2π [rad]) and information on the angle β is stored.

Then, similarly to the above embodiment, the azimuth angle estimating device 8, the processes to estimate the angle theta _N. Next, the azimuth angle estimation device 8 is based on the angle θ _N and the history of the position where the underwater vehicle 2 has moved, regardless of the first condition and the second condition, and the movement direction instructed to the underwater vehicle 2. using the first table indicated formula, in place of the azimuth angle [psi _N of underwater vehicle 2 performs a process for determining the azimuth underwater vehicle 2 has moved. Thereafter, the azimuth angle estimating device 8, from the direction in which the underwater vehicle 2 obtained has moved, by performing a process of pulling the said angle beta, if to perform the process of obtaining the azimuth angle [psi _N of underwater vehicle 2 Good.

As another application example of the azimuth angle estimation process performed by the estimation system 5, the maneuvering device 6 instructs the underwater moving body 2 to move at a certain angular velocity as a movement including a rotational movement. 5, the position of the underwater vehicle 2 that moves in accordance with the instruction is detected, the movement direction of the underwater vehicle 2 that moves at the instructed angular velocity is obtained from the change in the position, and the underwater vehicle 2 is moved based on the obtained movement direction. it is conceivable to estimate the azimuth angle [psi _N mobile 2.

  Note that the azimuth angle calibration system and the azimuth angle calibration method of the present disclosure are not limited to the above embodiments.

  In FIG. 1, the control device 6, the azimuth angle estimation device 8, and the determination device 9 in the estimation system 5 are described separately, but any two of the control device 6, the azimuth angle estimation device 8, and the determination device 9, or Of course, three may be provided together as a function in a certain device.

  The measuring device 7 that measures data used for detecting the position of the underwater moving body 2 is exemplified by an imaging sonar. On the other hand, the position of the underwater vehicle 2 is detected, and data that makes it possible to specify the coordinates of the position of the underwater vehicle 2 in the orthogonal coordinate system set in the use area of the underwater vehicle 2 is measured. Of course, the measuring device 7 may employ an active sonar other than the imaging sonar, or a measuring device other than the active sonar. In this case, the data output from the measuring device 7 is processed by the position estimation device according to the format of the data to estimate the position of the underwater moving body 2 and the position of the estimated underwater moving body 2 is estimated. Information may be sent to the azimuth estimation device 8.

  In the embodiment, a case has been described in which four directions of forward movement, backward movement, left movement, and right movement are provided as movement directions instructed to the underwater vehicle 2 by the control device 6. On the other hand, the azimuth angle calibration system and the azimuth angle calibration method of the present disclosure, when performing the azimuth angle estimation process in consideration of the type of the propulsion device of the underwater mobile body 2, the motion performance of the underwater mobile body 2, etc. As a movement direction that can be instructed to the underwater moving body 2 by the control device 6, only one direction among the four directions of forward movement, backward movement, left movement, and right movement, or two or three directions can be designated. Also good. In this case, the azimuth estimation device 8 only needs to store a table corresponding to the instructable direction from the first to fourth tables.

Determiner 9 and becomes equal to or less than the threshold value dispersion P _N is set to receive from the azimuth estimating device 8 showed estimate of the azimuth angle [psi _N received simultaneously, as a function of sending to the computing device 4. In contrast, determination unit 9, the dispersion P _N receive from azimuth estimating unit 8, a function of determining the rate of change of the dispersion P _N received in the previous step, the change rate is below the set value, the estimate of the azimuth angle [psi _N, or as a function of sending to the computing device 4. This dispersion P _N, in order not changed at all if there step has elapsed, it becomes less certain value rate of change of dispersion P _N is set, used to calibrate the azimuth information in the arithmetic unit 4 This is because it can be determined to have converged to a possible accuracy of the azimuth angle [psi _N.

Underwater vehicle 2 in accordance with the moving direction of the instruction by the control apparatus 6, while maintaining the azimuth angle [psi _N, if provided with movable in the indicated direction feature underwater vehicle of any type other than underwater robot Of course, the body 2 may be used, and the purpose of use is not particularly limited.

  The azimuth angle calibration system and the azimuth angle calibration method of the present disclosure further fly in the air so-called drone as long as the mobile body can move in the front-rear direction and the left-right direction without changing the azimuth angle. It is possible to apply to calibration of azimuth angle information acquired by the arithmetic unit 4 based on information from an existing azimuth angle detection device in a mobile body of the above type or a mobile body moving on the ground.

  In addition, it goes without saying that various modifications can be made without departing from the scope of the present invention.

2 Underwater moving object, 3 Gyro sensor (azimuth angle detection device), 4 Arithmetic device, 5 Estimation system, 6 Steering device, 7 Measuring device, 8 Azimuth angle estimation device, 9 Judgment device, 11 Movement locus, φ _N inclination, P _N dispersion, θ _N angle, ψ _N azimuth

Claims (4)

An azimuth angle detection device provided for the underwater vehicle,
An arithmetic device that acquires azimuth information based on information received from the azimuth detection device;
An estimation system,
The estimation system includes:
A control device for the underwater vehicle;
A measuring device for measuring data used for detecting the position of the underwater moving body;
An azimuth estimation device;
A judgment device, and
The azimuth estimation device is
A function of receiving, from the control device, information related to the moving direction of the underwater moving body instructed by the control device;
A function of receiving information on the position of the underwater moving body estimated based on data measured by the measuring device;
Based on the information on the position of the underwater moving body, a function of acquiring position information at the start of movement of the underwater moving body that starts moving according to an instruction from the control device;
A function of acquiring information on a position where the underwater moving body has moved for each set step;
A function of performing an update process of sequentially updating the inclination and variance from the reference direction of the movement trajectory of the underwater moving body by a least square method;
A function of performing a calculation process of calculating an angle of inclination with respect to a reference direction of the movement locus;
A function of performing a process of estimating an azimuth angle of the underwater moving body based on a history of a position where the underwater moving body has moved and a result of the calculation process;
The judging device is
From the azimuth angle estimation device, a function to receive the azimuth angle estimation information and the result of the dispersion,
A function of sending the azimuth angle estimation information to the computing device when the variance is less than a set value, or the computation of the azimuth angle estimation information when the variance change rate is less than a set value. A function to send to the device,
The arithmetic unit is:
A function of receiving estimation information of the azimuth angle estimated by the estimation system;
An azimuth calibration system comprising a function of calibrating the azimuth information using the estimated azimuth information. The azimuth angle calibration system according to claim 1, wherein the measurement device is an imaging sonar. The said control apparatus was equipped with the function which instruct | indicates the moving direction to any one of a forward movement, a reverse movement, a left movement, and a right movement to the said underwater moving body by the said azimuth angle estimation process. Azimuth calibration system. An arithmetic device that acquires azimuth angle information based on information received from an azimuth angle detection device provided in the underwater moving body, and an estimation system,
The estimation system includes an azimuth angle estimation device that estimates an azimuth angle of the underwater moving body that moves according to an instruction from a control device, and a determiner.
The azimuth estimation device is
A process of receiving information on a moving direction instructed to the underwater moving body from the control device;
Processing for receiving information on the position of the underwater moving body estimated based on data measured by a measuring device that measures data used for detecting the position of the underwater moving body;
A process of acquiring information on a position at the start of movement of the underwater moving body that starts moving according to an instruction from the control device;
Processing for acquiring information on the position where the underwater moving body has moved for each set step;
An update process for sequentially updating the inclination and variance from the reference direction of the movement trajectory of the underwater moving body by the least square method;
A calculation process for calculating an angle of inclination with respect to the reference direction of the movement locus;
Based on the history of the position where the underwater moving body has moved and the result of the calculation process, the azimuth angle of the underwater moving body is estimated, and
The judgment device is
A process of sending the azimuth angle estimation information to the computing device when the variance is less than or equal to a set value, or the computation of the azimuth angle estimation information when the variance change rate is less than or equal to a set value. Process to send to the device,
The arithmetic unit is
Receiving the estimated information of the azimuth angle estimated by the estimation system;
A method for calibrating the azimuth angle information using the estimated azimuth angle information.

Images