JP2007163335A - Attitude locating device, attitude locating method, and attitude locating program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To suppress the enlargement of the error of an attitude angle when an observed value of the double phase difference from at least one baseline vector is obtained in locating the attitude angle. <P>SOLUTION: A double phase difference calculation section 120 calculates the observed value of the double phase difference based on carrier wave phases observed by a plurality of GPS (Global Positioning System) receivers 952. A double phase difference estimation section 130 calculates an estimated value of the double phase difference based on the located value of the attitude angle. A double phase difference residual calculation section 140 calculates difference between the observed value of the double phase difference and the estimated value of the double phase difference. A Kalman filter 150 calculates a corrective amount based on the difference between the observed value and the estimated value of the double phase difference. An attitude angle calculation section 110 corrects, with the corrective amount, the calculated value of the attitude angle calculated by integrating the rate measured by a gyroscope 953, and calculates the located value of the attitude angle. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to an attitude determination device, an attitude determination method, and an attitude determination program in an attitude determination process using a GPS (Global Positioning System) and a gyro sensor (hereinafter referred to as a gyro).

GPS is used as the attitude determination technology, and there are an attitude determination using only GPS (non-coupling) and an attitude determination by coupling GPS and a gyro.
In conventional attitude orientation using GPS, it is necessary to observe a double phase difference with respect to the carrier phase at the same time, and in order to synchronize the reception timing between GPS receivers, all GPS receivers can transmit a single signal. A so-called dedicated receiver is used.
Further, in the conventional attitude orientation using only GPS (non-coupling), an integer value bias of the carrier phase is searched for each epoch (observation period) to obtain a double phase difference. Then, two or more baseline vectors are calculated based on the relative positions of the GPS receivers identified by the double phase difference, and the attitude angle is calculated based on the two or more baseline vectors.
Also, in the conventional attitude orientation that couples GPS and gyro, the attitude angle based on the gyro measurement value is corrected based on the difference between the attitude angle based on the gyro measurement value and the attitude angle based on the observation value of the GPS receiver. (Observation update) and the attitude angle is standardized. And in order to calculate the calculated value of the attitude angle based on the observation value of the GPS receiver, as in the conventional attitude orientation using only the GPS (non-coupling), two or more double phase differences are calculated. The baseline vector is calculated.
In the above conventional orientation determination, the azimuth angle and the elevation angle are determined by one baseline vector, and the rotation angle is determined with respect to the plane formed by the two baseline vectors. ], Elevation angle [pitch], azimuth angle [yaw]).
JP-A-11-94573

However, the conventional attitude determination using GPS has a limited use because a dedicated receiver is used, and the hardware configuration is also special, so that the attitude determination apparatus is inevitably expensive.
And in the conventional attitude | position orientation which uses only GPS (non-coupling), when the observation value of a double phase difference is not obtained with respect to two or more baseline vectors, an attitude angle cannot be obtained. That is, when the carrier phase is observed for three or more GPS receivers and the observation value of the double phase difference is not obtained, two or more baseline vectors cannot be calculated and the attitude angle cannot be obtained.
Furthermore, since it is necessary to search for an integer value bias of the carrier phase for each epoch, the calculation load is high, and inappropriate observation values cannot be rejected using the attitude angles calculated up to the previous epoch.
In addition, in the conventional attitude orientation that couples GPS and gyro, the attitude angle is used for coupling, so the observation update can be performed if no double phase difference is observed for two or more baseline vectors. The accuracy of the orientation value of the posture angle is reduced.
That is, since it is necessary to perform GPS observation simultaneously by three or more GPS receivers, there is a high possibility that the availability of GPS observation will be reduced. Therefore, in order not to deteriorate the accuracy of the orientation value of the posture angle, a highly stable gyroscope is required, and the posture orientation device becomes relatively expensive.
Also, there is a singular point (pitch is 90 °) in the Euler angle (attitude angle in the local coordinate system [for example, the coordinate system to which the measured value of the gyro belongs)], and the singular point is calculated by distinguishing between roll and yaw. Can not do it. For this reason, even when three or more GPS receivers can perform GPS observation at the same time, the observation update cannot be normally performed at the singular point, and the accuracy of the orientation value of the posture angle is lowered.

  An object of the present invention is to make it possible to suppress an increase in an error in posture angle when, for example, an observation value of a double phase difference with respect to at least one baseline vector is obtained. For example, it aims at improving the availability of GPS observation. Another object is to enable a configuration using a low-cost gyro with low stability.

  The posture orientation apparatus of the present invention corrects the calculated value of the posture angle calculated using the measured value of the gyro based on the observation value of a GPS (Global Positioning System) receiver, and calculates the orientation value of the posture angle. An attitude angle calculation unit that calculates a calculated value of the attitude angle using a central processing unit based on the measured value of the gyro, and a carrier phase that is observed by a plurality of GPS receivers, and the central processing unit is used. A double phase difference calculation unit for calculating an observation value of a double phase difference in a plurality of GPS receivers, and a double phase difference in a plurality of GPS receivers using a central processing unit based on the calculated orientation value of the attitude angle. A double phase difference estimator for calculating an estimated value; a double phase difference observed value in a plurality of GPS receivers calculated by the double phase difference calculator; and a double phase difference estimator calculated by the double phase difference estimator. A plurality of GPS units calculated by the dual phase difference residual calculation unit, and a double phase difference residual calculation unit that calculates a difference from the estimated value of the double phase difference using a central processing unit. A correction amount calculation unit for calculating a correction amount for the calculated value of the attitude angle based on the difference between the observed value and the estimated value of the double phase difference in the receiver using a central processing unit; and A posture angle locator that corrects a calculated value of the posture angle calculated by the posture angle calculator based on the correction amount calculated by the correction amount calculator using a central processing unit and calculates a posture value of the posture angle. It is characterized by that.

  According to the present invention, the GPS and the gyro are coupled, that is, the observation is updated by using the double phase difference instead of the Euler attitude angle, for example, the double phase difference with respect to at least one baseline vector is obtained. When the observed value is obtained, it is possible to suppress an increase in the error of the attitude angle. For example, the availability of GPS observation can be improved. Further, for example, a configuration using a low-cost gyro with low stability can be made possible.

