JP2015102330A - Movement information calculation device, movement information calculation method, movement information calculation program, and mobile body - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a movement information calculation device capable of highly accurately estimating a sensor error, and highly accurately calculating movement information even when there is a small number of observation values.SOLUTION: A movement information calculation device 10 comprises a first error estimation unit 11A, a second error estimation unit 11B, a first inertial navigation operation unit 12A, and a second inertial navigation operation unit 12B. The first error estimation unit 11A estimates a first sensor error which is the sensor error of an inertial sensor by using positioning movement information, sensor movement information, and first integrated movement information which has been already calculated. The second error estimation unit 11B estimates a second sensor error which is a sensor error of a different kind from the first sensor error by using the positioning movement information, sensor movement information, and second integrated movement information which has been already calculated. The first inertial navigation operation unit 12A calculates current first integrated movement information from first correction sensor movement information obtained by correcting the sensor movement information by using the first sensor error and the second sensor error. The second inertial navigation operation unit 12B calculates current second integrated movement information from second correction sensor movement information obtained by correcting the sensor movement information by using the first sensor error and the second sensor error.

Description

  The present invention includes an inertial attitude measurement device (IMU) having an inertial sensor and a positioning device that performs positioning from a positioning signal, and is integrated from the output result of the IMU and the output result of the positioning device. The present invention relates to a movement information calculation device, a movement information calculation method, and a movement information calculation program for calculating movement information.

  2. Description of the Related Art Conventionally, various types of movement information calculation devices that include an inertial posture measurement device (IMU: Internal Measurement Unit (hereinafter referred to as IMU)) and a positioning device have been devised. The IMU includes an inertial sensor such as a gyro sensor or an acceleration sensor, and measures an angular velocity, a posture angle, and an acceleration by using an inertial force. The positioning device receives positioning signals broadcast from a plurality of positioning satellites, and calculates the distance between each positioning satellite and the positioning device. The positioning device calculates the position and speed from the distance and the change in distance.

  In general, if the observation conditions of the positioning satellite are good, the position and speed calculated by the positioning device are highly accurate, but the observation conditions are not always good.

  Therefore, as shown in the navigation device of Patent Document 1, the integrated processing that uses the angular velocity, posture angle, and acceleration measured by the IMU by correcting the position and velocity calculated by the positioning device is currently used as the movement information of the moving object. It is often used for calculation.

  For example, in the navigation device of Patent Document 1, the error of the gyro sensor is corrected using the GPS azimuth (azimuth by positioning). Moreover, in the navigation device of Patent Document 1, the error of the vehicle speed sensor is corrected using the GPS speed (speed by positioning).

  Errors generated by sensors such as a gyro sensor and a vehicle speed sensor include a misalignment error, a bias error, and a scale factor error. In the navigation device described in Patent Literature 1, the bias error and the scale factor error of each sensor are estimated by a Kalman filter set for each sensor. That is, the bias error and the scale factor error of the gyro sensor are estimated by the Kalman filter for the gyro sensor, and the bias error and the scale factor error of the vehicle speed sensor are estimated by the Kalman filter for the vehicle speed sensor.

