WO2022149383A1 - Autonomous navigation device and autonomous navigation method - Google Patents

Abstract

This autonomous navigation device (100) comprises: an inertia measurement unit (110) that acquires an inertia measured value for a moving body; a motion assessment unit (120) that, on the basis of the inertia measured value acquired by the inertia measurement unit (110), assesses a motion action of the moving body; and a position/orientation estimation unit (140) that, on the basis of the inertia measured value acquired by the inertia measurement unit (110) and the result of assessment by the motion assessment unit (120), estimates the position and orientation of the moving body.

Description

Autonomous navigation system and autonomous navigation method

This disclosure relates to an autonomous navigation system and an autonomous navigation method.

In recent years, the position and orientation of an object have been determined using the inertial measurement value acquired by an inertial measurement unit (IMU). For example, Patent Document 1 discloses a technique for adjusting the number of samples for averaging inertial measurement values as a method for reducing an error in inertial measurement values.

In the technique disclosed in Patent Document 1, an allan variance indicating the magnitude of the error of the inertial measurement value for each number of samples to be averaged is obtained based on the inertial measurement values acquired at a plurality of times during the movement of the moving object. calculate. Then, the number of averaged samples with the smallest error of the inertial measurement value is set based on the calculated Allan variance.

Japanese Unexamined Patent Publication No. 2020-14392

The inertial measurement value fluctuates greatly depending on the state of the moving body. Therefore, the accuracy of the allan variance calculated based on the inertial measurement value is lowered, and the average number of samples cannot be set correctly. Therefore, in the prior art, the accuracy of estimating the position and posture of the moving body is lowered.

Therefore, the present disclosure provides an autonomous navigation device and an autonomous navigation method capable of accurately estimating the position and posture of a moving body.

The autonomous navigation system according to one aspect of the present disclosure is a determination unit that acquires an inertial measurement value of a moving body and determines the motion motion of the moving body based on the inertial measurement value acquired by the measurement unit. The unit includes an estimation unit that estimates the position and posture of the moving body based on the inertial measurement value acquired by the measurement unit and the determination result by the determination unit.

The autonomous navigation method according to one aspect of the present disclosure includes a step of acquiring an inertial measurement value of a moving body and a step of determining the motion motion of the moving body based on the acquired inertial measurement value. It includes a step of estimating the position and orientation of the moving body based on the inertial measurement value and the result of the determination.

It should be noted that these comprehensive or specific embodiments may be realized in a system, a method, an integrated circuit, a computer program, or a recording medium, and may be any of a system, an apparatus, a method, an integrated circuit, a computer program, and a recording medium. It may be realized by various combinations.

According to one aspect of the present disclosure, the position and posture of the moving body can be estimated with high accuracy.

FIG. 1 is a diagram showing a configuration of an autonomous navigation system according to an embodiment. FIG. 2 is a block diagram showing a functional configuration of the autonomous navigation system according to the embodiment. FIG. 3 is a block diagram showing a functional configuration of the position / posture estimation unit according to the embodiment. FIG. 4 is a block diagram showing a functional configuration of the averaging period calculation unit according to the embodiment. FIG. 5 is a flowchart showing the operation of the autonomous navigation system according to the embodiment. FIG. 6A is a diagram showing an example of the movement of the moving body on which the IMU according to the embodiment is mounted. FIG. 6B is a diagram showing an example of inertial measurement data of the moving body that has performed the motion shown in FIG. 6A. FIG. 7 is a flowchart showing the motion determination process (step S20) according to the embodiment. FIG. 8 is a flowchart showing the averaging period calculation process (step S40) according to the embodiment. FIG. 9 is a flowchart showing the dispersion characteristic calculation process (step S42) according to the embodiment. FIG. 10 is a flowchart showing a process of determining the number of averaged samples (step S43) according to the embodiment. FIG. 11 is a diagram showing an example of the dispersion characteristic calculated by the dispersion characteristic calculation unit according to the embodiment. FIG. 12 is a diagram showing the relationship between the motion of the moving body and the average number of samples according to the embodiment.

(Findings underlying this disclosure)
In recent years, in the technical field of autonomous driving, the position of the moving body and the position of the moving body by the IMU alone are considered in the environment where the GNSS reception condition is bad such as inside the tunnel or in the urban area of the skyscrapers, or with the view of using the IMU alone. There is an increasing demand for technology that requires high-precision posture.

Generally, IMU and GNSS are used to determine the position and posture of a moving body.

IMU acquires the inertial measurement value of the moving body. Specifically, the inertial measurement value includes the acceleration (gravitational acceleration) and the angular velocity acting on the moving body. The posture of the moving body is obtained based on the direction of the vector quantity of the angular velocity or the direction of each vector quantity of the acceleration and the angular velocity. Further, by integrating the accelerations, the speed of the moving body, that is, the distance traveled within a predetermined period is calculated, and the current position of the moving body is obtained by using the direction and the distance. A vehicle speedometer mounted on the moving body may be used to estimate the speed of the moving body.

GNSS is an abbreviation for Global Navigation Satellite System. The GNSS can directly obtain the posture and the position based on the positioning information of the GNSS receiver mounted on the moving body.

Then, the final position and posture of the moving body can be obtained with high accuracy by combining the positions and postures obtained by these IMUs and GNSS, respectively.

However, as mentioned above, in the inside of a tunnel or in an urban area of skyscrapers, the positioning information by GNSS is greatly disturbed or cannot be received in the first place. Therefore, in such an environment, it is not possible to estimate the position and attitude using GNSS. Therefore, it is required that the position and posture of the moving body be estimated with high accuracy by the IMU alone.

In order to estimate the position and posture of the moving body with high accuracy by the IMU alone, it is necessary to reduce the error of the inertial measurement value. As a method of reducing the error, it is known to average the inertial measurement values. The averaging is to calculate the average value of a plurality of inertial measurement values obtained within a certain period of time. This fixed time is called a time interval or an averaging period, and the position and posture of the moving body are estimated for each averaging period.