A posture locating device, a posture locating method, and a posture locating program for coupling a GPS and a gyro using a double phase difference will be described below.
In particular, based on the difference between the observed double phase difference value and the estimated double phase difference value based on the gyro measurement value in multiple GPS receivers, the calculated attitude angle based on the gyro measurement value is corrected. A posture locating device, a posture locating method, and a posture locating program for locating posture angles will be described.

Embodiment 1 FIG.
FIG. 1 is a schematic diagram of a configuration of posture orientation system 100 in the first embodiment.
A GPS antenna 951, a GPS receiver 952, and a gyro 953 are arranged on one platform 954 so as to move together. At this time, the relative positional relationship between the plurality of GPS antennas 951 is constrained and does not change.
The posture locating device 101 includes posture angles (roll [rotation angle], pitch [elevation angle], yaw [azimuth angle]] of a posture determination target (particularly, a moving object) in which a GPS antenna 951, a GPS receiver 952, and a gyro 953 are arranged. ).
Here, the platform 954 is not necessarily provided as long as the relative positional relationship between the plurality of GPS antennas 951 can be maintained.
Also, in order to position the coordinate axis of the coordinate system formed by the base line vector between the GPS antennas 951 and the coordinate axis of the coordinate system to which the measured value of the gyro 953 belongs in parallel, is the installation posture alignment of the gyro 953 physically executed? A process for calculating and correcting the deviation of the coordinate axes by software is implemented. Hereinafter, a mode will be described in which the posture locating apparatus 101 implements a process of calculating and correcting the deviation of the coordinate axis of the gyro 953 by software. Here, the baseline vector is a vector indicating the distance and direction between two points, for example, the vector from the master station to the slave station (1) or the vector from the master station to the slave station (2) in FIG. is there.
In FIG. 1, the posture locating system 100 includes three GPS antennas 951 and GPS receivers 952 on the platform 954, but the number may be two or four or more. If at least two of the three GPS receivers 952 can receive a carrier wave as a positioning signal from a GPS satellite via the GPS antenna 951 and calculate an observed value (GPS observation), one baseline vector is determined, and the attitude locator 101 can observe and update the attitude angle to determine the orientation.
The gyro 953 is a three-axis gyro that measures angular velocities in the directions of three orthogonal axes.
The posture locating device 101 integrates the angular velocity output from the gyro 953 and calculates the posture angle. Hereinafter, the posture locating device 101 that calculates and levels a correction amount in software for the angular velocity output from the gyro 953 and uses the leveled angular velocity for calculating the posture angle will be described.

FIG. 2 is a hardware configuration diagram of the posture orientation system 100 according to the first embodiment.
The posture orientation system 100 includes a plurality of GPS antennas 951, a GPS receiver 952 connected to each GPS antenna 951, a gyro 953 that measures angular velocities in three axial directions (roll, pitch, yaw), and a posture orientation device 101. Prepare.
Each GPS receiver 952 is based on a carrier wave of a positioning signal (L1, L2, L5, etc.) received from a GPS satellite by a GPS antenna 951 to which each GPS receiver 951 is connected. Is output.
The gyro 953 measures and outputs an angular velocity (hereinafter referred to as a rate) in three axial directions (roll, pitch, yaw).

The posture orientation apparatus 101 includes a CPU (Central Processing Unit) 911 that executes a program. The CPU 911 is connected to a storage device 920 via a bus, and the storage device 920 includes storage devices such as a RAM, a ROM, and a magnetic disk device.
The storage device 920 stores an operating system (OS) 921, a program group 923, and a file group 924. The program group 923 is executed by the CPU 911 and the OS 921.
The program group 923 stores programs for executing functions described as “˜unit”, “Kalman filter”, and “antenna separation” in the description of the embodiment. The program is read and executed by the CPU 911.
In the file group 924, in the description of the embodiment, the data described as “˜information”, the data indicating the determination result / calculation result when the function described as “˜part” is executed, and described as “˜part”. Data to be passed between programs that execute the function to be performed is stored as “˜file”. For example, information such as the orientation angle determined, the rate output by the gyro 953, and the carrier wave phase output by the GPS receiver 952 are stored as “˜file”.
Further, in the description of the embodiments, the arrows in the flowcharts and the configuration diagrams mainly indicate data input / output, and the data for the data input / output is stored in the storage device 920 or a signal line or other transmission medium. It is transmitted by.
Moreover, what is described as “˜unit” in the description of the embodiments may be realized by firmware stored in the ROM. Alternatively, it may be implemented by software alone, hardware alone, a combination of software and hardware, or a combination of firmware.