Japanese Patent Laid-Open No. 7-301541

  However, in the conventional movement information calculation device such as the navigation device described in Patent Document 1, when the error is estimated with high accuracy, that is, when the finally output speed and position are calculated with high accuracy, Problem arises.

  When estimating sensor errors with high accuracy, it is necessary to estimate each error factor for each orthogonal three-axis error component in the moving body, in addition to estimating and calculating for each bias factor, scale factor error, and error factor described above. There is. That is, when the sensor error is estimated with high accuracy, the number of unknowns estimated by the estimation calculation increases.

  On the other hand, in such estimation, a known number (observed value) greater than or equal to the number of unknowns must be obtained, and as the unknown number increases, more observed values are required. In this case, the observed value is a position and speed calculated by the positioning device, a pseudo distance or a Doppler frequency that is a source of these position and speed. The number of observation values is determined by the number of satellites that can be observed at the estimation timing, in other words, the number of positioning signals that can be demodulated.

  Therefore, in the above-described movement information calculation device, if the sensor error estimation is performed with high accuracy, the required number of observable positioning satellites may not be obtained, resulting in a decrease in accuracy of the estimation error. End up. In addition, when the sensor error estimation can be performed even if the number of observable positioning satellites is small, the error estimation with high accuracy cannot be performed due to the small number of positioning satellites.

  An object of the present invention is to provide a movement information calculation device capable of estimating a sensor error with high accuracy even when the number of observable positioning satellites is small.

  The present invention relates to a movement information calculation device that calculates integrated movement information of a moving object using positioning movement information obtained from a positioning signal and sensor movement information obtained from an inertial sensor. Have. The movement information calculation device includes a first error estimation unit, a second error estimation unit, and an inertial navigation calculation unit. The first error estimation unit estimates a first sensor error, which is a sensor error of the inertial sensor, using the positioning movement information, the sensor movement information, and the already calculated first integrated movement information. The second error estimator estimates a second sensor error, which is a different type of sensor error from the first sensor error, using the positioning movement information, the sensor movement information, and the already calculated second integrated movement information. The inertial navigation calculation unit calculates the current first integrated movement information and the second integrated movement information from the corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error and the second sensor error.

  In this configuration, each error component constituting the sensor error is individually estimated by a plurality of error estimation units. Therefore, the unknown number of each error estimation unit can be reduced. Thereby, the known number (number of positioning movement information and sensor movement information) required for estimation of the error component can be reduced without reducing the estimation accuracy.

  Moreover, in the movement information calculation apparatus of this invention, a 1st error estimation part estimates the 1st calculation error which is a calculation error of 1st integrated movement information with a 1st sensor error. The second error estimating unit estimates a second calculation error that is a calculation error of the second integrated movement information together with the second sensor error.

  The inertial navigation calculation unit includes a first inertial navigation calculation unit and a second inertial navigation calculation unit. The first inertial navigation calculation unit calculates the current first integrated movement information from the first corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error, the second sensor error, and the first calculation error. The first integrated movement information is fed back to the first error estimation unit. The second inertial navigation calculation unit calculates the current second integrated movement information from the second corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error, the second sensor error, and the second calculation error. The second integrated movement information is fed back to the second error estimation unit.

  In this configuration, a loop process for error estimation and integrated movement information calculation is individually configured for each error component of the sensor error. Thereby, each error component can be reliably and highly accurately estimated by each individual loop process, and highly accurate integrated movement information can be calculated.

  Moreover, in the movement information calculation apparatus of this invention, the integrated movement which outputs either the 1st integrated movement information and the 2nd integrated movement information or the representative value based on the 1st integrated movement information and the 2nd integrated movement information to the outside An information determination unit is further provided.

  In this configuration, since the integrated movement information for output is determined using a plurality of integrated movement information obtained by different loop processes, it is possible to calculate more accurate integrated movement information.

  A movement information calculation system according to the present invention includes an inertial attitude measurement including the movement information calculation device according to any one of the above, a positioning signal receiving unit that calculates positioning movement information, and an inertial sensor that calculates sensor movement information. And a device.

  In this configuration, the integrated movement information can be calculated with high accuracy only by connecting an antenna that receives a positioning signal to the positioning signal receiving unit.

  According to another aspect of the present invention, there is provided a moving body according to any one of the above-described movement information calculation apparatus or movement information calculation system, a control unit that performs movement control based on integrated movement information, and drive control based on movement control. A power generation unit.

  In this configuration, by using the movement information calculated as described above, it is possible to constantly move and control the moving body with high accuracy.

  According to the present invention, it is possible to estimate the error of the sensor with high accuracy and to calculate the movement information with respect to the moving body such as speed and position with high accuracy.

It is a block diagram of the movement information calculation device concerning a 1st embodiment of the present invention. It is a flowchart of the movement information calculation method which concerns on the 1st Embodiment of this invention. It is a block diagram of the movement information calculation apparatus which concerns on the 2nd Embodiment of this invention. It is a flowchart of the movement information calculation method which concerns on the 2nd Embodiment of this invention. It is a block diagram of the movement information calculation apparatus which concerns on the 3rd Embodiment of this invention. It is a block diagram of the movement information calculation apparatus which concerns on the 4th Embodiment of this invention. It is a schematic block diagram of the moving body which concerns on embodiment of this invention.

  A movement information calculation apparatus and movement information calculation method according to the first embodiment of the present invention will be described with reference to the drawings. FIG. 1 is a block diagram of a movement information calculation apparatus according to the first embodiment of the present invention. FIG. 2 is a flowchart of the movement information calculation method according to the first embodiment of the present invention.

  The movement information calculation apparatus 101 includes a first integration processing unit 10A and a second integration processing unit 10B.

  The first integrated processing unit 10A includes a first error estimation unit 11A and a first inertial navigation calculation unit 12A. The first inertial navigation calculation unit 12A includes a first IMU error correction unit 121A, a first position velocity calculation unit 122A, and a first 1 posture angle calculation part 123A is provided.

Position (GPS position) P _GPS and speed (GPS speed) V _GPS calculated by receiving a GPS signal that is a positioning signal are input to first error estimating unit 11A. These position P _GPS and speed V _GPS correspond to positioning movement information. These position P _GPS and velocity V _GPS are calculated by a known method based on a code pseudorange and a Doppler frequency obtained by tracking a plurality of positioning signals. Further, the acceleration (IMU acceleration) a _IMU and the angular velocity (IMU angular velocity) ω _IMU measured by the _IMU are input to the first error estimation unit 11A. These acceleration a _IMU and angular velocity ω _IMU correspond to the sensor movement information of the present invention.

In addition, the first error estimation unit 11A includes observation values (position P _GPS , velocity V _GPS , acceleration a _IMU and angular velocity ω _IMU ) at the previous sampling timing, and bias error Er _bi estimated at the previous sampling timing. , Scale position error Er _SF , attitude angle calculation error Er _ΦA , speed calculation error Er _VA , position calculation error Er _PA , integrated position P _un , integrated speed V _un , and integrated attitude angle Φ _un are input. . These integrated position P _un , integrated speed V _un , and integrated posture angle Φ _un correspond to the first integrated movement information. The bias error Er _bi and the scale factor error Er _SF are an inertial sensor (gyro sensor) provided in an IMU (not shown) that gives an acceleration a _IMU and an angular velocity ω _IMU to the first and second integrated processing units 10A and 10B. Or an acceleration sensor).

