Mobile control device, mobile control method, and program

Abstract

To realize a combined positioning system which can be configured using a small and inexpensive sensor and in which positioning accuracy of a mobile can be maintained even when an observation environment is inferior.SOLUTION: A mobile control device comprises a state estimation processing unit and a data generation unit. The state estimation processing unit outputs state information including the position, velocity, and attitude of a mobile using a state estimation filter operation for estimating the position, velocity, and attitude of the mobile on the basis of the inertia navigation data of the mobile and observation data obtained by observing at least one of the relative distance and relative velocity between the mobile and one or more reference objects. The data generation unit generates difference data which is used for correction processing of the state estimation filter operation and shows the difference between the observation data and the inertia navigation data. The state estimation processing unit calculates, as an error with regard to the inertia navigation data in the state estimation filter operation, error information displayed as a minute change unit quaternion and corrects the state information by multiplying the error information.SELECTED DRAWING: Figure 2

Description

[Technical field]

[0001] The present invention relates to a mobile object control device, a mobile object control method, and a program for locating a position of a mobile object and controlling the mobile object. [Background technology]

[0002] Conventionally, as a system for positioning a moving object, a hybrid navigation system that combines an inertial navigation system with a navigation system such as a Global Navigation Satellite System (GNSS) has been developed. For example, Patent Document 1 describes a method for estimating the position, speed, and attitude of a moving object using a Kalman filter, using inertial navigation data obtained by inertial navigation and positioning data measured by a GNSS receiver. In this method, it is possible to reduce the number of states to be calculated by expressing error information using a quaternion (a small change unit quaternion) with a scalar element of 1 (paragraphs

[0030] and

[0031] of the specification of Patent Document 1, etc.). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2007-276507 A Summary of the Invention [Problem to be solved by the invention]

[0004] For example, when using GNSS positioning data, it is necessary to observe the relative distance to at least four GNSS satellites. Therefore, the opportunity to correct the Kalman filter using positioning data is limited to cases where signals from four or more GNSS satellites can be received, and there is a possibility that the positioning accuracy of a moving object may decrease if the observation environment is poor, for example.

[0005] In recent years, the scope of application of positioning systems has expanded due to the miniaturization and cost reduction of sensors for inertial navigation. For this reason, there is a demand for technology to realize a hybrid positioning system that can provide sufficient positioning accuracy even when using small, inexpensive sensors.

[0006] In view of the above circumstances, an object of the present invention is to provide a mobile object control device, a mobile object control method, and a program that can realize a composite positioning system that can be constructed using small, inexpensive sensors and maintains the positioning accuracy of a mobile object even when the observation environment is poor. [Means for solving the problem]

[0007] In order to achieve the above object, a mobile object control device according to one embodiment of the present invention includes a state estimation processing unit and a data generation unit. The state estimation processing unit outputs state information including the position, velocity, and attitude of the moving body by a state estimation filter calculation that estimates the position, velocity, and attitude of the moving body based on inertial navigation data of the moving body and observation data that observes at least one of the relative distance or relative velocity between the moving body and one or more reference objects. The data generation unit generates difference data that represents a difference between the observation data and the inertial navigation data and is used in a correction process of the state estimation filter operation. In addition, the state estimation processing unit calculates error information related to the inertial navigation data in the state estimation filter calculation, the error information being expressed as a small-change unit quaternion specified by state variables having a smaller number of elements than the state variables representing the position, velocity, and attitude of the moving body included in the state information, and corrects the state information by multiplying the error information.

[0008] In this mobile object control device, the difference data used in the correction process of the state estimation filter calculation is generated from observation data that observes the relative distance and relative speed with respect to one or more reference objects. Therefore, for example, even if there is only one reference object, it is possible to correct the filter. Furthermore, the state estimation filter calculation uses error information expressed in a small change unit quaternion. Therefore, for example, it is possible to accurately correct the inertial navigation data while reducing the calculation load. This makes it possible to realize a composite positioning system that can be configured using small and inexpensive sensors and maintains the positioning accuracy of the mobile object even when the observation environment is poor.

[0009] The state estimation filter calculation may correct a related parameter that is related to the observation data and is used to calculate the difference data.

[0010] The reference object may transmit a positioning signal used for positioning the moving object. In this case, the moving object may be equipped with a receiver that receives the positioning signal and calculates the observation data based on the positioning signal. The related parameters may include a clock error used by the receiver to calculate the observation data.

[0011] The relevant parameters may include the change in error of the clock over time.

[0012] The state estimation filter calculation may correct a sensor parameter related to a sensor mounted on the moving body and used to calculate the inertial navigation data.

[0013] The data generation unit may generate calculated data that calculates at least one of the relative distance or relative velocity between the moving body and the reference object based on the inertial navigation data and information on the position or velocity of the reference object, and calculate the difference between the observation data and the calculated data as the difference data.

[0014] The inertial navigation data may be data expressing a position of the moving body as a quaternion. In this case, the data generator may calculate the calculated data using elements of the quaternion.

[0015] The reference object may be a GNSS satellite, in which case the observation data may be a pseudorange or a Doppler frequency to the GNSS satellite.

[0016] The data generation unit may generate the calculated data based on the orbital ephemeris received from the GNSS satellites and the inertial navigation data.

[0017] The data generating unit may correct the difference data based on data on a satellite clock delay, an ionospheric delay, and a tropospheric delay in the GNSS satellite.

[0018] A mobile object control method according to one embodiment of the present invention is a method executed by a computer system and includes the following steps. A step of outputting state information including the position, velocity, and attitude of the moving body by a state estimation filter calculation that estimates the position, velocity, and attitude of the moving body based on inertial navigation data of the moving body and observation data that observes at least one of the relative distance or relative velocity between the moving body and one or more reference objects. A step of generating difference data which represents a difference between the observation data and the inertial navigation data and is used in a correction process of the state estimation filter operation. The step of outputting the state information calculates error information related to the inertial navigation data in the state estimation filter calculation, the error information being expressed as a small-change unit quaternion specified by state variables with a smaller number of elements than the state variables representing the position, velocity, and attitude of the moving body included in the state information, and corrects the state information by multiplying the error information.

[0019] A program according to an embodiment of the present invention causes a computer system to execute the following steps. A step of outputting state information including the position, velocity, and attitude of the moving body by a state estimation filter calculation that estimates the position, velocity, and attitude of the moving body based on inertial navigation data of the moving body and observation data that observes at least one of the relative distance or relative velocity between the moving body and one or more reference objects. A step of generating difference data which represents a difference between the observation data and the inertial navigation data and is used in a correction process of the state estimation filter operation. The step of outputting the state information calculates error information related to the inertial navigation data in the state estimation filter calculation, the error information being expressed as a small-change unit quaternion specified by state variables with a smaller number of elements than the state variables representing the position, velocity, and attitude of the moving body included in the state information, and corrects the state information by multiplying the error information. Effect of the Invention

[0020] As described above, according to the present invention, it is possible to realize a composite positioning system that can be configured using small and inexpensive sensors and maintains the positioning accuracy of a moving object even when the observation environment is poor. Note that the effects described here are not necessarily limited to those described herein, and may be any of the effects described in this disclosure. [Brief description of the drawings]