FIG. 3 is a functional configuration diagram of posture orientation apparatus 101 in the first embodiment.
A functional configuration of the posture locating device 101 included in the posture locating device 101 according to the first embodiment will be described below with reference to FIG.
The initial value acquisition unit 210 acquires an initial value of the posture angle.
The antenna separation 230 calculates the geometrical arrangement of each GPS antenna 951 (baseline vector b _{BODY in the} body coordinate system).
The direction cosine matrix calculation unit 220 calculates a direction cosine matrix C _B ^N that converts the base line vector b _BODY of the airframe coordinate system into the base line vector b _NED of the orientation coordinate system based on the attitude angle. Hereinafter, the local coordinate system (the coordinate system of the posture angle determined by the posture locating apparatus 101) will be described as an NED (North East Down) coordinate system.
The design matrix calculation unit 240 receives satellite orbit information output from the GPS receiver 952 and receives a design matrix A based on a direction cosine basis vector e (hereinafter referred to as LOS [Line Of Light] vector e) determined by the satellite orbit information. Calculate The direction cosine basis vector e indicates a vector serving as a basis from the GPS receiver 952 to the GPS satellite.
The double phase difference estimation unit 130 calculates a double phase difference estimation value Ab _λ based on the base line vector b _NED of the NED coordinate system and the design matrix A.
The double phase difference calculation unit 120 calculates the observation value W _λ of the double phase difference based on each carrier phase φ output from each GPS receiver 952.
The double phase difference residual calculation unit 140 calculates a difference dZ _λ between the double phase difference estimated value Ab _λ and the double phase difference observed value W _λ .
The Kalman filter 150 (an example of a correction amount calculation unit) is a state equation modeling the dynamics of state quantities (for example, estimated values of attitude angles and gyro sensor measurement errors), observation quantities (for example, observation values of double phase differences). Is a feedback filter that improves the estimation accuracy of the state quantity based on both equations of the observation equation that formulates the relationship between the state quantity and the state quantity, and corrects the rate error of the gyro 953 based on the difference dZ _λ A correction amount such as a correction amount of an axis deviation between the gyro 953 and the GPS antenna 951 and a correction amount of an attitude angle error are calculated.
The rate correction unit 250 (an example of a posture angle locating unit) corrects the rate output by the gyro 953 based on the rate error correction amount.
The posture angle calculation unit 110 (an example of the posture angle locating unit) integrates the orientation value of the previous posture angle or the initial value of the posture angle at a rate output from the gyro 953 and corrected by the rate correction unit 250, and an axis deviation correction amount. Then, the orientation angle orientation correction value is corrected to calculate the orientation angle orientation value.

FIG. 4 is a flowchart showing the posture orientation processing of the posture orientation system 100 according to the first embodiment.
A flow of posture orientation processing (posture orientation method) executed by the posture orientation system 100 according to the first embodiment will be described below with reference to FIG.
Note that the orientation determination process can be executed by a computer. A program that causes a computer to execute posture orientation processing is a posture orientation program.

Attitude locating system 101 calculates the observed value W _lambda double phase difference based on carrier phase φ of the GPS receiver 952 has output: with (S101 and S102 double phase difference observation value calculation process), the gyro 953 Based on the measured rate, an estimated value Ab _λ of the double phase difference is calculated (S103 to S108: double phase difference estimated value calculation processing), and the observed value W _λ of the double phase difference and the estimated value of the double phase difference are calculated. The posture angle is corrected by the correction amount based on the difference dZ _{λ from} Ab _λ and the orientation angle orientation value is calculated (S109 to S111: orientation angle orientation value calculation processing).

Here, the relationship between the observed value and the estimated value of the double phase difference in the attitude determination process will be described.
First, the double phase difference will be described.
The difference between the observation values of two GPS receivers for the same GPS satellite is called a single difference between the receivers, and the difference between the GPS receiver observation values for two GPS satellites is called a single difference between the satellites. The difference between the single differences between the two receivers or the difference between the single differences between the two satellites is called a double difference, and the double phase difference is a double difference in phase.

FIG. 5 is a relationship diagram between the base line vector b, the LOS vector e, and the distance ρ from the GPS antenna to the GPS satellite.
In FIG. 5, “a” and “b” indicate GPS antennas, and “i” and “j” indicate GPS satellites. “B _ba ” indicates a baseline vector from the GPS antenna a to the GPS antenna b. “E _a ⁱ ” indicates the LOS vector from the GPS antenna a to the GPS satellite i, and “e _a ^j ” indicates the LOS vector from the GPS antenna a to the GPS satellite j. “Ρ _a ⁱ ” indicates the distance from the GPS antenna a to the GPS satellite i, “ρ _b ⁱ ” indicates the distance from the GPS antenna b to the GPS satellite i, and “ρ _a ^j ” indicates the distance from the GPS antenna a. The distance to the GPS satellite j is indicated, and “ρ _b ^j ” indicates the distance from the GPS antenna b to the GPS satellite j.

In FIG. 5, the double difference “ρ _ab ^ji ” of the distance is expressed by the following [Equation 1] using the distance ρ from the GPS antenna to the GPS satellite.

ρ _ab ^ji = (ρ _b ^j −ρ _a ^j ) − (ρ _b ⁱ −ρ _a ⁱ ) [Formula 1]

  The distance ρ from the GPS antenna to the GPS satellite is represented by a value obtained by multiplying the number of carriers between the GPS antenna and the GPS satellite by the wavelength λ of the carrier, and the number of carriers is Doppler (carrier wave) that can be observed by the GPS receiver. It is represented by the total value of the carrier phase φ, which is an integrated value of the time change rate of the phase), and an integer value N of the number of carriers called an integer value bias. That is, the distance ρ from the GPS antenna to the GPS satellite is expressed by the following [Equation 2].

  ρ = λ (φ + N) [Formula 2]

  Here, when [Expression 1] is expressed using [Expression 2], the following [Expression 3] is obtained.