It is also known that the magnitude of the error of the inertial measurement value differs depending on the number of inertial measurement values included in the averaging period (that is, the number of averaged samples). For example, in Patent Document 1, the number of averaged samples with the smallest error of the inertial measurement value is set based on the allan dispersion (dispersion characteristic) calculated based on the plurality of inertial measurement values.

However, in the technique described in Patent Document 1, when the inertial measurement values at the time of traveling on a curve or traveling on a rough road surface are used for calculating the dispersion characteristics, these inertial measurement values are the original dispersion characteristics of the IMU. It is very large compared to the inertial measurement value that should be used for the calculation of. Therefore, the dispersion characteristics cannot be estimated correctly. As a result, it becomes impossible to correctly estimate the number of averaged samples having the smallest error in the inertial measurement value of the IMU. Therefore, the accuracy of estimating the position and posture of the moving body may deteriorate.

Therefore, an object of the present disclosure is to provide an autonomous navigation system or the like capable of accurately estimating the position and posture of a moving body.

Specifically, the autonomous navigation system according to one aspect of the present disclosure includes a measuring unit that acquires an inertial measurement value of a moving body, and a motion motion of the moving body based on the inertial measurement value acquired by the measuring unit. It is provided with a determination unit for determining the above, and an estimation unit for estimating the position and posture of the moving body based on the inertial measurement value acquired by the measurement unit and the determination result by the determination unit.

As a result, the position and posture of the moving body are estimated using the determination result of the motion motion of the moving body, so that the estimation accuracy of the position and the posture can be improved.

Further, for example, the measurement unit repeatedly acquires the inertial measurement value of the moving body, the determination unit makes the determination for each inertial measurement value acquired by the measurement unit, and the estimation unit performs the measurement. A plurality of data sets are extracted from a storage unit in which a plurality of data sets of the inertial measurement value acquired by the unit and the determination result corresponding to the inertial measurement value are stored, and based on the extracted plurality of data sets, the said The position and orientation of the moving body may be estimated.

As a result, the inertial measurement value and the judgment result of the motion motion are stored in association with each other. Therefore, among the obtained multiple inertial measurement values, the inertial measurement value that should not be used for estimating the position and posture of the moving body is selected. , It can be determined based on the determination result. Therefore, since the inertial measurement value that causes the deterioration of the estimation accuracy can be excluded, the estimation accuracy of the position and the posture of the moving body can be improved.

Further, for example, the autonomous navigation system according to one aspect of the present disclosure may further include the storage unit.

As a result, it becomes easier to read and write data to and from the storage unit as compared with the case of using an external storage unit, which can contribute to speeding up the processing speed and reducing power consumption.

Further, for example, the inertial measurement value includes the angular velocity of the moving body, and the determination unit compares the absolute value of the angular velocity with the first threshold value as the determination, and the absolute value is the first threshold value. If it is smaller, the first determination value is output as the determination result, and if the absolute value is larger than the first threshold value, the second determination value different from the first determination value is output as the determination result. The storage unit may store the output determination result and the angular velocity corresponding to the determination result as the data set in association with each other.

This makes it possible to accurately determine the motion motion of the moving body based on the angular velocity. For example, when the angular velocity is high, it can be determined that the moving body is in a curved motion (curve), and when the angular velocity is low, it can be determined that the moving body is in a linear motion (straight).

Further, for example, the estimation unit includes a dispersion characteristic calculation unit that calculates the dispersion values of a plurality of angular velocities included in a plurality of data sets extracted from the storage unit for each candidate of the averaging period, and the dispersion characteristic calculation unit. Based on the variance value for each candidate of the averaging period calculated by, the inertial measurement value using the averaging period determination unit that determines the averaging period and the averaging period determined by the averaging period determination unit. An average value calculation unit for calculating the average value of the moving body and a position / attitude integration unit for estimating the position and posture of the moving body based on the average value calculated by the average value calculation unit may be included.

As a result, the averaging period can be calculated accurately and an appropriate number of averaging samples can be set. Therefore, the accuracy of estimating the position and posture of the moving body can be improved.

Further, for example, the dispersion characteristic calculation unit does not have to extract the data set including the first determination value and not the data set including the second determination value.

This makes it possible to exclude the data set when the change in the direction of the moving body such as a curve is large, so that the estimation accuracy of the dispersion characteristics can be improved. By increasing the accuracy of the dispersion characteristics, the calculation accuracy of the averaging period is improved, and a more appropriate number of averaging samples can be set. Therefore, the accuracy of estimating the position and posture of the moving body can be further improved.

Even if the correct dispersion characteristics can be estimated by the technique described in Patent Document 1, in the technique described in Patent Document 1, the average number of samples is always constant regardless of the motion of the moving object. Is used. Therefore, the time interval for estimating the position and the posture in the curve becomes too long, and the estimation accuracy of the position and the posture may deteriorate.

On the other hand, for example, when the determination result is the second determination value, the averaging period determination unit performs averaging in which the corresponding variance value is minimized from among a plurality of candidates for the averaging period. An averaging period shorter than the period may be selected.

As a result, when the change in the direction of the moving body such as a curve is large, the estimation accuracy of the position and posture of the moving body can be improved by shortening the averaging period.

Further, for example, when the determination result is the first determination value, the averaging period determination unit selects an averaging period in which the corresponding variance value is minimized from a plurality of candidates for the averaging period. You may.

As a result, when the change in the direction of the moving body such as going straight is small, the estimation accuracy of the position and posture of the moving body can be improved by selecting the averaging period having the smallest dispersion value.

Further, for example, the determination unit further compares the absolute value with the second threshold value larger than the first threshold value as the determination, and the absolute value is larger than the first threshold value and the second threshold value is larger than the first threshold value. When it is smaller than the threshold value, the second determination value is output as the determination result, and when the absolute value is larger than the second threshold value, both the first determination value and the second determination value are output as the determination result. A different third determination value may be output.