[0021] [Figure 1] 1 is a block diagram showing a configuration example of a mobile object control device according to an embodiment of the present invention; [Diagram 2] 2 is a block diagram showing an example of a functional configuration of a mobile object control device; [Diagram 3] 5 is a flowchart showing an example of the operation of the mobile object control device. [Figure 4] 1 is a graph showing an example of a reception state of a GPS signal. [Diagram 5] 10 is a graph showing another example of the reception status of a GPS signal. [Figure 6] This is the result of mapping the position of a mobile object using a mobile object control device. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0022] Hereinafter, an embodiment of the present invention will be described with reference to the drawings.

[0023] [Configuration of mobile control device] 1 is a block diagram showing an example of the configuration of a mobile body control device according to this embodiment. The mobile body control device 100 is a device that is mounted or attached to a mobile body and outputs the position, speed, attitude, etc. of the mobile body. Hereinafter, information such as the position of the mobile body output from the mobile body control device 100 will be referred to as status information of the mobile body. The status information is used for movement control, attitude control, etc. of the mobile body.

[0024] 1, the mobile object control device 100 includes an acceleration sensor 10, a gyro sensor 11, a GNSS receiving unit 12, a storage unit 13, a calculation processing unit 14, and an output unit 15. The mobile object control device 100 is also connected to a driving device 16 that controls the position and attitude of the mobile object.

[0025] The mobile object control device 100 is a composite positioning system that measures the position, speed, attitude, and the like of a mobile object by combining both inertial navigation data using an acceleration sensor 10 and a gyro sensor 11 and data of a GNSS signal transmitted from a GNSS satellite. In this embodiment, the GNSS satellite corresponds to a reference object that is referred to for positioning the mobile object. The GNSS signal transmitted by the GNSS satellite corresponds to a positioning signal used for positioning the mobile object.

[0026] In the present disclosure, a moving body is any object that moves. The moving body includes vehicles such as automobiles, bicycles, ships, and aircraft, and unmanned moving devices such as flying drones and self-propelled robots. The moving body may also be a person, an animal, or the like.

[0027] In the following description, each piece of position, velocity, and attitude information is expressed in one of the following coordinate systems. (1) A right-handed Cartesian coordinate system (hereafter referred to as i-frame) with the center of the Earth as the origin and the Z-axis defined to be parallel to the Earth's axis of rotation. (2) A right-handed Cartesian coordinate system with the center of the Earth as the origin, the X-axis at 0 degrees latitude and 0 degrees longitude, and the Z-axis parallel to the Earth's axis of rotation (hereafter referred to as the e-frame, after the initial letters of Earth). (3) A right-handed Cartesian coordinate system with the moving body as the origin, the downward direction perpendicular to the geodetic ground as the Z-axis, and the north direction as seen from the moving body as the X-axis (hereinafter referred to as the g-frame, an abbreviation of geodetic). (4) A right-handed Cartesian coordinate system with the moving body as the origin and the downward direction perpendicular to the geodesic ground as the Z axis, in which the g frame is rotated by α (rad) around the Z axis in the g frame (hereinafter referred to as the n frame, taking the initial letter of navigation). (5) A right-handed Cartesian coordinate system with the moving body as the origin, the nose direction of the moving body as the X-axis, and the downward direction as the Z-axis (hereinafter referred to as the b-frame, after the initial letter of body).

[0028] In the following, in order to identify which coordinate system the information is in, an index is added to the information. For example, the Z axis in the b frame is expressed as follows:

number

[0029] The acceleration sensor 10 is installed on a moving object and measures the acceleration of the moving object in at least one direction. For example, a MEMS sensor is used as the acceleration sensor 10, which measures the movement acceleration of the moving object in each direction of each axis of the b frame and outputs information representing the measured acceleration. The acceleration information is expressed as follows:

number

[0030] The gyro sensor 11 is installed on the moving body and measures the rotational angular velocity of the moving body around one axis. As the gyro sensor 11, for example, a MEMS sensor is used, which detects the rotational angular velocity based on the Coriolis force that the micro diaphragm receives due to rotation. In this embodiment, a plurality of gyro sensors 11 are provided so as to detect each rotational velocity around each direction of each axis of the b frame, and output information representing the rotational angular velocity around each axis. The information representing the rotational angular velocity is expressed as follows:

number

number

[0031] In addition, rotation around the X-axis in the b-frame is called "roll", rotation around the Y-axis is called "pitch", and rotation around the Z-axis is called "yaw".

[0032] The GNSS receiver 12 is a receiver that receives GNSS signals including navigation messages from GNSS satellites. The GNSS signals include data indicating, for example, the signal transmission time and the position of the GNSS satellite. By receiving the GNSS signals, it is possible to calculate the propagation delay time of the GNSS signals. The GNSS receiver 12 outputs information indicating the position (latitude, longitude, altitude) of the mobile object from the received GNSS signals (hereinafter, referred to as GNSS positioning data). In this disclosure, GNSS refers to any satellite positioning system including GPS, Michibiki, GLONASS, Galileo, and the like. The GNSS receiver 12 is appropriately configured to enable satellite positioning using at least one of these systems.

[0033] The storage unit 13 is a non-volatile storage device, and may be, for example, a solid state drive (SSD) or a hard disk drive (HDD), etc. The storage unit 13 stores a control program for the mobile body control device 100, various data necessary for the operation of the mobile body control device 100, etc.

[0034] The arithmetic processing unit 14 is a calculation device that controls the operation of the mobile object control device 100. The arithmetic processing unit 14 has a hardware configuration required for a computer, such as a CPU and memory (RAM, ROM). The CPU loads a control program stored in the storage unit 13 into the RAM and executes it, thereby executing various processes. Specifically, the arithmetic processing unit 14 generates and outputs information (status information) on the position, attitude, and speed of the mobile object based on each piece of information output from the acceleration sensor 10, the gyro sensor 11, and the GNSS receiving unit 12.

[0035] The output unit 15 is a module that outputs data to a device external to the mobile object control device 100. The output unit 15 outputs, for example, state information including the position, attitude, and speed of the mobile object output by the calculation processing unit 14 to an external device (for example, a driving device 16).

[0036] Fig. 2 is a block diagram showing an example of the functional configuration of a mobile object control device. Fig. 2 shows functional blocks constituting the GNSS receiving unit 12 and the calculation processing unit 14. In Fig. 2, data exchanged between each functional block is shown by dotted line blocks, and the functional blocks are shown by solid lines.

[0037] The GNSS receiver 12 includes a data acquisition unit 21 and an internal calculation unit 22. The data acquisition unit 21 acquires raw data necessary for calculating GNSS positioning data (such as latitude, longitude, and altitude of a moving object). Here, acquiring raw data includes, for example, calculating data based on observation results of a GNSS signal or the like, and calculating or reading corresponding data using a model or a database.

[0038] The data acquisition unit 21 calculates a pseudorange and a Doppler based on the received GNSS signal. Here, the pseudorange is a distance calculated based on a propagation delay time obtained from the GNSS signal, and is a parameter corresponding to the relative distance between the GNSS satellite and the moving object. The Doppler is a frequency change (Doppler frequency) that occurs in the carrier wave of the GNSS signal due to the Doppler effect, and is a parameter corresponding to the relative speed between the GNSS satellite and the moving object. Note that for Doppler, it is necessary to convert the dimension of frequency (e.g., 1 / s) to the dimension of speed (e.g., m / s). For this reason, hereinafter, the unit converted by multiplying the Doppler frequency by the carrier wave wavelength of the GNSS signal will be described as Doppler.