ρ _ab ^ji = λ (φ _ab ^ji −N _ab ^ji ) [Formula 3]

In [Equation 3], “λ (φ _ab ^ji −N _ab ^ji )” corresponds to the observation value of the double difference in distance, and “φ _ab ^ji −N _ab ^ji ” corresponds to the observation value of the double phase difference. Correspond.

In FIG. 5, the double difference “ρ _ab ^ji ” of the distance is expressed by the following [Equation 4] using the base line vector b and the LOS vector e.

_{^{ρ ab ji = - (e a}} j -e a i) × b ba [ Formula 4]

In [Expression 4], the base line vector _bba is a calculated value calculated by converting the body coordinate system to the NED coordinate system according to the attitude angle, and “− (e _a ^j −e _a ⁱ ” in [Expression 4]. ) × _{b ba} "corresponds to the estimated value of the double difference of the distance," ^{_{(- (e a j -e a}} i) × b ba) / λ "corresponds to the estimated value of the double phase difference.

  Further, since the base line vector b is specified based on the positional relationship of the four GPS satellites, the GPS satellites are changed from “i” and “j” to “i”, “j”, “k”, “l”. The following [Relational expression 5] and [Relational expression 6] are established using the above [Expression 3] and [Expression 4].

  W = Ab [Relational formula 6]

Then, the posture locating apparatus 101 uses “W / λ corresponding to the observed value of the double phase difference (hereinafter referred to as the observed value W _λ of the double phase difference) in the above [Relational Expression 5] and [Relational Expression 6]. (S101 to S102: double phase difference observation value calculation processing) and “Ab / λ (hereinafter referred to as double phase difference estimation value Ab _λ )” corresponding to the double phase difference estimation value (S103 to S108: double phase difference estimation value calculation processing), and the attitude angle is corrected by the correction amount based on the difference dZ _λ between W _λ and Ab _λ to calculate the orientation value of the attitude angle (S109 to S111). : Posture angle orientation value calculation processing).

Details of each process will be described below.
It will be described double phase difference of the observed value W _lambda double phase difference observed value calculation process for calculating a (S101 and S102).
<Double phase difference observation value calculation processing>
First, each GPS receiver 952 receives a carrier wave from four GPS satellites via the GPS antenna 951 to which each GPS receiver is connected, and observes and outputs the carrier phase φ and satellite orbit information for each GPS satellite (S101). : Carrier phase observation processing).
Then, the double phase difference calculation unit 120 calculates the observation value W _λ of the double phase difference based on the carrier phase φ output from the GPS receiver 952. At this time, the double phase difference calculation unit 120 calculates W _λ by dividing W by the wavelength λ of the carrier wave by representing W in the above [Relational expression 5] by “W = λφ” without the integer value bias N (S102: Double phase difference calculation process)

Next, the double phase difference estimated value calculation process (S103 to S108) for calculating the double phase difference estimated value Ab _λ will be described.
<Double phase difference estimated value calculation processing>
The design matrix calculation unit 240 calculates each LOS vector e based on the positional relationship of the four GPS satellites specified by the satellite orbit information output from each GPS receiver 952, and the design shown in [Relational Expression 5] above. The matrix A is calculated (S103: basis vector matrix calculation process).
The initial value acquisition unit 210 acquires the initial value of the posture angle by an arbitrary method. The initial value acquisition unit 210 calculates, for example, an integer value bias N by the Hatch method, calculates two baseline vectors b that satisfy the above [Relational Expression 5], and based on the two baseline vectors b, the initial value of the posture angle Is calculated. In addition, a roll (rotation angle) and pitch (elevation angle) calculated based on a relational expression between the rate measured by the gyro 953 and the direction of gravity, and a yaw (azimuth angle) measured by the compass with a compass You may use for the initial value of. In addition, an input device such as a keyboard may be provided to allow the user to specify an initial value of the posture angle (S104: posture angle initial value acquisition process).
Next, the posture angle calculation unit 110 integrates the initial value of the posture angle acquired by the initial value acquisition unit 210 at the rate output by the gyro 953, and calculates the posture angle at that time (S105: posture angle calculation processing). .
Then, the direction cosine matrix calculating unit 220, based on the attitude angle posture angle calculating unit 110 calculates the direction cosine matrix _{C B} for converting the baseline vector _{b BODY} aircraft coordinate system to the baseline vector _{b NED} of NED coordinate system ^N is calculated (S106: coordinate transformation matrix calculation process).
Next, double phase difference estimation unit 130, a baseline vector _{b NED} of converting the baseline vector _{b BODY} local coordinate system using a direction cosine matrix _C ^{B N} of the direction cosine matrix calculating unit 220 has calculated NED coordinate system calculate. At this time, the antenna separation 230 calculates the baseline vector b _BODY of the local coordinate system based on the geometric arrangement of each GPS antenna 951 and stores it in a storage device (for example, the storage device 920) in advance. (S107: Baseline vector coordinate system conversion process).
Next, the double phase difference estimating unit 130 multiplies the value obtained by multiplying the design matrix A calculated by the design matrix calculating unit 240 and the base line vector b _NED of the NED coordinate system calculated by the double phase difference estimating unit 130 of the carrier wave. The double phase difference estimated value Ab _λ is calculated by dividing by the wavelength λ (S108: double phase difference estimation process).

Next, the attitude angle standard value calculation is performed in which the attitude angle is corrected by the correction amount based on the difference dZ _λ between the observed double phase difference value W _λ and the estimated double phase difference value Ab _λ to calculate the attitude angle orientation value. Processing (S109 to S111) will be described.