11A of 1st error estimation parts are comprised by the Kalman filter which is a state estimation equation. In this Kalman filter, a position P _GPS , a speed V _GPS , an integrated position P _un , an integrated speed V _un , and an integrated attitude angle Φ _un are given as observation values, a bias error Er _bi , an attitude angle calculation error Er _ΦA , and a speed calculation error. Er _VA and position calculation error Er _PA are set to unknowns.

The first error estimator 11A inputs the above-described observation values at each sampling timing, and estimates the unknown bias error Er _bi , attitude angle calculation error Er _ΦA , speed calculation error Er _VA , and position calculation error Er _PA . calculate. The first error estimation unit 11A outputs the estimated calculation speed calculation error Er _VA and position calculation error Er _PA to the first position speed calculation unit 122A. The first error estimator 11A outputs the estimated posture angle calculation error _ErΦA to the posture angle calculator 123A. These attitude angle calculation error Er _.PHI.A, speed calculation error _{Er VA,} the position calculation error _{Er PA} corresponds to the first calculation error of the present invention.

The first error estimation unit 11A outputs the estimated and calculated bias error Er _bi to the first IMU error correction unit 121A. Further, the first error estimation unit 11A outputs the estimated and calculated bias error Er _bi to the second IMU error correction unit 121B of the second inertial navigation calculation unit 12B. This bias error Er _bi corresponds to the first sensor error of the present invention.

Acceleration (IMU acceleration) a _IMU and angular velocity (IMU angular velocity) ω _IMU measured by the _IMU are input to the first _IMU error correction unit 121A. The bias error Er _bi estimated by the first error estimation unit 11A is input to the first IMU error correction unit 121A. Furthermore, the scale factor error Er _SF estimated by the second error estimation unit 11B of the second integration processing unit 10B described later is input to the first IMU error correction unit 121A.

The first IMU error correction unit 121A corrects and outputs the acceleration a _IMU and the angular velocity ω _IMU with the bias error Er _bi and the scale factor error Er _SF . The first IMU error correction unit 121A outputs the corrected acceleration a _IMU to the first position / velocity calculation unit 122A, and outputs the corrected angular velocity ω _IMU to the first posture angle calculation unit 123A. These corrected acceleration a _IMU and corrected angular velocity ω _IMU correspond to the corrected sensor movement information of the present invention.

The first position speed calculation unit 122A calculates the integrated position P _un and the integrated speed V _un from the corrected acceleration a _IMU . For example, the first position speed calculation unit 122A calculates the integrated speed V _un by accumulating the corrected acceleration a _IMU, and uses the speed V _un and an initial value acquired in advance by another method to determine the integrated position. _Pun is calculated. At this time, the first position speed calculation unit 122A calculates the integrated position P _un and the integrated speed V _un using the speed calculation error Er _VA and the position calculation error Er _PA estimated by the first error estimation unit 11A.

The first position speed calculation unit 122A outputs the integrated position P _un and the integrated speed V _un to the outside of the movement information calculation apparatus 101 and feeds back to the first error estimation unit 11A.