As a result, for example, it is possible to distinguish between a sharp curve and a gentle curve, so that an appropriate averaging period can be selected according to each case, and the estimation accuracy of the position and posture of the moving body can be improved. Can be done.

Further, for example, the averaging period determination unit may select the minimum averaging period from a plurality of candidates for the averaging period when the determination result is the third determination value.

This makes it possible to improve the estimation accuracy of the position and posture of the moving body by selecting the minimum averaging period when the change in the direction of the moving body such as a sharp curve is considerably large.

Further, for example, the inertial measurement value may include at least one of the acceleration and the velocity of the moving body.

As a result, at least one of acceleration and velocity is used, so that the estimation accuracy of the position and posture of the moving body can be improved.

Further, for example, the autonomous navigation method according to one aspect of the present disclosure includes a step of acquiring an inertial measurement value of a moving body and a step of determining the motion motion of the moving body based on the acquired inertial measurement value. , A step of estimating the position and orientation of the moving body based on the acquired inertial measurement value and the result of the determination. Further, for example, the program according to one aspect of the present disclosure is a program for causing a computer to execute the above-mentioned autonomous navigation method. Alternatively, one aspect of the present disclosure can also be realized as a computer-readable non-temporary recording medium in which the program is stored.

As a result, the position and posture of the moving body are estimated using the determination result of the motion motion of the moving body, so that the estimation accuracy of the position and the posture can be improved.

Hereinafter, embodiments will be specifically described with reference to the drawings.

Note that all of the embodiments described below are comprehensive or specific examples. The numerical values, shapes, materials, components, arrangement positions and connection forms of the components, steps, the order of steps, and the like shown in the following embodiments are examples, and are not intended to limit the present disclosure. Further, among the components in the following embodiments, the components not described in the independent claims are described as arbitrary components.

Also, each figure is a schematic diagram and is not necessarily exactly illustrated. Therefore, for example, the scales and the like do not always match in each figure. Further, in each figure, substantially the same configuration is designated by the same reference numeral, and duplicate description will be omitted or simplified.

Further, in the present specification, the numerical range is not an expression expressing only a strict meaning, but an expression meaning that a substantially equivalent range, for example, a difference of about several percent is included.

Further advantages and effects in one aspect of the present disclosure will be clarified from the specification and drawings. Such advantages and / or effects are provided by some embodiments and the features described in the specification and drawings, respectively, but not all are provided in order to obtain one or more identical features. No need.

(Embodiment)
Hereinafter, embodiments of an autonomous navigation system and an autonomous navigation method capable of estimating the position and posture of a moving body with high accuracy will be described with reference to FIGS. 1 to 12.

[1. Configuration of autonomous navigation system]
First, the configuration of the autonomous navigation system according to the embodiment will be described.

FIG. 1 is a diagram showing an example of the configuration of the autonomous navigation system 100 according to the embodiment. The autonomous navigation device 100 is a device that realizes the operation of the autonomous navigation method according to the present embodiment. The autonomous navigation device 100 performs autonomous navigation of a moving body. The moving object here is mainly assumed to be a vehicle, but a flying object such as an airplane or a drone may be assumed.

As shown in FIG. 1, the autonomous navigation system 100 includes an IMU 101, a processor 102, a memory 103, and a signal line 104. The IMU 101, the processor 102 and the memory 103 are connected to each other via the signal line 104.

IMU101 is mounted on a moving body and measures the inertial measurement value of the moving body. The inertial measurement unit includes the angular velocity of the moving body. Further, the inertial measurement value may include at least one of the acceleration and the velocity of the moving body. The IMU 101 corresponds to the inertial measurement unit 110 shown in FIG. 2, and has a gyro sensor 111 and an acceleration sensor 112.

The gyro sensor 111 is an example of an angular velocity sensor that measures the angular velocity of a moving body. The gyro sensor 111 is, for example, a MEMS gyro or a ring laser gyro, but other types of angular velocity meters may be used. MEMS is an abbreviation for Micro Electro Mechanical Systems.

The acceleration sensor 112 is an example of an acceleration sensor that measures the acceleration of a moving body. The accelerometer 112 is, for example, a MEMS accelerometer or a servo accelerometer, but other types of accelerometers may be used.

The processor 102 is an IC that performs arithmetic processing and controls other hardware. Specifically, the processor 102 is a CPU. IC is an abbreviation for Integrated Circuit. CPU is an abbreviation for Central Processing Unit. The processor 102 has a motion determination unit 120 and a position / posture estimation unit 140 shown in FIG. These processing units are realized as software, for example.

The memory 103 is a storage device corresponding to the storage unit 130 shown in FIG. 2, and is a ROM, a RAM, a cache memory, an HDD, or the like. ROM is an abbreviation for Read Only Memory. RAM is an abbreviation for Random Access Memory. HDD is an abbreviation for Hard Disk Drive.

The memory 103 stores a navigation program for executing the processing of the motion determination unit 120 and the position / attitude estimation unit 140. The navigation program is loaded into memory 103 and executed by processor 102. The OS is further stored in the memory 103. OS is an abbreviation for Operating System. At least a portion of the OS is loaded into memory 103 and executed by processor 102. The processor 102 executes the navigation program while executing the OS. The input / output data of the navigation program is stored in the memory 103.

[2. Function configuration]
Next, the functional configuration of the autonomous navigation system 100 will be described with reference to FIGS. 2 to 4.

FIG. 2 is a block diagram showing a functional configuration of the autonomous navigation system 100 according to the embodiment. As shown in FIG. 2, the autonomous navigation device 100 includes an inertial measurement unit 110, a motion determination unit 120, a storage unit 130, and a position / attitude estimation unit 140. These functional components are realized, for example, by, but not limited to, the IMU 101, the memory 103, and the processor 102, as shown in FIG. The functional components of the autonomous navigation system 100 are realized by any combination of hardware and software.