Attitude angle orientation value calculation processing (S109 to S111) is performed by the dual phase difference (observation value of the double phase difference) of the carrier phase observation values by the plurality of GPS receivers 952 mounted on the orientation angle orientation target and the gyro 953. Based on the residual (difference) with the geometric double phase difference predicted value (estimated double phase difference) obtained by integrating the rate output, the rate and attitude angle calculated values output by the gyro 953 and This is a process of calculating a correction amount such as an alignment error between the GPS antenna array and the gyro 953.
In the conventional attitude locating method in which observation updating is performed using the attitude angle, it is necessary to be able to simultaneously observe a double phase difference with respect to two or more baseline vectors between the GPS antennas 951 constituting the array. On the other hand, in the attitude determination method using the double phase difference described in the first embodiment, the double phase difference is observed with respect to at least one base line between a plurality of GPS antennas 951 constituting the array. It only has to be done. Therefore, the observation angle can be frequently updated, and the attitude angle can be accurately determined using the low-cost gyro 953 with low stability. If the observation period of the GPS receiver 952 is sufficiently short with respect to the time for the gyro error to expand, the observation residual of the double phase difference is within ± 0.5λ (wavelength), and the observation residual of the double phase difference is Since it appears in the decimal value, the observation update can be performed without obtaining the integer value bias as described below. Furthermore, even when processing two or more baseline vectors, there is no need to take timing between the baseline vectors. That is, it is not necessary to make the calculation period of the double phase difference for the two baseline vectors the same, and the degree of freedom of selection of the GPS receiver 952 increases.

<Attitude angle standard value calculation processing>
First, the double phase difference residual calculation unit 140 includes the double phase difference observation value W _λ calculated by the double phase difference calculation unit 120 and the double phase difference estimation value calculated by the double phase difference estimation unit 130. The difference dZ _λ from Ab _λ is calculated. At this time, since the integer value of the difference dZ _λ does not exceed the wavelength λ if the observation period of the GPS receiver is sufficiently short with respect to the time during which the gyro error is expanded, the observation residual of the double phase difference is the double phase difference. residual calculation section 140 the fractional value of the difference dZ _lambda and the difference dZ _lambda double phase difference (S109: double phase difference residual calculation process).
Next, the Kalman filter 150 uses the difference dZ _λ calculated by the double phase difference residual calculation unit 140 to determine the rate error correction amount of the gyro 953 and the axis of the gyro 953 and the GPS antenna 951 based on the state equation and the observation equation. Correction amounts such as a deviation correction amount and a posture angle error correction amount are calculated (S110: correction amount calculation processing).
Then, the attitude angle calculation unit 110 integrates the orientation value of the previous attitude angle (initial value of the attitude angle acquired by the initial value acquisition unit 210 at the first time) at the rate output by the gyro 953, and calculates the integration. The posture angle at the time is corrected based on the posture angle error correction amount, and the orientation value of the posture angle is calculated. At this time, the rate correction unit 250 corrects the rate output by the gyro 953 with the rate error correction amount. Further, the attitude angle calculation unit 110 integrates the corrected rate and corrects the integral value (variation angle in each of the three axis directions) with the axis deviation correction amount. Then, the posture angle calculation unit 110 adds the corrected integrated value to the orientation value of the previous posture angle to calculate the posture angle at the time point, and corrects the posture angle at the time point based on the posture angle error correction amount. The orientation angle orientation value is calculated (S111: orientation angle orientation processing).

  The posture orientation system 100 after the orientation angle is standardized in the orientation angle location processing (S111), repeats the processing from the coordinate transformation matrix calculation processing (S106) using the orientation value of the orientation angle, and determines the orientation angle at the time point. .

In the double phase difference calculation process (S102), the double phase difference calculation unit 120 synchronizes the timing at which the carrier wave is received with a plurality of GPS receivers 952 by software, and the double phase difference observation value W _λ is calculated.

  At this time, the timing at which the plurality of GPS receivers 952 receive the carrier wave of the positioning signal from the GPS satellite is accurately obtained, and the double phase difference calculation unit 120 is, for example, the observation value of the carrier phase of the GPS receiver 952 of the main station. Is interpolated in accordance with the reception time of the slave GPS receiver 952 to synchronize the timing. Thereby, basically, any general-purpose GPS receiver 952 can be used for the attitude determination system 100, and the low-cost attitude determination system 100 can be configured. Further, a plurality of GPS receivers 952 having different functions and performance (reception frequency, output period of observation values, etc.) can be combined and used in the attitude determination system 100.

Further, there are two methods for calculating the observed value W _λ of the double phase difference: a method using a carrier wave of one frequency and a method using a carrier wave of a plurality of frequencies (for example, two frequencies).

Two-frequency receivers are currently expensive, but prices are expected to drop after new frequencies such as L5 are released. Therefore, assuming the attitude localization system 100 using a two-frequency receiver, the observation value of the double phase difference is calculated using the carrier phase of the wide lane obtained by linearly combining the observation values of the carrier frequency of the two frequencies. Can do. The wavelength of the carrier wave in the wide lane is longer than the wavelength of a single frequency (single frequency). For this reason, a residual that can be drawn by using the carrier phase of the wide lane (a double of the observed value and the estimated value). The range of the phase difference) is widened. For example, the wavelength of one frequency of L1 is about 19 cm (centimeter), whereas the wavelength in a wide lane in which the two frequencies of L1 and L5 are linearly combined is about 75 cm. Assuming that the difference in the double difference between the observed value and the estimated value is within ± 0.5λ (wavelength), the difference in the double difference in distance exceeds ± 9.5 cm at one frequency (L1). The difference in double phase difference does not fall within the decimal value, whereas the difference in double phase difference does not exceed ± 37.5 cm at two frequencies (L1 and L5). Fits in. Further, in the double phase difference residual calculation processing (S109), since double phase difference residual calculation section 140 for processing the fractional value of the difference dZ _lambda as the difference double phase difference, the difference of the double phase difference If the value does not fall within the decimal value, the accuracy of the orientation angle to be reduced will decrease. That is, as the range of residuals that can be pulled in increases, the consistency of the double phase difference residual calculation process (S109) increases, and a posture angle with high accuracy can be determined.

  Hereinafter, the double phase difference calculation unit 120 and the double phase difference calculation process (S102) will be described for each of the method using one frequency and the method using a plurality of frequencies.