[0039] The pseudorange and Doppler are observation data of the relative distance and relative velocity observed by receiving the GNSS signal. In this manner, the receiver (GNSS receiver 12) mounted on the moving object receives the GNSS signal and calculates the observation data based on the GNSS signal.

[0040] The data acquisition unit 21 also generates data on ionospheric delay and tropospheric delay. These delay data are calculated from the positions of GNSS satellites according to a predetermined model, for example. The data acquisition unit 21 also reads orbital information (ephemeris) included in the GNSS signal. The orbital information is information that indicates the positions of the GNSS satellites.

[0041] The internal calculation unit 22 calculates the position and speed of the mobile body equipped with the GNSS receiver unit 12 based on each raw data acquired by the data acquisition unit 21. For example, the latitude, longitude, and altitude of the mobile body are calculated as the position of the mobile body. For example, the speed along the latitude direction, longitude direction, and altitude direction are calculated as the speed of the mobile body. These data become positioning data measured using the GNSS.

[0042] Generally, if GNSS signals cannot be received from four or more GNSS satellites, GNSS positioning data cannot be calculated. Therefore, GNSS positioning data is output from the internal calculation unit 22 only when GNSS signals can be received from four or more GNSS satellites. Note that the data acquisition unit can acquire raw data such as the pseudorange and Doppler described above even if a signal can be received from only one GNSS satellite.

[0043] Next, the functional blocks of the arithmetic processing unit 14 will be described. In this embodiment, the CPU of the arithmetic processing unit 14 executes the program (control program) according to this embodiment stored in the storage unit 13, thereby realizing an inertial navigation processing unit 25, a filter processing unit 26, and a GNSS processing unit 27 as functional blocks. These functional blocks then execute the mobile object control method according to this embodiment. Note that dedicated hardware such as an IC (integrated circuit) may be used as appropriate to realize each functional block.

[0044] The inertial navigation processing unit 25 is a block that calculates the position, velocity, and attitude of a moving body using inertial navigation, and has a coordinate conversion unit 30, a velocity position calculation unit 31, a gravity model calculation unit 32, a velocity position output unit 33, an attitude calculation unit 34, an attitude output unit 35, and a bias update unit 36.

[0045] The coordinate conversion unit 30 converts the acceleration information in the b frame output by the acceleration sensor 10 into acceleration information for the n frame based on the information output by the attitude output unit 35, and outputs the converted information. Here, the conjugate quaternion of the quaternion q is defined as follows.

number

number

number

[0046] The speed / position calculation unit 31 uses an equation of motion related to the speed and position to calculate and output the speed and position. Here, the equation of motion related to the speed is written as follows.

number

number

[0047] When a moving body is affected by gravity (for example, in the case of an airplane or the like), gravity model calculation unit 32 outputs a quaternion representing the effect of gravity in the nth frame based on the position information output by speed position output unit 33, and outputs the quaternion to speed position calculation unit 31. The quaternion representing the effect of gravity is expressed as follows.

number

[0048] This quaternion is a parameter used in the second term on the right side of equation (1). Therefore, velocity position calculation unit 31 generates the equation of motion related to velocity (the above-mentioned equation (1)) using the output of coordinate conversion unit 30 and the output of gravity model calculation unit 32.

[0049] The speed position output unit 33 stores information (inertial navigation data) representing the speed and position output by the speed position calculation unit 31, and outputs it as necessary. The speed position output unit 33 also corrects the information representing the speed and position using the calculation result of the filter processing unit 26.

[0050] The attitude calculation unit 34 uses an equation of motion related to the attitude of the moving body to calculate and output the attitude. The equation of motion related to the attitude is written as follows.

number

number

[0051] The attitude output unit 35 stores the attitude information (inertial navigation data) output by the attitude calculation unit 34, and outputs it as necessary. The attitude output unit 35 also uses the calculation result of the filter processing unit 26 to correct the attitude information.

[0052] The bias update unit 36 ​​updates the zero points (bias amounts) of the acceleration and angular velocity data on each detection axis. Specifically, the bias update unit 36 ​​corrects the zero points of the total of six sensor outputs of the triaxial acceleration sensor 10 and the triaxial gyro sensor 11 using the calculation results of the filter processing unit 26.

[0053] The filter processing unit 26 executes a calculation using a Kalman filter (more specifically, an extended Kalman filter). The calculation using the Kalman filter according to the present disclosure is a calculation for estimating the position, velocity, and attitude of a moving body based on inertial navigation data of the moving body and observation data that observes at least one of the relative distance or relative velocity between the moving body and one or more reference objects. Note that the type of Kalman filter that can be used in the present invention is not limited. For example, in addition to the extended Kalman filter, it is possible to use a state estimation method such as an unded Kalman filter or a particle filter.

[0054] Here, a calculation algorithm using a Kalman filter is configured so that the position, speed, and attitude of a moving object can be estimated using the inertial navigation data calculated by the inertial navigation processing unit 25 and at least one of the pseudorange and Doppler, which are observation data obtained by the GNSS receiving unit 12 (data acquisition unit 21). Below, a case where both the pseudorange and Doppler are used as the observation data will be described. Note that it is also possible to use either the pseudorange or the Doppler as the observation data.

[0055] The filter processing unit 26 also outputs information (error information) for correcting the parameters handled by each block to the speed position output unit 33, the attitude output unit 35, the bias update unit 36, and the clock error update unit 38 provided in the GNSS processing unit 27 described later.

[0056] The calculation process by the filter processing unit 26 and the correction processes by each block (speed and position correction process by the speed position output unit, attitude correction process by the attitude output unit 35, zero point correction process by the bias update unit 36, and clock error correction process by the clock error update unit 38) correspond to the state estimation filter calculation according to this embodiment. The filter processing unit 26, the speed position output unit 33, the attitude output unit 35, the bias update unit 36, and the clock error update unit 38 configure a state estimation processing unit that outputs state information including the position, speed, and attitude of the moving object by the state estimation filter calculation.

[0057] The GNSS processing unit 27 generates difference data that represents the difference between the observation data and the inertial navigation data and is used in the correction process of the state estimation filter calculation. The correction process here refers to a process of updating (correcting) parameters such as an error covariance matrix and a Kalman gain used in the Kalman filter by using the observation data. In this embodiment, the GNSS processing unit 27 corresponds to a data generating unit. As shown in FIG. 2, the GNSS processing unit 27 has a relative distance / relative speed calculation unit 37, a clock error update unit 38, and a subtractor 39.

[0058] The relative distance / relative velocity calculation unit 37 calculates the relative distance or relative velocity between the GNSS satellite and the moving object using the orbit information of the GNSS satellite and the inertial navigation data. The calculation result becomes calculated data of the relative distance or relative velocity.

[0059] The clock error update unit 38 updates the error (clock error) of the clock of the GNSS receiver 12 used to calculate the observation data. Specifically, the clock error update unit 38 corrects two parameters, the clock error of the GNSS receiver 12 and the time change of the clock error, using the calculation result of the filter processing unit 26.

[0060] The subtractor 39 subtracts the calculation data calculated by the relative distance / relative velocity calculation unit 37 from the observation data of the relative distance and relative velocity observed by the GNSS receiving unit 12 (data acquisition unit 21). The result of this subtraction is used as difference data for the filter processing unit 26. In this embodiment, the difference data is corrected by the clock error updated by the clock error update unit 38, etc.