The inertial measurement unit 110 acquires the inertial measurement value of the moving body. In the present embodiment, the inertial measurement unit 110 repeatedly acquires the acceleration and the angular velocity at regular time intervals. This time interval is called the sampling cycle of the inertial measurement unit 110. Specifically, the inertial measurement unit 110 is realized by the IMU 101 that measures acceleration and angular velocity, but is not limited thereto. The inertial measurement unit 110 may be realized by at least one of a vehicle speedometer, a directional sensor, and a geomagnetic sensor, and may detect acceleration and angular velocity using the obtained sensor values.

The motion determination unit 120 determines the motion motion of the moving body based on the inertial measurement value acquired by the inertial measurement unit 110. Specifically, the motion determination unit 120 makes a determination for each inertial measurement value acquired by the inertial measurement unit 110. That is, the motion determination unit 120 determines the motion motion of the moving body corresponding to the time when the inertial measurement value is acquired. In the present embodiment, the motion motion includes "straight ahead", "curve (curve motion)", and "stop". "Curve" includes "curve (sudden)" and "curve (slow)". The specific processing method performed by the motion determination unit 120 will be described later.

The storage unit 130 stores a plurality of data sets of the inertial measurement value acquired by the inertial measurement unit 110 and the determination result corresponding to the inertial measurement value. That is, the storage unit 130 stores a set of the inertial measurement data output at each time and the motion determination value as a data set. The inertial measurement data includes the angular velocity and acceleration acquired at a predetermined time by the inertial measurement unit 110. The motion determination value is a determination result by the motion determination unit 120, and is information indicating the motion motion of the moving body at the time when the corresponding inertial measurement value is acquired. Each of the plurality of data sets may be associated with the time when the inertial measurement value is acquired. For example, a plurality of data sets are stored as time series data. A plurality of data sets stored in the storage unit 130 are referred to as historical data.

The position / posture estimation unit 140 estimates the position and posture of the moving body based on the inertial measurement value acquired by the inertial measurement unit 110 and the determination result by the motion determination unit 120. Specifically, the position / posture estimation unit 140 extracts historical data from the storage unit 130, and estimates the position and posture of the moving body based on the extracted historical data. At this time, the position / posture estimation unit 140 changes the extraction target of the history data based on the determination result. Specifically, the position / orientation estimation unit 140 excludes the inertial measurement value data associated with the determination condition satisfying a predetermined condition from the extraction target. Further, the position / posture estimation unit 140 determines the averaging period based on the motion determination value. This makes it possible to estimate the position and posture of the moving body with high accuracy.

FIG. 3 is a block diagram showing a functional configuration of the position / orientation estimation unit 140 according to the embodiment. As shown in FIG. 3, the position / attitude estimation unit 140 includes an averaging period calculation unit 141, an average value calculation unit 142, and a position / attitude integration unit 143.

The averaging period calculation unit 141 determines the inertial measurement value appropriate for estimating the position and posture of the moving body based on the motion determination value output from the motion determination unit 120 and the historical data extracted from the storage unit 130. Calculate the averaging period. Further, the averaging period calculation unit 141 calculates the number of averaging samples based on the calculated averaging period.

The averaging period is a period for averaging a plurality of inertial measurement values (specifically, angular velocities) obtained at equal time intervals. The calculation of the averaging period is substantially synonymous with the calculation of the number of inertial measurement units to be averaged, that is, the number of averaging samples. The number of averaged samples is a value obtained by dividing the calculated average period by the sampling cycle of IMU101. For example, when the averaging period is 0.02 seconds and the sampling period is 0.01 seconds (sampling rate 100 Hz), the number of averaging samples is 2 (= 0.02 ÷ 0.01).

FIG. 4 is a block diagram showing a functional configuration of the averaging period calculation unit 141 according to the embodiment. As shown in FIG. 4, the averaging period calculation unit 141 includes a variance characteristic calculation unit 141a and an averaging period determination unit 141b.

The dispersion characteristic calculation unit 141a calculates the dispersion values of a plurality of angular velocities included in the plurality of data sets extracted from the storage unit 130 for each candidate of the averaging period. The relationship between a plurality of candidates for the averaging period (hereinafter, may be referred to as a candidate averaging period) and the dispersion value corresponding to the candidate is referred to as a variance characteristic. That is, the dispersion characteristic calculation unit 141a calculates the dispersion characteristic of the angular velocity obtained by the measurement.

The averaging period determination unit 141b determines the averaging period based on the variance value for each candidate of the averaging period calculated by the variance characteristic calculation unit 141a. That is, the averaging period determination unit 141b determines the averaging period (the number of averaging samples) based on the dispersion characteristics. Further, in the present embodiment, the averaging period determination unit 141b further determines the averaging period based on the exercise determination value. Details will be described later.

Returning to FIG. 3, the average value calculation unit 142 outputs the average value of the inertial measurement values based on the historical data output from the storage unit 130 and the average number of samples output from the average period calculation unit 141. do. Specifically, the mean value calculation unit 142 calculates the average value of the inertial measurement values for each averaging period calculated by the averaging period calculation unit 141. More specifically, the average value calculation unit 142 calculates the average value of the angular velocity data for the number of the most recent averaged samples among the angular velocity data stored in the storage unit 130, and outputs the calculated average value. .. As the historical data used by the average value calculation unit 142, the extraction target is not changed depending on the determination result of the motion motion, and the data of all the inertial measurement values are sequentially used.

The position / attitude integration unit 143 estimates the position and attitude of the moving body based on the average value of the inertial measurement values calculated by the average value calculation unit 142. The position and attitude may be estimated using various sensor fusion methods such as an extended Kalman filter.