FIG. 6 is a configuration diagram of the double phase difference calculation unit 120 using one frequency in the first embodiment.
The double phase difference calculation unit 120 using one frequency includes the following functional elements.
The reception timing calculation unit 121 calculates a reception time t _rv when each GPS receiver 952 receives a carrier wave.
The carrier phase interpolation calculation unit 122 performs interpolation calculation to calculate the carrier phase φ _X at the same reception time for a plurality of GPS receivers 952.
The double phase difference vector calculation unit 123 calculates the double phase difference observation value W _λ for the carrier phase φ at the same reception time of the plurality of GPS receivers 952.

FIG. 7 is a flowchart showing a double phase difference calculation process (S102) using one frequency in the first embodiment.
The double phase difference calculation process (S102) using one frequency will be described below with reference to FIG.
At this time, in the carrier phase observation process (S101), each GPS receiver 952 receives a carrier wave of one frequency (for example, L1) and outputs a carrier phase φ _X (φ _a , φ _b , φ _c ). And

<Double phase difference calculation process (S102)>
First, the reception timing calculation unit 121 acquires the observation time t _{rv of the} carrier wave from each GPS receiver 952, and calculates the clock bias t _rvBias of each GPS receiver 952 by the least square method or the like. Then, by subtracting the clock bias _{t RvBias} from observation time _{t rvo} carrier calculates the reception time _{t rv} carrier (S201: reception timing calculation process).
Then, the carrier phase interpolation calculation unit 122, reception time _t RVX carrier in the GPS receiver 952 carrier phase phi _X and reception timing calculation unit 121 by each GPS receiver 952 is calculated is calculated _{(t rva, t RVB} , T _rvc ) and interpolation calculation is performed to calculate the carrier phase φ _X (t) at the same reception time. The interpolation calculation may be any interpolation calculation such as linear interpolation, primary interpolation, and secondary interpolation. For example, in the case of quadratic interpolation calculation, the carrier phase interpolation calculation unit 122 uses the reception time k shown in FIG. 8 as the reception time k for the carrier phase φ _a , φ _b , φ _c output from each GPS receiver 952. The synchronized carrier phase φ _a (k), φ _b (k), φ _c (k) is calculated by calculating the following [Equation 7] (S202: carrier phase interpolation calculation process).