[0061] [Error model] The calculation processing unit 14 uses a Kalman filter to perform a process of correcting the information output by the speed / position output unit 33 and the attitude output unit 35 as inertial navigation data, using the observation data (pseudorange and Doppler) observed by the GNSS receiving unit 12. This process is a so-called tight coupling process.

[0062] In this embodiment, since the equations of motion for the position, velocity, and attitude are nonlinear, the Kalman filter is expanded to apply an error model. This error model is described as follows.

number

[0063] In this error model, as an error related to the inertial navigation data in the calculation by the Kalman filter (state estimation filter calculation), error information is calculated which is related to the state variables representing each of the position, speed, and attitude of the moving body included in the output (state information) of the calculation processing unit 14, and is expressed as an infinitesimal change unit quaternion specified by state variables having a smaller number of elements than the state variables, and the state information is corrected by multiplying this error information.

[0064] In the examples of equations (5) to (14),

number

number

number

number

[0065] Here, we will explain the parameters shown on the left side of equations (5) to (8) and equations (12) to (14) that are the subject of correction. Note that in the text, the tilde symbol ~ attached to the quaternions in equations (6) and (8) will be omitted. Also, in the text, a parameter α with a dot above it (time derivative of α) will be represented by a superscript d attached before the parameter α. d This is denoted as α.

[0066] The parameters shown in equations (5) to (8) have already been mentioned in explaining the operation of the inertial navigation processing unit 25. d re n ) represents the speed of the moving object. Also, the parameter (quaternion q e n ) represents the latitude and longitude of the moving object, and the parameter (h) in equation (7) represents the altitude of the moving object. Therefore, the parameters in equations (6) and (7) can be said to represent the position of the moving object. Also, the parameter in equation (8) (quaternion q n b ) represents the attitude of the moving object. In this way, the speed, position, and attitude of the moving object, which are the outputs of the device, are corrected by the Kalman filter.

[0067] The parameter (underlined b) in equation (12) is the zero point (bias) on each detection axis of the three-axis acceleration sensor 10 and the three-axis gyro sensor 11, and is expressed as a six-dimensional vector. In this manner, in this embodiment, the zero point (underlined b) of the sensor mounted on the moving body and used to calculate the inertial navigation data is corrected by calculation using the Kalman filter. Here, the zero point is an example of a sensor parameter. In the following text, the underlined b may be written as b_.

[0068] In order to correct the zero point, a motion equation is required to describe the time change of the zero point. Here, the fluctuation of the zero point of the acceleration sensor 10 and the gyro sensor 11 follows a first-order Gauss-Markov process. In this case, the time change of the zero point is expressed by a 6×6 diagonal matrix Λ b_ and a 6-dimensional vector of white noise w b_ It is written as follows using

number

[0069] It is known that the acceleration sensor 10 and the gyro sensor 11 suffer from a drift phenomenon in which the zero point varies over time. Therefore, by correcting the zero point of each sensor, it is possible to improve the positioning accuracy of the speed, position, attitude, etc. of a moving object. Sensor parameters other than the zero point may be corrected. Also, the correction of the zero point may not be necessary. Examples of sensor parameters other than the zero point include a scale factor (the ratio between the raw value output by the sensor and the physical quantity corresponding to that value) and misalignment (a parameter that corrects the fact that the three axes are not perpendicular to each other).

[0070] Parameter (B) in equation (13) is the error (clock error) of the clock used by the GNSS receiver 12 (GNSS receiver). More specifically, the clock error (s) is multiplied by the speed of light (m / s) and converted to distance (m), and this is used as parameter (B). Here, the time indicated by the clock of the GNSS receiver 12 is used when calculating the pseudorange, which is the observation data. That is, in the calculation using the Kalman filter, the clock error B of the clock used by the GNSS receiver 12 to calculate the observation data is corrected.

[0071] The parameter in equation (14) (B with a dot on the upper side) is the time change of the clock error (dB / dt). In this text, the time change of the clock error B is expressed as d B. For example, a clock may have a nonlinear error. For this reason, in addition to the clock error, the time change is also corrected as a state quantity. In other words, in the calculation by the Kalman filter, the time change of the clock error of the clock of the GNSS receiver 12 is d B is corrected. Time change of clock error d B is used, for example, for Doppler correction.

[0072] Receiver clock error B and its time change d To correct B, the time variation of each parameter is described as follows:

number

[0073] The clock error and its change over time shown in equations (13) and (14) are updated by the clock error update unit 38 and used in the calculation process of the differential data. In this way, in the calculation using the Kalman filter, the relevant parameters that are related to the observation data and used in the calculation of the differential data are corrected. Here, the clock error and its change over time are an example of the relevant parameters. Note that when a highly stable clock is used in the GNSS receiver unit 12, only the clock error is corrected and the change over time of the clock error may be ignored.

[0074] Here, the state variable x in the error model shown in equations (5) to (14) and the input u representing an external force acting on the error model are set as follows:

number

number

[0075] [Relative distance difference and relative speed difference] The following describes the difference data z input to the Kalman filter. The GNSS processing unit 27 generates data required to calculate the difference data z. Then, the difference data z is calculated using the data.

[0076] Specifically, calculated data is generated by calculating the relative distance and relative velocity between the moving object and the GNSS satellite based on the inertial navigation data and the position or velocity information of the GNSS satellite by the relative distance / relative velocity calculation unit 37. The calculated data is not a result of observing the relative position or relative velocity, but a result of calculation based on the position and velocity indicated by the inertial navigation data.

[0077] By using the calculated data of the relative position and relative velocity, it is possible to calculate the difference (error) between the inertial navigation data and the observation data, and this calculation result is used as the difference data z for the Kalman filter. That is, the difference between the observation data and the calculated data is calculated as the difference data z. Specifically, the difference data z is calculated for each of the relative distance and the relative velocity. As will be described later, the difference data z is corrected using related parameters (such as the clock error B) associated with the observation data.

[0078] The differential data z is a vector quantity, and its magnitude (number of elements) is the number of observed GNSS satellites, N sat , and depends on whether pseudoranges and Dopplers are available from each satellite. For example, if pseudoranges and Dopplers are available from all observed GNSS satellites, z is 2×N sat In the following, we will explain the correspondence between the pseudorange and Doppler in the difference data z. Note that the pseudorange is written as δ, and in the formula, the Doppler is written as δ with a dot above it, and in the text, the Doppler is written as d This is written as δ. Note that Doppler d δ is the Doppler frequency multiplied by the carrier wavelength of the GNSS signal and converted into units of speed.

[0079] [Differential data on pseudoranges] A row of the differential data z is the pseudorange δ obtained from the i-th GNSS satellite. i When the component z pr (The subscript pr comes from pseudorange) is written as follows:

number

[0080] In equation (20), r sati is the position of the GNSS satellite, and r useri is the position of the moving object. r sati and r useri are all three-dimensional vectors, but the arrows indicating the vectors are omitted in the text. ||α|| indicates the absolute value of α, so ||r sati -r useri represents the relative distance (calculated data) between the GNSS satellite and the moving object. This relative distance is calculated by the relative distance / relative speed calculation unit 37.

[0081] GNSS satellite positions sati is calculated based on the satellite's orbit model described in the ICD (Interface Control Document) of the GNSS to which the satellite belongs and the orbital information (ephemeris) broadcast by the satellite. useri is the quaternion q that indicates the position obtained as inertial navigation data. e n and altitude h. In this manner, in the relative distance / relative velocity calculation unit 37, calculation data of the relative distance is generated based on the orbital ephemeris and inertial navigation data received from the GNSS satellites.