[3. Operation of autonomous navigation system (autonomous navigation method)]
Subsequently, the operation (autonomous navigation method) of the autonomous navigation device 100 according to the present embodiment will be described. First, the overall operation of the autonomous navigation system 100 will be described with reference to FIG. FIG. 5 is a flowchart showing the operation of the autonomous navigation system 100 according to the present embodiment.

First, in step S10, the inertial measurement unit 110 acquires and outputs the inertial measurement value of the moving body.

Next, in step S20, the motion determination unit 120 determines the motion motion of the moving body based on the inertial measurement value output from the inertial measurement unit 110. The motion determination unit 120 outputs an exercise determination value indicating a determination result of the exercise motion. The specific operation of step S20 will be described later.

Next, in step S30, the storage unit 130 stores a data set of the inertial measurement value output from the inertial measurement unit 110 and the motion determination value output from the motion determination unit 120. Each time an inertial measurement value is obtained, the storage unit 130 stores a data set. The time-series data of the data set of the stored inertial measurement value and the motion judgment value is the historical data.

Next, in step S40, the averaging period calculation unit 141 calculates the averaging period based on the exercise determination value output from the exercise determination unit 120 and the history data extracted from the storage unit 130. Specifically, the averaging period calculation unit 141 calculates the number of averaging samples. The specific operation of step S40 will be described later.

Next, in step S50, the mean value calculation unit 142 averages the inertial measurement values included in the historical data based on the number of averaged samples calculated by the average period calculation unit 141. Specifically, the average value calculation unit 142 averages the inertial measurement values for the number of the latest averaged samples in the historical data, and outputs the inertial measurement average value.

Next, in step S60, the position / attitude integrating unit 143 estimates the attitude and position of the moving body by integrating the inertial measurement average values. Specifically, the position / attitude integrating unit 143 integrates the inertial measurement average value based on the position and attitude estimated immediately before, and estimates the current position and attitude. For example, the position / attitude integrating unit 143 calculates the moving distance and the moving direction of the moving body based on the inertial measurement average value. The position / posture integrating unit 143 estimates the current position and posture by adding the calculated movement distance and movement direction to the position and posture estimated immediately before.

[3-1. Exercise determination process (step S20)]
Next, a specific example of the motion determination process (step S20) will be described with reference to FIGS. 6A, 6B, and 7.

FIG. 6A is a diagram showing an example of the movement of the moving body 10 on which the IMU 101 according to the embodiment is mounted. FIG. 6B is a diagram showing an example of inertial measurement data of the moving body 10 that has performed the motion shown in FIG. 6A.

FIG. 6A shows how the moving body 10 is traveling on a straight line and a curve. Specifically, at time t = 0 and T, the moving body 10 travels on a straight line (that is, goes straight), and at t = t1 and t2, the moving body 10 travels on a curve. The movement locus 20 is a travel route of the moving body 10 during the period from time t = 0 to time t = T.

Peaks 21 and 22 shown in FIG. 6B represent angular velocities at time t = t1 and t2, respectively. As shown in FIG. 6B, the absolute value of the angular velocity when the moving body 10 is traveling on a curve is larger than that when traveling in a straight line. Therefore, it is possible to determine whether the motion of the moving body is a "straight line" motion or a "curve" motion based on the angular velocity of the moving body 10. Specifically, it is possible to determine the motion motion of the moving body based on the comparison result between the absolute value of the angular velocity and the threshold value of 1 or more.

Hereinafter, a specific process of the motion determination process (step S20) will be described with reference to FIG. 7. FIG. 7 is a flowchart showing the motion determination process (step S20) in the present embodiment.

As shown in FIG. 7, first, in step S21, the motion determination unit 120 determines the motion motion of the moving body based on the acceleration output from the inertial measurement unit 110. Specifically, the motion determination unit 120 compares the acceleration with the threshold value X. If the acceleration is within the threshold value X (Yes in step S21), the process proceeds to step S22, and the motion determination unit 120 determines that the motion of the moving body is in the stopped state, and the motion indicating "stop" as the determination result. Output the judgment value. When the acceleration is larger than the threshold value X (No in step S21), the process proceeds to step S23. Since step S21 is a step of determining whether or not the moving body is "stopped", the determination may be made using other information such as a vehicle speedometer.

Next, in step S23, the motion determination unit 120 determines the motion motion of the moving body based on the angular velocity output from the inertial measurement unit 110. Specifically, the motion determination unit 120 compares the absolute value of the angular velocity with the threshold value Y. The threshold value Y is an example of the first threshold value. When the absolute value of the angular velocity is smaller than the threshold value Y (Yes in step S23), the process proceeds to step S24, and the motion determination unit 120 outputs a motion determination value indicating "straight ahead" as a determination result. The motion determination value representing "straight ahead" is an example of the first determination value. When the absolute value of the angular velocity is larger than the threshold value Y (No in step S23), the process proceeds to step S25.

Next, in step S25, the motion determination unit 120 compares the absolute value of the angular velocity output from the inertial measurement unit 110 with the threshold value Z. The threshold value Z is an example of the second threshold value, and is a value larger than the threshold value Y. When the absolute value of the angular velocity is smaller than the threshold value Z (Yes in step S25), the process proceeds to step S26, and the motion determination unit 120 outputs a motion determination value representing "curve (slow)" as the determination result. The motion determination value representing "curve (slow)" is an example of the second determination value. When the absolute value of the angular velocity is larger than the threshold value Z (No in step S25), the process proceeds to step S27, and the motion determination unit 120 outputs a motion determination value representing "curve (steep)" as the determination result. The motion determination value representing "curve (sudden)" is an example of the third determination value.