Then, the double phase difference vector calculation unit 123 calculates a double phase difference observation value W _λ (vector) based on the carrier phase φ _X (t) calculated by the carrier phase interpolation calculation unit 122. At this time, the double phase difference vector calculation unit 123 calculates W _λ by dividing W in the above [Relational Expression 5] by “W = λφ”, omitting the integer bias N, and dividing it by the wavelength λ of the carrier wave. For example, for each of the GPS satellite i and the GPS satellite j, the observed value φ _ab ^ji (double phase difference between the carrier phase φ _a ⁱ , φ _a ^j of the master station and the carrier phase φ _b ⁱ , φ _b ^j of the slave station 1 When calculating (scalar), the double phase difference vector calculation unit 123 calculates the following [Equation 8]. Then, the observed value W _λ (vector) of the double phase difference for the four GPS satellites is calculated (S203: Double phase difference observed value calculation process).

φ _ab ^ji = φ _b ^j −φ _a ^j − (φ _b ⁱ −φ _a ⁱ ) [Equation 8]

FIG. 9 is a configuration diagram of the double phase difference calculation unit 120 using the two frequencies in the first embodiment.
The double phase difference calculation unit 120 using two frequencies includes a linear combination calculation unit 124 in addition to the functional elements of the double phase difference calculation unit 120 using one frequency described with reference to FIG.
The linear combination calculator 124 calculates the carrier phase φ _X after the linear combination by linearly calculating the carrier phases φ _XY of a plurality of frequencies output from each GPS receiver 952.

FIG. 10 is a flowchart showing a double phase difference calculation process (S102) using a plurality of frequencies in the first embodiment.
The double phase difference calculation process (S102) using a plurality of frequencies will be described below with reference to FIG.
At this time, in the carrier wave phase observation process (S101), each GPS receiver 952 receives a carrier wave of two frequencies (for example, L1 and L5) and receives carrier wave phases φ _XY (φ _a1 , φ _a5 , φ _b1 , φ _b5 , Φ _c1 , φ _c5 ) (φ _a1 , φ _b1 , φ _c1 : an example of the first carrier phase) (φ _a5 , φ _b5 , φ _c5 : an example of the second carrier phase).

<Double phase difference calculation process (S102)>
First, the reception timing calculation unit 121 acquires the observation time t _rvo of each frequency carrier from each GPS receiver 952, and calculates the clock bias t _rvBias of each GPS receiver 952 by the least square method. Then, by subtracting the clock bias _{t RvBias} from observation time _{t rvo} carrier calculates the reception time _{t rv} carrier (S201: reception timing calculation process).
Next, the linear combination calculation unit 124 calculates the carrier phase φ _X after linear combination by linearly calculating the carrier phase φ _XY of each frequency output from each GPS receiver 952 (S302: linear combination calculation process).
Hereinafter, the double phase difference calculation unit 120 executes S303 and S304 in the same manner as S202 and S203, respectively.

Although FIG. 6 and FIG. 9 show three GPS receivers 952, three or more GPS receivers 952 may be used, and the number may be increased.
In addition, although a plurality of reception timing calculation units 121, carrier phase interpolation calculation units 122, double phase difference vector calculation units 123, and linear combination calculation units 124 are shown corresponding to the GPS receiver 952, one may be provided. .
Further, it is not necessary to calculate the observation value of the double phase difference in synchronization between the base line vectors. For example, one of the carrier phase interpolation calculation units 122 shown in FIG. 6 and FIG. You don't have to.

The posture locating apparatus 101 described in the first embodiment updates (integrates) the initial value of the posture angle calculated by obtaining two baseline vectors between the GPS antennas 951 with the rate output of the gyro 953, and as time passes. It is a device that outputs attitude angles with high accuracy by updating observations using a double phase difference based on GPS observation.
In particular, the double phase difference calculation unit 120, the double phase difference residual calculation unit 140, and the Kalman filter 150, which are indicated by (1) and (2) in FIG.

The posture locating device 101 described in the first embodiment has the following merits by coupling the GPS and the gyro.
First, when the GPS satellite is visible, the gyro error can be corrected with high precision by updating the observation, and the attitude angle can be highly accurate.
Further, even when the GPS satellite is invisible, it is possible to output the attitude angle by integrating at a rate output by the gyro.
In addition, since the gyro output period is generally faster than the GPS receiver observation period, the attitude angle can be output at a high frequency.

The attitude locating device 101 described in the first embodiment performs interpolation calculation of carrier phase observation values using reception times obtained with high accuracy by single positioning or the like, and synchronizes between GPS receivers in software. . Thereby, the posture locating device described in the first embodiment has the following merits.
Basically, any general-purpose receiver (GPS receiver) can be used, and the low-cost attitude orientation system 100 can be configured.
It is also possible to combine a plurality of GPS receivers having different functions and performance (reception frequency, observation value output cycle, etc.). Therefore, for example, the attitude determination system 100 can determine the attitude angle by using a GPS receiver that is optimal for the configuration of the positioning system as well as the positioning system.

The posture locating apparatus 101 described in the first embodiment has the following advantages by performing observation and updating using a double phase difference instead of the Euler posture angle used in the conventional posture locating method.
First, the determination of the conventional attitude angle using only GPS requires obtaining observation values of the double difference phase difference for two or more baseline vectors at the same time, whereas at least one baseline vector. If the observation value of the double difference phase difference with respect to is obtained, the observation can be updated and the expansion of the error can be suppressed.
That is, since the availability of observation update based on the observation value of the GPS receiver is improved, a configuration using a low-cost gyro with low stability is possible.
In addition, due to the nature of Euler angles, there are singular points (pitch 90 °), and the conventional attitude determination method that uses Euler angles for observation updates cannot perform appropriate observation updates at singular points. In the posture locating apparatus 101 of the first embodiment that uses a double phase difference for updating, there is no case where appropriate observation updating cannot be performed at a singular point.
In addition, after determining the integer value bias of the double phase difference and determining the absolute amount of posture angle (initial value of posture angle), the posture angle can be updated by integrating the gyro rate output. Is possible.
Also, if the GPS receiver's observation period is sufficiently short with respect to the time when the gyro error is expanded, the observation residual of the double phase difference will be within ± 0.5λ (wavelength), so without finding the integer bias Observation updates can be performed.
In other words, when the observation residual of the double phase difference exceeds a specific threshold (for example, ± 0.2λ), the GPS receiver has output an inappropriate observation value for some reason, or the gyro is inappropriate It can be determined that the rate has been output, and these data can be appropriately rejected.
Also, if you use a current expensive dual-frequency receiver or a new frequency-compatible receiver such as L5, which is expected to be low in the future, the residual of observation using a wide lane by linear combination of two frequencies Can be calculated. Thereby, the range of the region (for example, ± 0.5λ) in which the residual of the double phase difference can be drawn can be expanded.
In addition, compared to the case where the observation value used when coupling the GPS and the gyroscope is the Euler attitude angle, the noise model of the double phase difference (state equation and observation equation used in the Kalman filter) is easier to grasp, The estimation accuracy is improved.

  The GPS in the description of the first embodiment may be another GNSS (Global Navigation Satellite System) such as Galileo or GLONASS (Global Navigation Satellite System).

1 is a schematic diagram of a configuration of a posture orientation system 100 according to Embodiment 1. FIG. FIG. 3 is a hardware configuration diagram of posture orientation system 100 in the first embodiment. FIG. 3 is a functional configuration diagram of posture orientation apparatus 101 in the first embodiment. 3 is a flowchart showing posture orientation processing of the posture orientation system 100 according to the first embodiment. FIG. 6 is a relationship diagram between a base line vector b, a LOS vector e, and a distance ρ from a GPS antenna to a GPS satellite. FIG. 3 is a configuration diagram of a double phase difference calculation unit 120 that uses one frequency in the first embodiment. 5 is a flowchart showing double phase difference calculation processing (S102) using one frequency in the first embodiment. The relationship figure of the carrier wave reception time in a master station and a slave station. FIG. 3 is a configuration diagram of a double phase difference calculation unit 120 that uses two frequencies in the first embodiment. 5 is a flowchart showing a double phase difference calculation process (S102) using a plurality of frequencies in the first embodiment.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 100 Attitude orientation system, 101 Attitude orientation apparatus, 110 Attitude angle calculation part, 120 Double phase difference calculation part, 121 Reception timing calculation part, 122 Carrier phase interpolation calculation part, 123 Double phase difference vector calculation part, 124 Linear combination calculation Unit, 130 double phase difference estimation unit, 140 double phase difference residual calculation unit, 150 Kalman filter, 210 initial value acquisition unit, 220 direction cosine matrix calculation unit, 230 antenna separation, 240 design matrix calculation unit, 250 rate correction unit 911 CPU, 920 storage device, 921 OS, 923 program group, 924 file group, 951 GPS antenna, 952 GPS receiver, 953 gyro, 954 platform.

Claims (7)

An attitude determination device that calculates an attitude angle orientation value by correcting an attitude angle calculation value calculated using a gyro measurement value based on an observation value of a GPS (Global Positioning System) receiver;
A posture angle calculation unit that calculates a calculated value of the posture angle using a central processing unit based on the measured value of the gyro;
A double phase difference calculation unit that inputs the carrier phase observed by a plurality of GPS receivers and calculates an observation value of the double phase difference in the plurality of GPS receivers using a central processing unit;
A double phase difference estimator that calculates a double phase difference estimate in a plurality of GPS receivers using a central processing unit based on the calculated orientation angle orientation value;
The difference between the observed value of the double phase difference in the plurality of GPS receivers calculated by the dual phase difference calculating unit and the estimated value of the double phase difference in the plurality of GPS receivers calculated by the double phase difference estimating unit A double phase difference residual calculation unit for calculating the difference using a central processing unit;
A correction amount for the calculated attitude angle calculated by the attitude angle calculator based on the difference between the observed value and the estimated value of the dual phase difference in the plurality of GPS receivers calculated by the dual phase difference residual calculator. A correction amount calculation unit that calculates using a central processing unit;
A posture angle locator that calculates a posture angle orientation value by correcting a calculated value of the posture angle calculated by the posture angle calculator based on the correction amount calculated by the correction amount calculator using a central processing unit. A posture locating device characterized by that. The double phase difference calculator is
Interpolation calculation is performed based on the carrier phase observed by each GPS receiver and the reception time of the carrier at each GPS receiver to calculate the carrier phase of each GPS receiver at the same time, and the calculated carrier phase of each GPS receiver 2. The posture locating apparatus according to claim 1, wherein an observed value of a double phase difference in a plurality of GPS receivers is calculated based on The double phase difference calculator is
The first carrier wave phase and the second carrier wave phase observed by each of the plurality of GPS receivers are inputted, and the first carrier wave phase and the second carrier wave phase are linearly combined to calculate the linear combination of the GPS receivers. The attitude determination device according to claim 1, wherein an observed value of a double phase difference in a plurality of GPS receivers is calculated based on a carrier phase obtained by calculating a carrier phase and linearly combining the carrier phase. The double phase difference estimator is
Baseline vectors in a plurality of GPS receivers are calculated based on the orientation values of attitude angles determined by the attitude determination device, and based on the calculated baseline vector and the direction cosine basis vector of the GPS receiver with respect to the GPS satellite that transmitted the carrier wave. 4. The posture locating apparatus according to claim 1, wherein an estimated value of a double phase difference in a plurality of GPS receivers is calculated. The double phase difference residual calculation unit is:
The difference between the double phase difference observation value calculated by the double phase difference calculation unit and the double phase difference estimation value calculated by the double phase difference estimation unit is calculated, and a plurality of decimal values of the calculated difference are calculated. The attitude determination device according to any one of claims 1 to 4, wherein a difference between an observed value and an estimated value of a double phase difference in a GPS receiver is provided. An attitude determination method for an attitude determination device that calculates an attitude angle orientation value by correcting an attitude angle calculation value calculated using a gyro measurement value based on an observation value of a GPS (Global Positioning System) receiver,
A posture angle calculation process in which the posture angle calculation unit calculates a calculated value of the posture angle using the central processing unit based on the measured value of the gyro;
A double phase difference calculation unit that inputs a carrier phase observed by each GPS receiver and calculates an observation value of a double phase difference in a plurality of GPS receivers using a central processing unit;
A double phase difference estimation unit for calculating an estimated value of a double phase difference in a plurality of GPS receivers using a central processing unit based on the orientation value of the attitude angle determined by the attitude determination device; ,
The double phase difference residual calculation unit calculates the observation value of the double phase difference in the plurality of GPS receivers calculated by the double phase difference calculation unit and the two values in the plurality of GPS receivers calculated by the double phase difference estimation unit. A double phase difference residual calculation process for calculating a difference from the estimated value of the multiple phase difference using a central processing unit;
Calculation of the posture angle calculated by the posture angle calculation unit based on the difference between the observed value and the estimated value of the double phase difference in the plurality of GPS receivers calculated by the double phase difference residual calculation unit by the correction amount calculation unit Correction amount calculation processing for calculating a correction amount for the value using a central processing unit;
Attitude angle in which the attitude angle locator corrects the calculated value of the attitude angle calculated by the attitude angle calculator based on the correction amount calculated by the correction amount calculator using the central processing unit, and calculates the orientation value of the attitude angle A posture orientation method characterized by executing an orientation process.   A posture orientation program that causes a computer to execute the posture orientation method according to claim 6.

Images