[0082] In addition, the position of the moving object r useri is the satellite position r useri In this case, we will convert the coordinates to the WGS84 coordinate system (a Cartesian coordinate system that is Earth-centered and Earth-fixed), which is used to represent the satellite positions in GPS and is also commonly used for other purposes. In this case, the position r useri is written as follows:

number

[0083] Here, h is the altitude mentioned above. In the following, qi (i=0~3) is the quaternion q that represents the position on the earth. e n where the 0th component is the scalar part, and the 1st, 2nd, and 3rd components are the vector parts. Note that the symbol q0 used in the explanation of the rotation of a quaternion in the above explanation of the coordinate conversion unit 30 is used as a notation for a general-purpose quaternion, and is used differently here.

[0084] Also, R e ' is written as follows:

number

[0085] In this way, the inertial navigation data includes data expressing the position of the moving object by a quaternion. Therefore, the relative distance and relative velocity calculation unit 37 calculates the calculated data ||r sati -r useri is calculated using the elements of a quaternion, which makes it possible to eliminate calculation errors caused by the divergence of trigonometric functions, etc., and improve positioning accuracy.

[0086] In addition, in equation (20), the observation data (pseudo distance δ i ) from the calculated data (||r sati -r useri The part obtained by subtracting ||) is the basic component of the differential data z, and the subsequent terms are terms that correct the differential data z. Of these, B is the clock error of the GNSS receiver 12 described above.

[0087] Also, b iis the satellite clock delay, I is the ionospheric delay, and T is the tropospheric delay. All of these three delays are calculated using a model described in the ICD of the GNSS or parameters broadcast from the GNSS satellite. In this way, the GNSS processing unit 27 corrects the difference data Z based on the data of the satellite clock delay, the ionospheric delay, and the tropospheric delay in the GNSS satellite.

[0088] Note that these delays b i In addition to I and T, the position of the GNSS satellite r sati It is possible to replace the parameters of r with high-performance model parameters other than those mentioned above. For example, more accurate model parameters corrected by post-processing are available, so they can be used to sati may be calculated.

[0089] [Differential Doppler data] A row of the differential data z is the Doppler obtained from the i-th GNSS satellite. d When it corresponds to δ, its component z dp (The subscript dp comes from Doppler) is written as follows:

number

[0090] In equation (23), d r sati is the velocity of the GNSS satellite, d r useri is the speed of the moving object. d r sati and d r useri are all three-dimensional vectors, but the arrows indicating the vectors are omitted in the text. As in the case of equation (20), ||α|| represents the absolute value of α, and "·" represents an inner product, so the second term on the right side of equation (23) represents the relative velocity (calculated data) in the line of sight between the GNSS satellite and the moving object. This relative velocity is calculated by relative distance / relative velocity calculation unit 37.

[0091] GNSS satellite speed d r sati is the position of the GNSS satellite r sati Like the GNSS ICD, it is calculated based on the satellite orbit model and the orbital information (ephemeris) broadcast by the satellite. d r useri is the velocity vector already mentioned, which indicates the velocity obtained as inertial navigation data. d r e n In this way, the relative distance / relative velocity calculation unit 37 generates calculation data of the relative velocity based on the orbital ephemeris and inertial navigation data received from the GNSS satellites.

[0092] In addition, the velocity vector d r e n is expressed in n frames. Therefore, the velocity of the GNSS satellite d r sati (in the case of GPS, the WGS84 geodetic system mentioned above). This transformation is done by the quaternion q e n By coordinate transformation using, it can be written as follows:

number

[0093] In addition, in equation (23), the observation data (Doppler d δ i ) minus the calculated data (second term on the right side) is the basic component of the difference data z, and the subsequent terms are terms that correct the difference data z. d B is the change over time of the clock error of the GNSS receiver 12 described above.

[0094] Also, d b iis the time change in the satellite clock delay, which is calculated using the model described in the GNSS ICD and the parameters broadcast. Note that in equation (23) for relative velocity, the ionospheric delay and tropospheric delay, which existed as terms in equation (20) for relative distance, have been omitted because they change very slowly over time. On the other hand, just as in the case of relative distance, d b i or d r sati It is also possible to replace the parameters with other high-performance model parameters in addition to the methods mentioned above.

[0095] [Observation equation] As shown in equations (20) and (23), each component of the differential data z (z pr and z dp ), the relationship between the state variable x of the error model shown in equation (17) and the difference data z can be described by the following observation equation.

number

[0096] Here, H is a matrix that associates z with x, and v is an observation error ν for the observation data in the GNSS receiver 12. As described below, the matrix H is obtained by dividing the relative distance difference data z pr and the relative velocity difference data z dp Each is composed of

[0097] [Observation equation: relative distance] Differential data z pr The observation equation for is written as follows:

number

[0098] In the matrix H of equation (26), r LoSis a 1-row, 3-column matrix, which is the transposed unit vector of the line of sight of the satellite as seen from the moving object. LoS is written as follows:

number

[0099] Also, in the second column of the matrix H, LoS The matrix elements acting together with R in Eq. (22) are e It is written as follows using '.

number

[0100] In addition, the newly introduced parameter q α , q β , q γ is written as follows:

number

[0101] [Observation equation: relative velocity] Differential data z dp The observation equation for is written as follows:

number

[0102] Here, LoS is a 1-row, 3-column matrix shown in equation (27). In the first column of the matrix H in equation (30), r LoS The matrix elements acting together are written as follows:

number

[0103] In addition, in the second column of the matrix H in equation (30), LoSThe matrix elements acting together are written as follows:

number

[0104] In this way, the difference data z pr and z dp is calculated by applying the corresponding matrix H. In this process, the position and orientation of the moving object are expressed as quaternions, so there is no need to perform calculations using trigonometric functions that can cause accuracy problems. This makes it possible to sufficiently improve the positioning accuracy.

[0105] [State estimation filter calculation using Kalman filter] Since the relationship between the state variable x of the error model and the difference data z is expressed by the observation equation as described above, it is possible to estimate the state variable x by the Kalman filter.

[0106] For example, when the GNSS receiver 12 is unable to observe observation data, i.e. when there is no GNSS satellite from which it can observe pseudorange or Doppler, the filter processing unit 26 calculates a predicted value of the state variable x according to the prediction equation of the Kalman filter.

[0107] Here, the error covariance matrix P of the state variable x, the error covariance matrix Q of the input u, and the error covariance matrix R of the observation error ν are defined as follows:

number

[0108] Using these error covariance matrices P, Q, and R, the prediction equation of the Kalman filter is written as follows:

number

[0109] When the GNSS receiver 12 is able to observe observation data, i.e., when there is at least one GNSS satellite from which the pseudorange or Doppler can be observed, the filter processor 26 calculates an estimate of the state variable x according to the Kalman filter correction formula. At this time, the Kalman gain K and the error covariance matrix P are also updated as shown below.

[0110] The equation for modifying the Kalman filter and the update process for the parameters to be corrected are described as follows:

number

[0111] Here, equations (34) to (36) correspond to equations for correcting the Kalman filter. Equation (34) is an equation for correcting the Kalman gain K, and equation (35) is an equation for correcting the error covariance matrix P. Furthermore, equation (36) applies the Kalman gain to calculate an estimate of the state variable x. In this embodiment, the processes shown in equations (34) to (36) correspond to the correction process of the state estimation filter calculation.