As described above, in the present embodiment, the motion determination unit 120 has 4 of "stop", "straight ahead", "curve (slow)" and "curve (sudden)" as the determination result of the motion motion of the moving body. Outputs one of the two judgment values. The four determination values are labeled with a label such as "curve (loose)" for convenience, but the present invention is not limited to this. For example, depending on the road surface condition on which the moving body travels, the absolute value of acceleration or angular velocity may be larger than the threshold value even when strong vibration is applied to the moving body. The determination value of the motion motion may be expressed by the magnitude and the magnitude of the vibration.

Note that steps S23 and S25 are steps for determining whether the moving body is "straight ahead" or "curve movement", and the strength of the curve. Therefore, instead of the angular velocity, the steering of the moving body or the like may be detected to make these determinations.

[3-2. Average period calculation process (step S40)]
Next, a specific example of the averaging period calculation process (step S40) will be described with reference to FIGS. 8 to 11.

FIG. 8 is a flowchart showing the averaging period calculation process (step S40) according to the present embodiment.

As shown in FIG. 8, first, in step S41, the dispersion characteristic calculation unit 141a determines the motion representing "curve (sudden)" or "curve (slow)" among the historical data stored in the storage unit 130. Exclude datasets containing values from extraction. That is, the dispersion characteristic calculation unit 141a extracts a data set including a motion determination value representing "stop" or "straight ahead". In other words, the angular velocity data whose absolute value of the angular velocity is smaller than the threshold value Y is the target of extraction.

Next, in step S42, the dispersion characteristic calculation unit 141a calculates the dispersion characteristics using the extracted plurality of data sets. Next, in step S43, the averaging period determination unit 141b calculates the number of averaging samples based on the variance characteristics calculated by the variance characteristic calculation unit 141a.

[3-2-1. Dispersion characteristic calculation process (step S42)]
Next, a specific process of the dispersion characteristic calculation process (step S42) will be described with reference to FIG. 9. FIG. 9 is a flowchart showing a dispersion characteristic calculation process (step S42) according to the present embodiment.

As shown in FIG. 9, first, in step S421, the dispersion characteristic calculation unit 141a selects an unselected averaging period from a plurality of candidate averaging periods. The candidate averaging period is a candidate for a period during which the inertial measurement value is averaged. For example, it is a set of about 10 to 100 pieces obtained by multiplying the sampling period of the inertial measurement unit 110 by a constant.

In step S422, the variance characteristic calculation unit 141a calculates the variance σ ² (τ) corresponding to the candidate averaging period τ selected in step S421 based on (Equation 1) to (Equation 3).

In (Equation 1), ΔT is the sampling period of the IMU 101. m is the number of averaged samples. Further, x _i represents an inertial measurement value (specifically, an angular velocity) of the i-sample eye other than "curve (sudden)" and "curve (slow)".

As shown by (Equation 1) to (Equation 3), the motion determination value is calculated based on historical data other than "curve (sudden)" and "curve (slow)" in the calculation of the dispersion characteristic. That is, since the dispersion characteristic is calculated excluding the angular velocity having a large absolute value, the dispersion characteristic of the inertial measurement unit 110 can be estimated with high accuracy.

The criteria for determining the historical data used for calculating the dispersion characteristics may follow other criteria. For example, the motion determination unit 120 detects the vibration state of running, and the history data determined to be "high vibration" is not used for the calculation of the dispersion characteristic.

Next, in step S423, the variance characteristic calculation unit 141a selects an unselected candidate averaging period (that is, a candidate averaging period for which the corresponding variance value has not been calculated) among the plurality of candidate averaging periods in step S421. Determines if is present. If there is an unselected candidate averaging period (Yes in step S423), the process proceeds to step S421, and the selection of the candidate averaging period and the calculation of the corresponding variance value are repeated. If there is no unselected candidate averaging period (No in step S423), that is, if the variance values for all the candidate averaging periods are calculated, the variance characteristic calculation unit 141a may use the plurality of candidate averaging periods. The set with the corresponding variance value is output as a variance characteristic (for example, information corresponding to the graph shown in FIG. 11).

[3-2-2. Calculation process of averaged sample size (step S43)]
Next, the calculation process of the average number of samples (step S43) will be described with reference to FIGS. 10, 11 and 12.

FIG. 10 is a flowchart showing a process of determining the number of averaged samples (step S43) according to the present embodiment. FIG. 11 is a diagram showing an example of the dispersion characteristic calculated by the dispersion characteristic calculation unit 141a according to the present embodiment.

As shown in FIG. 10, first, in step S431, when the motion determination value output by the motion determination unit 120 represents a “curve (steep)” (Yes in step S431), the process transitions to step S432. In step S432, the averaging period determination unit 141b selects the minimum (shortest) averaging period from the plurality of candidate averaging periods. In the example shown in FIG. 11, the smallest averaging period 31 is selected from the candidate averaging periods corresponding to the 13 black circles.

If the motion determination value does not represent "curve (sudden)" (No in step S431), the process proceeds to step S433. In step S433, when the motion determination value represents a “curve (slow)” (Yes in step S433), the process transitions to step S434. In step S434, the averaging period determination unit 141b selects an averaging period shorter than the averaging period in which the estimated variance value is minimized among the plurality of candidate averaging periods. In the example shown in FIG. 11, the averaging period 33 is the averaging period in which the corresponding variance value is minimized. Therefore, in step S434, the averaging period determination unit 141b selects an averaging period shorter than the averaging period 33. For example, the averaging period 32 is selected, but is not limited to this. For example, the minimum averaging period 31 may be selected. In this case, since it is the same as step S432, the determination in step S431 may not be performed.

The averaging period selected in steps S432 and S434 may be selected by a different method. For example, in step S434, the motion determination unit 120 determines the curvature of the curve of the motion of the moving body, and sets the averaging period in inverse proportion to the magnitude of the curvature.

When the motion determination value does not represent a "curve (slow)" (No in step S433), in step S435, the averaging period determination unit 141b has the variance value among the variance characteristics calculated by the dispersion characteristic calculation unit 141a. Select the minimum averaging period. In the example shown in FIG. 11, the averaging period 33 is selected.