First attitude angle calculation unit 123A calculates the integrated attitude angles [Phi _un from the corrected angular velocity omega _IMU. For example, the first attitude angle calculation unit 123A calculates the integrated attitude angles [Phi _un by integrating the angular velocity omega _IMU corrected. At this time, the first posture angle calculation unit 123A, using the estimated posture angle calculation error Er _.PHI.A, calculates the integrated attitude angles [Phi _un.

First attitude angle calculation unit 123A outputs the integration posture angle [Phi _un outside the movement information calculation unit 101 is fed back to the first error estimator 11A.

With such a configuration, the first error estimation unit 11A estimates the bias error Er _bi , the attitude angle calculation error Er _ΦA , the speed calculation error Er _VA , and the position calculation error Er _PA with high accuracy, and the first inertial navigation calculation unit 12A can calculate the integrated position P _un , the integrated speed V _un and the integrated attitude angle Φ _un with high accuracy.

  The second integration processing unit 10B includes a second error estimation unit 11B and a second inertial navigation calculation unit 12B. The second inertial navigation calculation unit 12B includes a second IMU error correction unit 121B, a second position speed calculation unit 122B, and a second attitude angle calculation unit 123B.

The position P _GPS and the velocity V _GPS calculated by receiving a GPS signal that is a positioning signal are input to the second error estimation unit 11B. Further, the acceleration (IMU acceleration) a _IMU and the angular velocity (IMU angular velocity) ω _IMU measured by the _IMU are input to the second error estimation unit 11B. These position P _GPS , velocity V _GPS , acceleration (IMU acceleration) a _IMU , and angular velocity (IMU angular velocity) ω _IMU are the same as those input to the first error estimation unit 11A, and the input timing is also synchronized. Yes.

The second error estimator 11B also includes the observation values (position P _GPS , velocity V _GPS , acceleration a _IMU and angular velocity ω _IMU ) at the previous sampling timing and the bias error Er _bi estimated at the previous sampling timing. , Scale position error Er _SF , attitude angle calculation error Er _ΦB , speed calculation error Er _VB , position calculation error Er _PB , integrated position P _unB , integrated speed V _unB , and integrated attitude angle Φ _unB are input. . The integrated position P _unB , the integrated speed V _unB , and the integrated posture angle Φ _unB correspond to the second integrated movement information.

The second error estimation unit 11B is configured by a Kalman filter. In this Kalman filter, a position P _GPS , a speed V _GPS , an integrated position P _unB , an integrated speed V _unB , and an integrated attitude angle Φ _unB are given as observation values, a bias error Er _bi , an attitude angle calculation error Er _ΦB , a speed calculation error Er _VB and position calculation error Er _PB are set to unknowns.

The second error estimator 11B inputs the above observed values at each sampling timing, and calculates the unknown scale factor error Er _SF , attitude angle calculation error Er _ΦB , speed calculation error Er _VB , and position calculation error Er _PB . Estimate and calculate. The second error estimation unit 11B outputs the estimated calculation speed calculation error Er _VB and the position calculation error Er _PB to the second position speed calculation unit 122B. The second error estimator 11B outputs the estimated posture angle calculation error _ErΦB to the second posture angle calculator 123B. These attitude angle calculation error Er _.PHI.B, speed calculation error _{Er VB,} the position calculation error _{Er PB} corresponds to the second calculation error of the present invention.

The second error estimation unit 11B outputs the estimated scale factor error Er _SF to the second IMU error correction unit 121B. Further, the second error estimation unit 11B outputs the estimated scale factor error Er _SF to the first IMU error correction unit 121A of the first inertial navigation calculation unit 12A. This scale factor error Er _SF corresponds to the second sensor error of the present invention.

The acceleration I _IMU and angular velocity ω _IMU measured by the _IMU are input to the second _IMU error correction unit 121B. The acceleration a _IMU and the angular velocity ω _IMU are the same as those input to the first _IMU error correction unit 121A, and the input timing is also synchronized.

The scale factor error Er _SF estimated by the second error estimation unit 11B is input to the second IMU error correction unit 121B. Furthermore, the bias error Er _bi estimated by the first error estimation unit 11A of the first integration processing unit 10A is input to the second IMU error correction unit 121B.

The second IMU error correction unit 121B corrects and outputs the acceleration a _IMU and the angular velocity ω _IMU by the bias error Er _bi and the scale factor error Er _SF . The second IMU error correction unit 121B outputs the corrected acceleration a _IMU to the second position / velocity calculation unit 122B, and outputs the corrected angular velocity ω _IMU to the second posture angle calculation unit 123B.

The second position speed calculation unit 122B calculates the integrated position P _unB and the integrated speed V _unB from the corrected acceleration a _IMU . For example, the second position speed calculation unit 122B calculates the integrated speed V _unB by integrating the corrected acceleration a _IMU, and integrates using the integrated speed V _unB and an initial value acquired in advance by another method. The position _Pun is calculated. At this time, the second position speed calculation unit 122B calculates the integrated position P _unB and the integrated speed V _unB using the speed calculation error Er _VB and the position calculation error Er _PB estimated by the second error estimation unit 11B.

The second position speed calculation unit 122B _{feeds back} the integrated position P _unB and the integrated speed V _unB to the second error estimation unit 11B.

Second posture angle calculator 123B calculates the integrated attitude angles [Phi _UNB from the corrected angular velocity omega _IMU. For example, the second posture angle calculator 123B calculates the integrated attitude angles [Phi _UNB by integrating the angular velocity omega _IMU corrected. At this time, the second posture angle calculator 123B, using the estimated posture angle calculation error Er _.PHI.B, calculates the integrated attitude angles [Phi _UNB.

The second posture angle calculation unit 123B _{feeds back} the integrated posture angle _ΦunB to the second error estimation unit 11B.

With such a configuration, the second error estimating unit 11B estimates the scale factor error Er _SF , the attitude angle calculation error Er _ΦB , the speed calculation error Er _VB , and the position calculation error Er _PB with high accuracy, and performs the second inertial navigation calculation. The unit 12B can calculate the integrated position P _unB , the integrated speed V _unB, and the integrated posture angle Φ _unB with high accuracy.

Further, in the configuration of the present embodiment, the first error estimator 11A estimates the bias error Er _bi for the IMU sensor, and the second error estimator 11B estimates the scale factor error Er _SF for the IMU sensor. That is, the bias error Er _bi and the scale factor error Er _SF that constitute the sensor error of the same sensor are estimated by different error estimation units. Thereby, the number of unknowns estimated by each error estimator can be reduced, and the number of required observation values (known numbers) can be reduced. Therefore, in the configuration of the present embodiment, it is possible to estimate the bias error Er _bi and the scale factor error Er _SF with a smaller number of observation values without degrading the estimation accuracy than estimating with one error estimation unit. it can.

Here, since the observation value is obtained by tracking the positioning signal as described above, the number of positioning signals that need to be tracked can be reduced by reducing the number of observation values. Thereby, even if the number of positioning signals that can be tracked is smaller than that of the conventional configuration, the bias error Er _bi and the scale factor error Er _SF can be estimated with high accuracy, and the integrated position P _un , integrated speed V _un and it is possible to calculate the integrated attitude angles [Phi _un high precision.

  In the above description, the first and second error estimators 11A and 11B are configured by Kalman filters. However, other state estimation equations may be used. Further, if the first and second error estimation units 11A and 11B are individually configured, it is possible to integrate the inertial navigation calculation unit.

In the above configuration, an example in which the integrated position P _un , the integrated speed V _un and the integrated attitude angle Φ _un output from the first integration processing unit 10A are output to the outside is shown. The output integrated position P _unB , integrated speed V _unB, and integrated attitude angle Φ _unB may be output to the outside.

  In the above description, the movement information is calculated separately for each functional block. However, the movement information may be calculated by storing each function described above as a program and executing the program by an arithmetic processor such as a computer (see FIG. 2).

First, a GPS velocity V _GPS , a GPS position P _GPS , an IMU angular velocity ω _IMU , and an IMU acceleration a _IMU are acquired as observed values (S101). These observation values are continuously acquired at preset sampling intervals.

Next, a first Kalman filter is set with the bias error Er _bi , the attitude angle calculation error Er _ΦA , the speed calculation error Er _VA , and the position calculation error Er _PA as unknowns. The known number (observed value) of the first Kalman filter is the acquired GPS speed V _GPS , GPS position P _GPS , integrated speed V _un , integrated acceleration a _un , and integrated attitude angle Φ _un obtained in the previous calculation process. is there. Then, by executing the first Kalman filter, the bias error Er _bi , the attitude angle calculation error Er _ΦA , the speed calculation error Er _VA , and the position calculation error Er _PA are estimated and calculated (S102).

A second Kalman filter is set with the scale factor error Er _SF , the attitude angle calculation error Er _ΦB , the speed calculation error Er _VB , and the position calculation error Er _PB as unknowns. The known number (observed value) of the second Kalman filter is the acquired GPS speed V _GPS , GPS position P _GPS , integrated speed V _unB , integrated acceleration a _unB , and integrated attitude angle Φ _unB obtained in the previous calculation process. is there. Then, by executing the second Kalman filter, the scale factor error Er _SF , the attitude angle calculation error Er _ΦB , the speed calculation error Er _VB , and the position calculation error Er _PB are estimated and calculated (S 103).

Next, the acquired IMU angular velocity ω _IMU and IMU acceleration a _IMU are corrected by the bias error Er _bi estimated by the first Kalman filter and the scale factor error Er _SF estimated by the second Kalman filter. Using the corrected IMU angular velocity ω _IMU , IMU acceleration a _IMU , attitude angle calculation error Er _ΦA , speed calculation error Er _VA , and position calculation error Er _PA , the integrated speed V _un , the integrated acceleration a _un , and the integrated attitude angle calculating the Φ _un (S104). The integrated velocity V _un , integrated acceleration a _un , and integrated posture angle Φ _un calculated in this way are output to the outside and fed back to the first Kalman filter.

Using the corrected IMU angular velocity ω _IMU , IMU acceleration a _IMU , attitude angle calculation error Er _ΦB , speed calculation error Er _VB , and position calculation error Er _PB , the integrated speed V _unB , the integrated acceleration a _unB , and the integrated attitude angle Φ _unB is calculated (S105). The integrated velocity V _unB , integrated acceleration a _unB , and integrated posture angle Φ _unB calculated in this way are fed back to the second Kalman filter.

By performing such processing, the bias error Er _bi and the scale factor error Er _SF can be estimated with high accuracy even if the number of positioning signals that can be tracked is small, and the integrated position P _un , the integrated speed V the _un and integration posture angle [Phi _un can be calculated with high accuracy.

  Note that step S102 and step S103 may be processed in parallel or may be time-series processed. Moreover, step S104 and step S105 may be processed simultaneously in parallel, or may be time-series processed.

  Next, a movement information calculation apparatus and movement information calculation method according to the second embodiment will be described with reference to the drawings. FIG. 3 is a block diagram of a movement information calculation apparatus according to the second embodiment of the present invention. FIG. 4 is a flowchart of the movement information calculation method according to the second embodiment of the present invention.

  The movement information calculation apparatus 102 of this embodiment includes an integrated movement information determination unit 120, and is the same as the movement information calculation apparatus 101 shown in the first embodiment, except for portions related to the integration movement information determination unit 120. is there. Therefore, only different parts will be specifically described.

The integrated movement information determining unit 120 includes a first integrated position P _unA , a first integrated speed V _unA and a first integrated posture angle Φ _unA output from the first integrated processing unit 10A, and a second integrated processing unit. The second integrated position P _unB , the second integrated speed V _unB and the second integrated attitude angle Φ _unB output from 10B are input.

Integrated mobile information determination unit 120 determines the integrated position _{P un} from the first integrated position _{P una} and second integrated position _{P UNB.} Specifically, integrated movement information determining unit 120 calculates the average value of the first integration position _{P una} and second integrated position _{P UNB,} and outputs as an integrated position _{P un.} The integrated movement information determination unit 120 compares the error variance of the first Kalman filter that constitutes the first error estimator 11A with the error variance of the second Kalman filter that constitutes the second error estimator 11B. The integrated position with the smaller variance may be output as the integrated position _Pun as the movement information calculation device 102. Further, the integrated movement information determination unit 120 may _{perform a} weighted averaging process on the first integrated position _PunA and the second integrated position _PunB . At this time, the weight may be set by the error variance of the Kalman filter described above. Based on the first integrated position P _una the average value and the second integrated position P _UNB, generally statistical computation value to suppress the error corresponds to a "typical" of the present invention. The same applies to the following speeds and posture angles.

The integrated movement information determination unit 120 determines an integrated speed V _un as the movement information calculation device 102 from the first integrated speed V _unA and the second integrated speed P _unB . Method of determining the integrated velocity _{V un} is the same as the integrated position _{P un.}

Integrated mobile information determination unit 120 determines the integrated attitude angles [Phi _un as movement information calculation device 102 and a first integrated attitude angles [Phi _una and second integrated attitude angles [Phi _UNB. Method of determining the integrated attitude angles [Phi _un is the same as the integrated position _{P un} and integration speed _{V un.}

With such a configuration, as the moving information calculating unit 102, and integrated position P _un integrated velocity V _un integrated attitude angles [Phi _un, can be calculated more accurately.

  In the above description, the case where the calculation of movement information is divided into functional blocks is shown. However, the movement information may be calculated by storing each function described above as a program and executing it by a computer such as a computer (see FIG. 4).

The movement information calculation method (movement information calculation program) shown in FIG. 4 is different from the movement information calculation method (movement information calculation program) shown in FIG. 2 except that the calculation processing of the integrated position P _un , the integrated speed V _un, and the integrated attitude angle Φ _un is different. This is the same as the movement information calculation program. That is, steps S201, S202, S203, and S204 in FIG. 4 are the same as steps S101, S102, S103, and S104 in FIG.

When a first integrated position _{P una} and second integrated position _{P UNB} is obtained, from these first integrated position _{P una} and second integrated position _{P UNB,} to determine an integrated position _{P un} to external output ( S206). This determination method is the same as the determination method by the integrated movement information determination unit 120 described above.

When the first integrated speed V _unA and the second integrated speed V _unB are obtained, the integrated speed V _un to be externally output is determined from the first integrated speed V _unA and the second integrated speed V _unB ( S207). This determination method is the same as the determination method by the integrated movement information determination unit 120 described above.

When the first integrated posture angle _ΦunA and the second integrated posture angle _ΦunB are obtained, an integrated posture angle Φ output from the first integrated posture angle _ΦunA and the second integrated posture angle _{ΦunB is} output. _un is determined (S208). This determination method is the same as the determination method by the integrated movement information determination unit 120 described above.

By using such a method, an integrated position P _un integrated velocity V _un integrated attitude angles [Phi _un, can be calculated more accurately.

  Next, a movement information calculation apparatus according to the third embodiment will be described with reference to the drawings. FIG. 5 is a block diagram of a movement information calculation apparatus according to the third embodiment of the present invention.

  The movement information calculation apparatus 103 of this embodiment shows a case where there are three integrated processing units, and the basic configuration is the same as that of the movement information calculation apparatus 101 according to the first embodiment. Therefore, only different parts will be specifically described.

The movement information calculation device 103 includes a first integration processing unit 10A, a second integration processing unit 10B, and a third integration processing unit 10C. The first error estimation unit 11A of the first integration processing unit 10A estimates and calculates the bias error Er _bi . The second error estimation unit 11B of the second integration processing unit 10B estimates and calculates the scale factor error Er _SF . The third error estimation unit 11C of the third integration processing unit 10C estimates and calculates the misalignment error Er _mis .

The first inertial navigation calculation unit 12A of the first integration processing unit 10A corrects the angular velocity ω _IMU and the acceleration a _IMU with the bias error Er _bi , the scale factor error Er _SF, and the misalignment error Er _mis . The first inertial navigation calculation unit 12A uses the corrected angular velocity ω _IMU and acceleration a _IMU , the attitude angle calculation error Er _ΦA , the velocity calculation error Er _VA , and the position calculation error Er _PA , and uses the integrated position P _un , speed _{V un,} to calculate the integrated attitude angle Φ _un. The first inertial navigation calculation unit 12A outputs the integrated position P _un , the integrated speed V _un , and the integrated attitude angle Φ _un to the outside, and feeds it back to the first error estimating unit 11A.

The second inertial navigation calculation unit 12B of the second integration processing unit 10B corrects the angular velocity ω _IMU and the acceleration a _IMU with the bias error Er _bi , the scale factor error Er _SF, and the misalignment error Er _mis . The second inertial navigation calculation unit 12B uses the corrected angular velocity ω _IMU and acceleration a _IMU , the posture angle calculation error Er _ΦB , the velocity calculation error Er _VB , and the position calculation error Er _PB to use the integrated position P _unB , The velocity V _unB and the integrated posture angle Φ _unB are calculated. The second inertial navigation calculation unit 12B _{feeds back} the integrated position P _unB , the integrated speed V _unB , and the integrated attitude angle Φ _unB to the second error estimating unit 11B.

The third inertial navigation calculation unit 12C of the third integration processing unit 10C corrects the angular velocity ω _IMU and the acceleration a _IMU with the bias error Er _bi , the scale factor error Er _SF, and the misalignment error Er _mis . The third inertial navigation calculation unit 12C uses the corrected angular velocity ω _IMU and acceleration a _IMU , the attitude angle calculation error Er _ΦC , the velocity calculation error Er _VC , and the position calculation error Er _PC , and uses the integrated position P _unC , The velocity V _unC and the integrated posture angle Φ _unC are calculated. The third inertial navigation calculation unit 12C _{feeds back} the integrated position P _unC , the integrated speed V _unC , and the integrated attitude angle Φ _unC to the error estimating unit 11C.

With such a configuration, it is possible to estimate more error factors without increasing the number of observation values. Thereby, the error correction of the sensor can be performed with higher accuracy, and the integrated position P _un , the integrated speed V _un , and the integrated posture angle Φ _un can be calculated with higher accuracy.

  In addition, you may add the integrated movement information determination part shown in 2nd Embodiment to the structure of this embodiment.

  Next, a movement information calculation system according to the fourth embodiment of the present invention will be described with reference to the drawings. FIG. 6 is a block diagram of a movement information calculation system according to the fourth embodiment of the present invention.

  The movement information calculation system 104 of this embodiment is obtained by adding a positioning signal receiver 20 and an IMU 30 to the movement information calculation apparatus 101 shown in the first embodiment. Therefore, a specific description regarding the configuration of the movement information calculation apparatus 101 is omitted.

  The movement information calculation system 104 includes a movement information calculation apparatus 101, a positioning signal receiver 20 and an IMU 30.

The positioning signal receiving unit 20 receives and tracks a GPS signal that is a positioning signal, and calculates a pseudorange and a Doppler frequency from a code phase and a carrier phase by tracking. The positioning signal receiving unit 20 calculates the position P _GPS and the velocity V _GPS from the pseudorange and the Doppler frequency, and outputs them to the first and second error estimation units 11A and 11B.

The IMU 30 includes a gyro sensor and an acceleration sensor. The _{IMU 30} detects the inertial force applied to the sensor, measures the acceleration a _IMU and the angular velocity ω _IMU , and outputs them to the first and second IMU error correction units 121A and 121B.

With such a configuration, the integrated position P _un , the integrated speed V _un , the integrated attitude angle can be obtained simply by attaching the movement information calculation system 104 to the moving body and connecting the positioning signal receiving unit 20 to the antenna. the [Phi _un, can be calculated with high accuracy.

  Such a configuration in which the positioning signal receiving unit 20 and the IMU 30 are added may be applied to other embodiments.

  The movement information calculation device or movement information calculation system configured as described above is mounted on a moving body as described below. FIG. 7 is a schematic configuration diagram of a moving body according to the embodiment of the present invention. In addition, the mobile body of this embodiment is an example provided with the movement information calculation apparatus 101 which concerns on 1st Embodiment, and may be provided with the movement information calculation apparatus or movement information calculation system of other embodiment. At this time, when the movement information calculation device 104 of the fourth embodiment is provided, the positioning signal receiving unit 20 and the IMU 30 may be omitted.

  The moving body 400 includes a movement information calculation device 101, a positioning signal receiving unit 20, an antenna 21, an IMU 30, a control unit 40, and a power generation unit 50.

The antenna 21 is attached to the moving body 400, receives a positioning signal, and outputs it to the positioning signal receiving unit 20. The positioning signal receiving unit 20 calculates the position P _GPS and the velocity V _GPS using the positioning signal and outputs them to the movement information calculation device 101.

The IMU 30 is attached to the moving body 400, detects a change in posture of the moving body 400, measures the acceleration a _IMU and the angular velocity ω _IMU , and outputs them to the movement information calculation apparatus 101.

The movement information calculation apparatus 101 calculates the integrated position P _un , the integrated speed V _un , and the integrated posture angle Φ _un by the above-described method, and outputs the calculated position to the control unit 40.

Based on the integrated position P _un , the integrated speed V _un , and the integrated posture angle Φ _un , the control unit 40 controls the power generation unit 50 so that the moving body 400 has a desired posture and moving state.

  With such a configuration, the moving body 400 can move in a desired posture and moving state (speed, acceleration, turning speed, etc.). Since the movement information calculation device 101 having the above-described configuration is provided, the posture and movement state of the moving body 400 can be calculated with high accuracy, so that the movement of the moving body 400 can be controlled with high accuracy.

101, 102, 103: movement information calculation device 104: movement information calculation system 10A: first integration processing unit, 10B: second integration processing unit, 10C: third integration processing unit 11A: first error estimation unit, 11B: first 2 error estimation unit, 11C: third error estimation unit 12A: first inertial navigation calculation unit, 12B: second inertial navigation calculation unit, 12C: third inertial navigation calculation unit 20: positioning signal reception unit 21: antenna 30: IMU
40: control unit 50: power generation unit 120: integrated movement information determination unit 121A: first IMU error correction unit 121B: second IMU error correction unit 122A: first position speed calculation unit 122B: second position speed calculation unit 123A: First posture angle calculation unit, 123B: second posture angle calculation unit 400: moving object

Claims (13)

A movement information calculation device that calculates integrated movement information of a moving body using positioning movement information obtained from a positioning signal and sensor movement information obtained from an inertial sensor,
A first error estimation unit that estimates a first sensor error that is a sensor error of the inertial sensor using the positioning movement information, the sensor movement information, and the first integrated movement information that has already been calculated;
Second error estimation that estimates a second sensor error that is a different type of sensor error from the first sensor error, using the positioning movement information, the sensor movement information, and the already calculated second integrated movement information. And
An inertial navigation calculation unit that calculates the current first integrated movement information and the second integrated movement information from the corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error and the second sensor error;
A movement information calculation device comprising: The movement information calculation device according to claim 1,
The first error estimating unit estimates a first calculation error that is a calculation error of the first integrated movement information together with the first sensor error;
The second error estimating unit estimates a second calculation error that is a calculation error of the second integrated movement information together with the second sensor error;
The inertial navigation calculation unit is
The first integrated movement information of this time is calculated from the first corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error, the second sensor error, and the first calculation error, and the first A first inertial navigation calculation unit that feeds back integrated movement information to the first error estimation unit;
The second integrated movement information of this time is calculated from the second correction sensor movement information obtained by correcting the sensor movement information using the first sensor error, the second sensor error, and the second calculation error, and the second A second inertial navigation calculation unit that feeds back integrated movement information to the second error estimation unit;
Movement information calculation device. The movement information calculation device according to claim 2,
An integrated movement information determination unit that outputs either one of the first integrated movement information and the second integrated movement information or a representative value based on the first integrated movement information and the second integrated movement information;
A movement information calculation device further provided. The movement information calculation device according to any one of claims 1 to 3,
The first sensor error is a bias error;
The second sensor error is a scale factor error;
Movement information calculation device. The movement information calculation device according to any one of claims 1 to 4,
The first error estimator and the second error estimator are movement information calculation devices comprising Kalman filters having the same observation value but different unknowns. The movement information calculation device according to any one of claims 1 to 5,
A positioning signal receiver for calculating the positioning movement information;
An inertial posture measuring device comprising the inertial sensor for calculating the sensor movement information;
A movement information calculation system comprising: Each part of the movement information calculation device according to claim 1 or the movement information calculation system according to claim 6,
A control unit that performs movement control based on the integrated movement information;
A power generation unit that is driven and controlled based on the movement control;
A moving object comprising: A movement information calculation method for calculating integrated movement information of a moving body using positioning movement information obtained from a positioning signal and sensor movement information obtained from an inertial sensor,
A first error estimation step of estimating a first sensor error that is a sensor error of the inertial sensor using the positioning movement information, the sensor movement information, and the first integrated movement information that has already been calculated;
Second error estimation that estimates a second sensor error that is a different type of sensor error from the first sensor error, using the positioning movement information, the sensor movement information, and the already calculated second integrated movement information. Process,
An inertial navigation calculation step of calculating the current first integrated movement information and the second integrated movement information from the corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error and the second sensor error;
A movement information calculation method. It is the movement information calculation method of Claim 8, Comprising:
The first error estimating step estimates a first calculation error that is a calculation error of the first integrated movement information together with the first sensor error;
The second error estimating step estimates a second calculation error that is a calculation error of the second integrated movement information together with the second sensor error,
The inertial navigation calculation step includes:
The first integrated movement information of this time is calculated from the first corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error, the second sensor error, and the first calculation error, and the first A first inertial navigation calculation step of feeding back integrated movement information to the first error estimator;
The second integrated movement information of this time is calculated from the second correction sensor movement information obtained by correcting the sensor movement information using the first sensor error, the second sensor error, and the second calculation error, and the second A second inertial navigation calculation step of feeding back integrated movement information to the second error estimator,
Movement information calculation method. It is the movement information calculation method of Claim 9, Comprising:
An integrated movement information determination step of outputting a representative value based on either the first integrated movement information and the second integrated movement information or the first integrated movement information and the second integrated movement information;
Furthermore, the movement information calculation method which has. A movement information calculation program for causing a computer to execute processing for calculating integrated movement information of a moving body using positioning movement information obtained from a positioning signal and sensor movement information obtained from an inertial sensor,
The computer
A first error estimation process for estimating a first sensor error that is a sensor error of the inertial sensor using the positioning movement information, the sensor movement information, and the first integrated movement information that has already been calculated;
Second error estimation that estimates a second sensor error that is a different type of sensor error from the first sensor error, using the positioning movement information, the sensor movement information, and the already calculated second integrated movement information. Processing,
An inertial navigation calculation process for calculating the current first integrated movement information and the second integrated movement information from corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error and the second sensor error;
A movement information calculation program for executing The movement information calculation program according to claim 11,
The computer
As the first error estimation process, a first calculation error that is a calculation error of the first integrated movement information is estimated together with the first sensor error,
As the second error estimation process, a second calculation error that is a calculation error of the second integrated movement information is estimated together with the second sensor error,
As the inertial navigation calculation processing,
The first integrated movement information of this time is calculated from the first corrected sensor movement information obtained by correcting the sensor movement information using the first sensor error, the second sensor error, and the first calculation error, and the first A first inertial navigation calculation process for feeding back integrated movement information to the first error estimator;
The second integrated movement information of this time is calculated from the second correction sensor movement information obtained by correcting the sensor movement information using the first sensor error, the second sensor error, and the second calculation error, and the second Performing a second inertial navigation calculation process for feeding back integrated movement information to the second error estimation unit;
Movement information calculation program. It is a movement information calculation program of Claim 12, Comprising:
The computer
Either one of the first integrated movement information and the second integrated movement information, or an integrated movement information determination process for outputting a representative value based on the first integrated movement information and the second integrated movement information to the outside;
A movement information calculation program to be executed.

Images