[0112] Equations (37) to (43) are equations for correcting parameters that are actually to be corrected, using error information set as state variable x. Here, the subscript "INS" means information output by speed position output unit 33 and attitude output unit 35 as inertial navigation data. For example, the corrections for the speed and position shown in equations (37) to (39) are performed by speed position output unit 33. The correction for the attitude shown in equation (40) is performed by attitude output unit 35.

[0113] Moreover, the correction of the zero point (bias) of the inertial navigation sensor shown in equation (41) is performed by the bias update unit 36. The correction of the clock error of the GNSS receiver unit 12 and its change over time shown in equations (42) and (43) is performed by the clock error update unit 38.

[0114] The driving device 16 receives the values ​​(status information of the moving body) output by the speed position output unit 33 and attitude output unit 35 corrected in this manner via the output unit 15, detects the position, speed and attitude of the moving body using these values, and drives the moving body so as to match this information with the instructed information.

[0115] As described above, in this embodiment, a calculation is performed by a Kalman filter, which is a state estimation filter, based on inertial navigation data (INS), which is acceleration and angular velocity information obtained from a simple sensor such as a MEMS sensor, and observation data observed by receiving GNSS signals from GNSS satellites. Through this calculation, the position, velocity, and attitude information obtained from the INS is corrected using observation data (pseudorange and Doppler), which is raw data used for positioning a moving object by GNSS (so-called tight coupling method).

[0116] As a result, if there is even one GNSS satellite that can observe pseudorange and Doppler, it is possible to perform correction processing of the Kalman filter and correct the position, speed, and attitude obtained from the INS with high accuracy. In other words, even if GNSS positioning of a moving object is not possible (if signals from four or more GNSS satellites cannot be received), it is possible to maintain the positioning accuracy of composite positioning.

[0117] In addition, in order to expand the Kalman filter due to the fact that the equation of motion for the INS is nonlinear, an error model for the true value of the INS is constructed using infinitesimal change unit quaternions that are fewer than the state quantities to be corrected, and the output values ​​of the INS are corrected by multiplying the position and orientation by these infinitesimal change unit quaternions.

[0118] By applying such an error model to configure a mobile control device, it is possible to realize a composite navigation system that can effectively correct values ​​to values ​​close to the true values, even when using lightweight, small, and inexpensive sensors such as MEMS (Micro Electro Mechanical Systems) sensors.

[0119] [Example of operation of a mobile control device] A description will now be given of an example of the operation of the mobile object control device 100. Here, it is assumed that the mobile object control device 100 switches between the above-mentioned combined positioning of the tight coupling method and combined positioning of the loose coupling method.

[0120] Here, the loose coupling method is a method of correcting inertial navigation data using positioning data calculated by the internal calculation unit 22 of the GNSS receiver 12. The positioning data is data obtained by measuring the position and speed of a moving object from observation data (pseudorange and Doppler) from four or more GNSS satellites. Therefore, composite positioning using the loose coupling method is executable only when observation data from four or more GNSS satellites are obtained.

[0121] To realize the loose coupling method, the calculation processing unit 14 is provided with a subtractor 23 and a switching unit 24. The subtractor 23 subtracts the positioning data (position and velocity) output from the GNSS receiving unit 12 from the inertial navigation data (position and velocity) output from the inertial navigation processing unit 25, and outputs the position difference and the velocity difference as difference data.

[0122] Depending on the selected mode, the switching unit 24 switches between the output of the subtractor 39 of the GNSS processing unit 27 (tight coupling type difference data) and the output of the subtractor 23 (loose coupling type difference data) and supplies it to the filter processing unit 26.

[0123] Moreover, the filter processing unit 26 is configured to perform the above-mentioned arithmetic processing in the tight coupling method when the tight coupling method is selected, and to perform the arithmetic processing in the loose coupling method when the loose coupling method is selected. As the arithmetic processing in the loose coupling method, for example, the method described in Patent Document 1 is used.

[0124] Fig. 3 is a flowchart showing an example of operation of the mobile object control device 100. When the initialization of the mobile object control device 100 is completed, the process shown in Fig. 3 starts. This process is executed continuously while the mobile object control device 100 is in operation.

[0125] First, the process goes into a standby state for a preset predetermined time (step 101). The preset time is set to approximately the same as the update time of the GNSS positioning by the GNSS receiver 12, for example.

[0126] Next, the parameters to be corrected and the error covariance matrix of the Kalman filter are updated based on the acceleration detected by the acceleration sensor 10 and the angular velocity detected by the gyro sensor 11 (step 102). Here, the speed of the moving body is updated by equation (1), the position (latitude, longitude, altitude) of the moving body is updated by equations (2) and (3), and the attitude of the moving body is updated by equation (4). Furthermore, the zero points of the acceleration sensor 10 and the gyro sensor 11 are updated by equation (15), and the clock error of the receiver and its time change are updated by equation (16). Moreover, the error covariance matrix P of the state variables is updated by the prediction equation of the Kalman filter, equation (33).

[0127] Next, it is determined whether the output (GNSS output) by the GNSS receiver 12 is available (step 103). For example, if no GNSS signals are received from the GNSS satellites, it is determined that the GNSS output is unavailable (No in step 103), and step 109 is executed to output the latest parameters (position, velocity, attitude, etc.).

[0128] If at least one GNSS signal from a GNSS satellite is properly received, the GNSS output is deemed to be available (Yes in step 103), and it is determined whether positioning data is being supplied from the GNSS receiver 12 (step 104). For example, if the number N of GNSS satellites from which GNSS signals can be received is satIf is less than 4, the positioning data cannot be calculated. There are also cases where the positioning data cannot be calculated due to the accuracy of the GNSS signal, etc. In such cases, it is determined that the positioning data is not provided (No in step 104), and correction processing is performed in step 107 using the tight coupling method.

[0129] If the positioning data is provided (Yes in step 104), it is determined whether or not to use the positioning data in the correction process (step 105). This determination process is a process for determining whether to execute a tight coupling mode or a loose coupling mode. For example, the user of the mobile object control device 100 may manually select the mode, or the mode may be automatically selected depending on the reception status of the GNSS signal in the GNSS receiver 12, etc.

[0130] For example, when the loose coupling mode is selected, the positioning data is used for the correction process (Yes in step 105), and the correction process of the loose coupling method is executed. In this case, the switching unit 24 supplies the output of the subtractor 23 to the filter processing unit 26, and the error covariance matrix of the Kalman filter is updated in the filter processing unit 26 using the positioning data, and error information related to the speed, position, and attitude of the moving object is estimated.

[0131] In the loose coupling method, the number of state variables (error information) handled is smaller than in the tight coupling method, for example, so the effect of reducing the calculation load can be expected. Therefore, for example, when it is desired to reduce battery consumption, the loose coupling method mode may be selected.

[0132] On the other hand, when the tight coupling mode is selected or the reliability of the positioning data is low, the positioning data is not used in the correction process (No in step 105), and the correction process of the tight coupling method is executed (step 107). In this case, the switching unit 24 supplies the output of the subtractor 39 of the GNSS processing unit 27 to the filter processing unit 26, and the error covariance matrix of the Kalman filter is updated using the observation data (pseudorange and Doppler), and error information is estimated.