Note that the averaging period selected in step S435 may become an unnecessarily long value, so it is possible to set an upper limit value for the averaging period. In this case, in step S435, the averaging period determination unit 141b selects the averaging period in which the variance value is the minimum within the range of the upper limit value or less.

After the averaging period is selected in steps S432, S434 or S435, in step S436, the averaging period determination unit 141b determines the selected averaging period τ as represented by the above-mentioned (Equation 1). The value divided by the sampling period ΔT of the IMU 101 is output as the average number of samples m.

FIG. 12 is a diagram showing the relationship between the motion of the moving body and the average number of samples according to the present embodiment. FIG. 12A shows the estimated position of the moving body by autonomous navigation when the average number of samples is small during linear motion or stoppage. The white circles indicate the estimated positions at each time. Further, the broken line represents the actual movement locus of the moving body. It should be noted that these illustrated methods are the same for (b) to (d). Further, here, "the number of averaged samples is small" refers to a period larger and shorter than the average period 33 selected in step S435, for example, a value near the average period 31 selected in step S432. (B) represents the estimated position of the moving body by autonomous navigation when a large number of averaged samples is taken during linear motion and stoppage. Here, "the number of averaged samples is large" refers to a value near the average period 33 selected in step S435.

At this time, in the case of linear motion and stoppage, the dispersion of the inertial measurement value of the inertial measurement unit 110 becomes smaller by lengthening the averaging period, that is, by taking a large number of averaging samples. This makes it possible to reduce the estimation error in autonomous navigation. Further, at this time, the dispersion characteristic used for determining the averaging period is calculated based on the data excluding the data having a large inertial measurement value as described above. Therefore, since the accuracy of the dispersion characteristics is high, the averaging period and the number of averaging samples can be appropriately selected. Therefore, the position and posture of the moving body can be estimated more accurately.

(C) of FIG. 12 shows the estimated position of the moving body by autonomous navigation when the average number of samples is taken small during the curve motion. FIG. 12D shows the estimated position of the moving body by autonomous navigation when a large number of averaged samples are taken during the curve motion.

During the curve motion, take a large number of averaging samples and take a time interval rather than the contribution to the deterioration of the estimation accuracy of autonomous navigation due to the large variance of the inertial measurement value due to the small number of averaging samples. It contributes greatly to the deterioration of the estimation accuracy of autonomous navigation due to the increase in the length. Therefore, by reducing the number of averaged samples during a curve, it is possible to reduce the estimation error of the position and attitude of the moving body in autonomous navigation.

As described above, according to the present embodiment, different averaging sample numbers (averaging period) are selected based on the determination result of the motion determination unit 120. This makes it possible to set an appropriate number of averaged samples according to the motion motion of the moving body, so that the estimation accuracy of the position and posture of the moving body can be improved.

(Other embodiments)
Although the autonomous navigation system and the autonomous navigation method according to one or more embodiments have been described above based on the embodiments, the present disclosure is not limited to these embodiments. As long as the gist of the present disclosure is not deviated, various modifications that can be conceived by those skilled in the art are applied to the present embodiment, and a form constructed by combining components in different embodiments is also included in the scope of the present disclosure. Will be.

For example, in the above embodiment, the autonomous navigation system 100 includes the storage unit 130, but the autonomous navigation system 100 does not have to include the storage unit 130 (memory 103). For example, the storage unit 130 may be provided in an external device (for example, a server device) different from the autonomous navigation system 100. The autonomous navigation device 100 can communicate with an external device including a storage unit 130 by wire or wirelessly, and may transmit inertial measurement values and motion determination values, and may receive historical data.

Further, for example, in the motion determination process shown in FIG. 7, in step S21, the speed and the threshold value may be compared. When the speed is equal to or less than the threshold value (Yes in step S21), the motion determination unit 120 may output an exercise determination value indicating "stop" as the determination result. If the speed is greater than the threshold (No in step S21), step S23 may be executed.

Alternatively, step S21 may be omitted. That is, the motion determination unit 120 does not have to compare the acceleration with the threshold value X, and the determination result may not include "stop".

Further, for example, in the motion determination process shown in FIG. 7, step S25 may be omitted. That is, the motion determination unit 120 does not have to compare the absolute value of the angular velocity with the threshold value Z. That is, the motion determination unit 120 does not have to discriminate between a gentle curve and a sharp curve. In this case, when the absolute value of the angular velocity is equal to or greater than the threshold value Y (No in step S23), the motion determination unit 120 outputs a motion determination value indicating a “curve” as the determination result.

In this way, when the strength of the curve is not distinguished, in the determination of the average number of samples shown in FIG. 10, in step S431, it is determined whether or not the motion determination value is a “curve”, and step S434 is performed. It may be omitted. In this case, if the motion determination value is not a "curve" (No in step S431), any of steps S434 and S435 may be executed.

Further, for example, the curve may be divided into three or more stages. In this case, the steeper the curve, the shorter the averaging period is set.

Further, the communication method between the devices described in the above embodiment is not particularly limited. When wireless communication is performed between devices, the wireless communication method (communication standard) is, for example, short-range wireless communication such as ZigBee (registered trademark), Bluetooth (registered trademark), or wireless LAN (Local Area Network). be. Alternatively, the wireless communication method (communication standard) may be communication via a wide area communication network such as the Internet. Further, wired communication may be performed between the devices instead of wireless communication. Specifically, the wired communication is a power line carrier communication (PLC: Power Line Communication) or a communication using a wired LAN.

Further, in the above embodiment, another processing unit may execute the processing executed by the specific processing unit. Further, the order of the plurality of processes may be changed, or the plurality of processes may be executed in parallel. For example, the components of one device may be included in another device.

For example, the processing described in the above embodiment may be realized by centralized processing using a single device (system), or may be realized by distributed processing using a plurality of devices. good. Further, the number of processors that execute the above program may be singular or plural. That is, centralized processing may be performed, or distributed processing may be performed.

Further, in the above embodiment, all or a part of the components such as the control unit may be configured by dedicated hardware, or may be realized by executing a software program suitable for each component. May be good. Each component may be realized by a program execution unit such as a CPU or a processor reading and executing a software program recorded on a recording medium such as an HDD or a semiconductor memory.

Further, a component such as a control unit may be composed of one or a plurality of electronic circuits. The one or more electronic circuits may be general-purpose circuits or dedicated circuits, respectively.

One or more electronic circuits may include, for example, a semiconductor device, an IC, an LSI, or the like. The IC or LSI may be integrated on one chip or may be integrated on a plurality of chips. Here, it is called IC or LSI, but the name changes depending on the degree of integration, and it may be called system LSI, VLSI or ULSI. Also, FPGAs programmed after the LSI are manufactured can be used for the same purpose. LSI is an abbreviation for Large Scale Integration. In addition, VLSI and ULSI are abbreviations for Very Large Scale Integration and Ultra Large Scale Integration, respectively. FPGA is an abbreviation for Field Programmable Gate Array.

Further, the general or specific aspects of the present disclosure may be realized by a system, an apparatus, a method, an integrated circuit or a computer program. Alternatively, it may be realized by a computer-readable non-temporary recording medium such as an optical disk, HDD or semiconductor memory in which the computer program is stored. Further, it may be realized by any combination of a system, an apparatus, a method, an integrated circuit, a computer program and a recording medium.

Further, in each of the above embodiments, various changes, replacements, additions, omissions, etc. can be made within the scope of claims or the scope thereof.

The present disclosure can be used as an autonomous navigation device capable of accurately estimating the position and posture of a moving body, and can be used, for example, for automatic driving of a moving body.

10 Moving object 20 Moving locus 21, 22 Peak 31, 32, 33 Average period 100 Autonomous navigation device 101 IMU
102 Processor 103 Memory 104 Signal line 110 Inertivity measurement unit 111 Gyro sensor 112 Acceleration sensor 120 Motion determination unit 130 Storage unit 140 Position / orientation estimation unit 141 Averaged period calculation unit 141a Dispersion characteristic calculation unit 141b Averaged period determination unit 142 Mean value calculation Unit 143 Position / posture integration unit

Claims (13)

The measurement unit that acquires the inertial measurement value of the moving body,
A determination unit that determines the motion motion of the moving body based on the inertial measurement value acquired by the measurement unit, and a determination unit.
It includes an estimation unit that estimates the position and posture of the moving body based on the inertial measurement value acquired by the measurement unit and the determination result by the determination unit.
Inertial navigation system. The measuring unit repeatedly acquires the inertial measurement value of the moving body, and obtains the inertial measurement value.
The determination unit makes the determination for each inertial measurement value acquired by the measurement unit.
The estimation unit extracts a plurality of data sets from a storage unit in which a plurality of data sets of the inertial measurement value acquired by the measurement unit and the determination result corresponding to the inertial measurement value are stored, and a plurality of extracted data sets. Estimate the position and orientation of the moving object based on the data set,
The autonomous navigation system according to claim 1. Further, the storage unit is provided.
The autonomous navigation system according to claim 2. The inertial measurement unit includes the angular velocity of the moving body.
The determination unit
As the determination, the absolute value of the angular velocity is compared with the first threshold value.
When the absolute value is smaller than the first threshold value, the first determination value is output as the determination result.
When the absolute value is larger than the first threshold value, a second determination value different from the first determination value is output as the determination result.
The storage unit stores the output determination result and the angular velocity corresponding to the determination result as the data set in association with each other.
The autonomous navigation system according to claim 2 or 3. The estimation unit
A dispersion characteristic calculation unit that calculates the dispersion values of a plurality of angular velocities included in the plurality of data sets extracted from the storage unit for each candidate of the averaging period.
An averaging period determination unit that determines the averaging period based on the dispersion value for each candidate of the averaging period calculated by the dispersion characteristic calculation unit.
An average value calculation unit that calculates the average value of the inertial measurement values using the averaging period determined by the averaging period determination unit, and
A position / posture integrating unit that estimates the position and posture of the moving body based on the average value calculated by the average value calculation unit, and the like.
The autonomous navigation system according to claim 4. The dispersion characteristic calculation unit extracts the data set including the first determination value, and does not extract the data set including the second determination value.
The autonomous navigation system according to claim 5. When the determination result is the second determination value, the averaging period determination unit selects an averaging period shorter than the averaging period in which the corresponding variance value is minimized from among the plurality of candidates for the averaging period. select,
The autonomous navigation system according to claim 5 or 6. When the determination result is the first determination value, the averaging period determination unit selects an averaging period in which the corresponding variance value is minimized from a plurality of candidates for the averaging period.
The autonomous navigation system according to any one of claims 5 to 7. The determination unit
As the determination, further, the absolute value is compared with the second threshold value larger than the first threshold value.
When the absolute value is larger than the first threshold value and smaller than the second threshold value, the second determination value is output as the determination result.
When the absolute value is larger than the second threshold value, a third determination value different from both the first determination value and the second determination value is output as the determination result.
The autonomous navigation system according to any one of claims 5 to 8. The averaging period determination unit selects the minimum averaging period from a plurality of candidates for the averaging period when the determination result is the third determination value.
The autonomous navigation system according to claim 9. The inertial measurement unit includes at least one of the acceleration and the velocity of the moving body.
The autonomous navigation system according to any one of claims 1 to 10. The step to acquire the inertial measurement value of the moving body,
Based on the acquired inertial measurement value, the step of determining the motion motion of the moving body and
Including a step of estimating the position and posture of the moving body based on the acquired inertial measurement value and the result of the determination.
Autonomous navigation method. A program that causes a computer to execute the autonomous navigation method according to claim 12.

Images