[0133] Specifically, the Kalman gain is updated by equation (34), and the error covariance matrix P is updated by equation (35). For these updates, the H matrix shown in equations (21) and (26) is used. Furthermore, error information related to the speed, position, and attitude of the moving body is estimated by equation (36). The zero point (bias) of the inertial navigation sensor, the clock error of the GNSS receiver unit 12, and its change over time are also estimated at the same time.

[0134] When step 106 or step 107 is completed, the parameters to be corrected are corrected by the estimated value of the error information (step 108). Specifically, the speed of the moving body is corrected by equation (37), the position (latitude, longitude, altitude) of the moving body is corrected by equations (38) and (39), and the attitude of the moving body is corrected by equation (40). When the correction process of the tight coupling method is performed, the zero point (bias) of the sensor for inertial navigation is corrected by equation (41), and the clock error and its time change of the GNSS receiver unit 12 are corrected by equations (42) and (43). Note that even in the loose coupling method, it is possible to correct the zero point of the sensor for inertial navigation by equation (41) by appropriately configuring an error model.

[0135] When step 108 is completed, each parameter such as the corrected position, speed, attitude, etc. is output appropriately to the driving device 16, etc. (step 109). When each parameter is output, the processing from step 101 onwards is executed again.

[0136] In this way, even if the mobile object control device 100 is unable to perform GNSS positioning (No in step 104), as long as there is observation data, it is possible to perform correction processing using the tight coupling method. Furthermore, since the tight coupling method uses the observation data that is the source of the positioning data, it is possible to perform detailed control for each GNSS satellite, for example. This enables correction processing that takes into account errors and the like for each GNSS satellite, making it possible to sufficiently improve the positioning accuracy and reliability.

[0137] [Example] An operation experiment of the mobile object control device 100 will be described. In the experiment, an inertial measurement unit, a GNSS receiver, and a general-purpose computer were used. The inertial measurement unit (IMU) is a sensor unit equipped with an acceleration sensor 10 and a gyro sensor 11, and outputs acceleration and angular velocity required for inertial navigation. The GNSS receiver is a unit that constitutes the GNSS receiving unit 12. The GNSS receiver used is capable of outputting pseudo-range (or carrier phase) and Doppler frequency, which are observation quantities such as relative distance and relative velocity used to calculate positioning data, and error information thereof. The general-purpose computer is an arithmetic device that constitutes the arithmetic processing unit 14.

[0138] In the experiment, the output data (velocity and angular velocity) of the inertial measurement unit and the output data (relative distance and relative velocity data) of the GNSS receiver were processed by the program of the present invention implemented in a general-purpose computer. Based on each output data, the program performed state estimation filter calculations and output the velocity, position, and attitude of the moving object, as well as their estimation errors.

[0139] In the experiment, a bicycle was used as the moving object. An inertial measurement unit and a GNSS receiver were fixed to the bicycle, and the bicycle was driven while acquiring data. After driving, the acquired data was imported into a general-purpose computer and processed using a program. In this experiment, an MPU9250 (formerly manufactured by Invensense) was used as the inertial measurement unit, and an NEO-8T (manufactured by u-blox) was used as the GNSS receiver. GPS was also used in the experiment.

[0140] In this example, a bicycle was used as the moving object, but the present invention can be applied to any moving object such as an automobile, an airplane, or a ship. In this experiment, offline positioning was performed in which data was acquired and then processed by a general-purpose computer. However, the present invention is not limited to this, and can be applied to real-time positioning in which data is acquired and output is obtained at the same time.

[0141] FIG. 4 is a graph showing an example of the reception status of a GPS signal. FIG. 4 is a graph showing the reception status of a GPS signal while a moving object (bicycle) is moving. The upper graph in FIG. 4 plots the elevation angle for each satellite at a certain time. The horizontal axis of the graph is the time [seconds] recorded in the GPS calendar. The vertical axis of the graph is the elevation angle [deg]. The elevation angle for each satellite represents, for example, the angle at which the satellite is looked up from the current position. The upper graph also lists satellite numbers No. 1 to No. 11 to distinguish each GPS satellite.

[0142] The graph at the bottom of Figure 4 shows the number of GPS satellites visible to the receiver at a certain time (horizontal axis). GPS satellites visible to the receiver here are GPS satellites whose GPS signals are being properly received by the receiver. The horizontal axis of the graph is the same time [seconds] as the graph at the top, and the vertical axis of the graph is the number of GPS satellites visible to the receiver.

[0143] Figure 5 is a graph showing another example of the reception status of GPS signals. Figure 5 shows the data shown in Figure 4 processed to reduce the number of GPS satellites visible to the receiver within a certain period. Here, the data is processed so that only three satellites with satellite numbers No. 1, No. 2, and No. 3 are visible for one minute from 172900 seconds to 172960 seconds (limited period 50). Therefore, as shown in the graph below, only a maximum of three satellites are visible during limited period 50, so positioning data cannot be calculated using GPS positioning.

[0144] Fig. 6 shows the result of mapping the position of a moving body using the moving body control device 100. Fig. 6 shows mapping data in which the position of a moving body (bicycle) is mapped along the horizontal direction. The vertical axis in the figure represents latitude, and the horizontal axis represents longitude. The area surrounded by a dotted line in the figure represents the area (restricted area 52) in which the moving body moved during the restricted period 50 shown in Fig. 5.

[0145] Mapping data 51a is a result of performing positioning in a tight coupling mode using observation data, using the unprocessed data shown in Fig. 4. Mapping data 51b is a result of performing positioning in a tight coupling mode using observation data, using the processed data shown in Fig. 5. Mapping data 51c is a result of performing positioning in a loose coupling mode using positioning data, using the processed data shown in Fig. 5.

[0146] For example, the trajectory of the mapping data 51c in the restricted area 52 is found to deviate significantly from the trajectory of the mapping data 51a that has not been processed into data. This is thought to be the result of errors accumulating because, with the loose coupling method, no positioning data was obtained in the restricted area 52 and no corrections were received from the GPS.

[0147] In contrast, when the trajectory of the mapping data 51b in the restricted area 52 is viewed, it can be seen that the trajectory is substantially identical to the trajectory of the mapping data 51a. In this way, even if the number of satellites visible from the receiver becomes less than four, the inertial navigation data can be successively corrected using observation data (pseudorange and Doppler) obtained by observing the GPS signals from the satellites visible at that time, so it is possible to avoid a deterioration in positioning accuracy. This makes it possible to maintain a high level of positioning accuracy of the moving object even in a poor observation environment, compared to, for example, the loose coupling method.

[0148] As described above, in the mobile object control device 100 according to this embodiment, the difference data used in the correction process of the calculation by the Kalman filter is generated from the observation data that observes the relative distance and relative speed with respect to one or more GNSS satellites (GPS satellites, etc.). Therefore, for example, even if there is only one GNSS satellite, the filter can be corrected. Furthermore, in the state estimation filter calculation, error information expressed by a small change unit quaternion is used. Therefore, for example, it is possible to accurately correct the inertial navigation data while reducing the calculation load. This makes it possible to realize a composite positioning system that can be configured using small and inexpensive sensors and maintains the positioning accuracy of the mobile object even in a poor observation environment.

[0149] In a composite positioning system that requires positioning data, the opportunity to correct the Kalman filter, which is part of the state estimation filter's operation, was limited to cases where positioning data was obtained as input. This meant that the positioning accuracy could decrease at the exits of tunnels and in urban areas with many buildings, limiting the environments in which the system could be used.

[0150] Furthermore, compared to the observational data such as relative distance and relative velocity used in the calculation of positioning data, positioning data loses information about the inherent errors in relative distance and relative velocity. This makes it impossible to process the data according to the state of the GNSS satellites that provide the relative distance and relative velocity. For example, it is known that the relative position and relative velocity provided by GNSS satellites located at low elevation angles are likely to have large errors in the relative position due to the so-called multipath effect. If positioning data is used as is, it is not possible to reflect such known information, which adversely affects the estimated position.

[0151] In the mobile body control device 100 of this embodiment, while using the infinitesimal change unit quaternion as error information, a state estimation filter calculation is performed using observation data (relative distance and relative speed) used to calculate the positioning data instead of the positioning data, and the position, attitude, speed and their estimated errors of the mobile body are output.

[0152] For example, in order to calculate positioning data, it is generally necessary to observe the relative distance from at least four GNSS satellites in order to resolve the four degrees of freedom of three-dimensional position and time. In contrast, the present invention, which does not use positioning data as input, makes it possible to correct the Kalman filter with the relative distance from at least one GNSS satellite. This allows for more frequent correction of the Kalman filter than methods that require positioning data, making it possible to maintain high accuracy of the final output even in poor observation environments for GNSS satellites.

[0153] In addition, in the present invention, by inputting the relative position and relative velocity of each satellite, information regarding these errors can be incorporated into the Kalman filter. This makes it possible to set the estimation error to a large value for the relative distance observed from a GNSS satellite with a low elevation angle, which generally reduces the observation accuracy. As a result, it becomes possible to limit the influence of GNSS satellites with low observation accuracy on the finally estimated speed, position, and attitude of the moving object, as well as the information on the estimation error.

[0154] When using relative position and relative velocity as inputs to a Kalman filter, it is necessary to calculate the position and velocity of a GNSS satellite from an orbital ephemeris and convert the coordinate system. In the present invention, by using a small change unit quaternion, it is possible to eliminate trigonometric function calculations even when performing such a coordinate system conversion process. This makes it possible to reduce numerical errors caused by trigonometric function calculations in a computer, and improve positioning accuracy.

[0155] <Other embodiments> The present invention is not limited to the above-described embodiment, and various other embodiments can be realized.

[0156] In the above, the method of calculating the relative speed between the moving object and the GNSS satellite based on the Doppler frequency has been described. However, the present invention is not limited to this, and the relative speed may be calculated based on other parameters. For example, the relative speed may be calculated based on the time change of the carrier phase of the GNSS signal.

[0157] In the above, an example has been described in which a GNSS receiver that receives radio waves from GNSS satellites such as GPS is used. However, the present invention is not limited to this, and other measuring devices that can output relative position and relative speed using light, radio waves, sound waves, etc. can be used alone or in combination with other measuring devices.

[0158] Of the characteristic features of the present invention described above, at least two of the characteristic features may be combined. In other words, the various characteristic features described in each embodiment may be arbitrarily combined without distinction between the embodiments. In addition, the various effects described above are merely examples and are not limiting, and other effects may be exhibited. [Explanation of symbols]

[0159] 12…GNSS receiver 14...Computation processing unit 25...Inertial navigation processing unit 26...Filter processing section 27…GNSS processing unit 37...Relative distance and relative speed calculation unit 38...Clock error update unit 39…Subtractor 100...Mobile control device

Claims

1. a state estimation processing unit that outputs state information including the position, velocity, and attitude of the moving body by a state estimation filter operation that estimates the position, velocity, and attitude of the moving body based on inertial navigation data of the moving body and observation data that observes at least one of a relative distance or a relative velocity between the moving body and one or more reference objects; a data generating unit that generates difference data representing a difference between the observation data and the inertial navigation data and that is used in a correction process of the state estimation filter calculation; Equipped with The state estimation processing unit calculates error information related to state variables representing the position, velocity, and attitude of the moving body included in the state information and expressed as an infinitesimal change unit quaternion specified by state variables having a smaller number of elements than the state variables, as an error related to the inertial navigation data in the state estimation filter calculation, and corrects the state information by multiplying the error information. Mobile control device.

2. The mobile object control device according to claim 1, The state estimation filter operation corrects a related parameter that is related to the observation data and is used to calculate the difference data. Mobile control device.

3. The mobile object control device according to claim 2, The reference object transmits a positioning signal used to position the moving object; the moving object is equipped with a receiver that receives the positioning signal and calculates the observation data based on the positioning signal; The related parameters include the error of the clock used by the receiver to calculate the observation data. Mobile control device.

4. The mobile object control device according to claim 3, The relevant parameters include the time variation of the clock error. Mobile control device.

5. The mobile object control device according to claim 1, The state estimation filter calculation corrects sensor parameters related to a sensor mounted on the moving body and used to calculate the inertial navigation data. Mobile control device.

6. The mobile object control device according to claim 1, The data generation unit generates calculated data by calculating at least one of a relative distance or a relative velocity between the moving body and the reference object based on the inertial navigation data and information on a position or a velocity of the reference object, and calculates a difference between the observation data and the calculated data as the difference data. Mobile control device.

7. The mobile object control device according to claim 6, the inertial navigation data is data expressing a position of the moving body in a quaternion; The data generation unit calculates the calculated data using elements of the quaternion. Mobile control device.

8. A mobile object control device according to any one of claims 1 to 7, the reference object is a GNSS satellite; The observation data is a pseudorange or a Doppler frequency with respect to the GNSS satellite. Mobile control device.

9. The mobile object control device according to claim 8, The data generation unit generates the calculated data based on the orbital ephemeris received from the GNSS satellite and the inertial navigation data. Mobile control device.

10. The mobile object control device according to claim 8, The data generating unit corrects the difference data based on data of a satellite clock delay, an ionospheric delay, and a tropospheric delay in the GNSS satellite. Mobile control device.

11. a step of outputting state information including the position, velocity, and attitude of the moving body by a state estimation filter operation that estimates the position, velocity, and attitude of the moving body based on inertial navigation data of the moving body and observation data that observes at least one of a relative distance or a relative velocity between the moving body and one or more reference objects; generating difference data representing a difference between the observation data and the inertial navigation data and used in a correction process for the state estimation filter calculation; A mobile object control method executed by a computer system, comprising: The step of outputting the state information calculates error information related to the inertial navigation data in the state estimation filter calculation, the error information being expressed as an infinitesimal change unit quaternion specified by state variables having a smaller number of elements than the state variables representing the position, velocity, and attitude of the moving body included in the state information, and correcting the state information by multiplying the error information. A method for controlling a moving object.

12. a step of outputting state information including the position, velocity, and attitude of the moving body by a state estimation filter operation that estimates the position, velocity, and attitude of the moving body based on inertial navigation data of the moving body and observation data that observes at least one of a relative distance or a relative velocity between the moving body and one or more reference objects; generating difference data representing a difference between the observation data and the inertial navigation data and used in a correction process for the state estimation filter calculation; A program for causing a computer system to execute the above, The step of outputting the state information calculates error information related to the inertial navigation data in the state estimation filter calculation, the error information being expressed as an infinitesimal change unit quaternion specified by state variables having a smaller number of elements than the state variables representing the position, velocity, and attitude of the moving body included in the state information, and correcting the state information by multiplying the error information. program.

Images