JP6221295B2 - Position calculation method and position calculation apparatus - Google Patents

Description

  The present invention relates to a method for calculating a position and the like.

  In the inertial navigation calculation that calculates the position using the measurement result of the sensor, there is a problem that the accuracy of the position calculation is lowered due to various errors that may be included in the measurement result of the sensor. As a technique for solving this problem, for example, Patent Document 1 discloses a technique for determining whether or not initialization of a yaw angle of a moving body is abnormal.

JP 2012-42285 A

In order to start the inertial navigation calculation, it is generally necessary to first obtain the direction of the moving direction. This is because inertial navigation is started based on the direction. For this reason, the start of the inertial navigation calculation is delayed depending on the time required to acquire the reference bearing information.
The present invention has been devised in view of the above problems, and its purpose is to propose a new method for starting inertial navigation calculation before acquiring information on a reference azimuth. is there.

  According to a first aspect of the present invention, there is provided a first invention for calculating a movement position on an absolute provisional coordinate system in which an absolute coordinate system is temporarily set by inertial navigation calculation, calculating a movement direction, Using the direction to obtain the azimuth difference between the absolute coordinate system and the absolute temporary coordinate system, and using the calculation start position of the inertial navigation calculation and the azimuth difference, the moving position is represented on the absolute coordinate system. The position calculation method includes correcting to the position of.

  Further, as another invention, an inertial navigation calculation unit that calculates a movement position on an absolute temporary coordinate system in which an absolute coordinate system is temporarily set by inertial navigation calculation, a movement direction calculation unit that calculates a movement direction, and the movement direction Using the azimuth difference acquisition unit that acquires the azimuth difference between the absolute coordinate system and the absolute temporary coordinate system using the calculation start position of the inertial navigation calculation and the azimuth difference, the moving position is determined as the absolute coordinate. It is good also as comprising a position calculation apparatus provided with the correction part which corrects to the position on a system.

  According to the first aspect of the present invention, it is possible to start calculating the movement position on the absolute temporary coordinate system in which the absolute coordinate system is temporarily set by inertial navigation calculation. And the orientation difference between the absolute coordinate system and the absolute temporary coordinate system (for example, the east, west, south, and north directions defined in the absolute coordinate system (in this case, the true direction) and the absolute temporary coordinate system are defined) If the difference from the direction of east, west, south, and north (in this case, a temporary direction) can be acquired, the moving position can be corrected to a position on the absolute coordinate system using the difference in orientation. Therefore, the inertial navigation calculation can be started even in a state where the reference orientation (in this case, the orientation of the absolute coordinate system) is unknown. Also, the position on the absolute temporary coordinate system calculated with the absolute coordinate system unknown is corrected later to the correct position on the absolute coordinate system using the azimuth difference between the absolute coordinate system and the absolute temporary coordinate system. be able to. For this reason, it is not necessary to wait for the start of the inertial navigation calculation until the direction of the absolute coordinate system is known.

  Further, as a second invention, in the position calculation method according to the first invention, a predetermined filtering process is performed on the calculated movement position before the correction and on the corrected position after the correction. Further, a position calculation method that further includes the above may be configured.

  According to the second aspect of the present invention, the predetermined filter processing is performed on the movement position calculated by the inertial navigation calculation before the position correction and after the correction on the movement position after the correction. Regardless of this, the position can be calculated appropriately.

  According to a third aspect of the present invention, in the position calculation method according to the second aspect of the present invention, the filter process is a Kalman filter process including performing an error estimation calculation by inheriting an estimated error value before and after the correction. A method may be configured.

  According to the third aspect of the invention, since the Kalman filter process including inheriting the estimated error value before and after the position correction and performing the error estimation calculation is performed, the error estimated value is reset before and after the position correction. There is no need. In the Kalman filter processing, a predetermined filter processing operation is repeatedly executed, but it takes a certain time until the estimated error value is stabilized. Since it is not necessary to reset the error estimated value before and after the position correction, the final position does not vary due to the error estimated value of the Kalman filter processing just because the position is corrected.

  As a fourth invention, in the position calculation method of any one of the first to third inventions, calculating the moving direction is based on a satellite signal from a positioning satellite on the absolute coordinate system. The method may include calculating a moving direction, and obtaining the azimuth difference may constitute a position calculating method including obtaining the azimuth difference using a moving direction based on the satellite signal.

  According to the fourth aspect of the invention, the absolute coordinate system and the absolute temporary coordinate system are calculated by calculating the movement direction on the absolute coordinate system based on the satellite signal from the positioning satellite and using the movement direction based on the satellite signal. It is possible to easily obtain the difference in orientation.

  Further, as a fifth invention, in the position calculation method according to any one of the first to third inventions, acquiring the azimuth difference acquires the azimuth difference using a measurement azimuth measured by an azimuth meter. It is good also as comprising the position calculation method including this.

  According to the fifth aspect of the invention, by using the measurement azimuth measured by the azimuth meter, it is possible to easily obtain the azimuth difference between the absolute coordinate system and the absolute temporary coordinate system.

  Further, as a sixth invention, in the position calculation method of any one of the first to fifth inventions, the method further includes determining a stable time when the moving direction is stable, and obtaining the heading difference The position calculation method may be configured to include acquiring the orientation difference at the stable time.

  According to the sixth aspect of the invention, by determining the stable time when the moving direction is stable and acquiring the azimuth difference at the stable time, an incorrect azimuth difference due to a change in the moving direction of the user or the like is acquired. The possibility of being reduced can be reduced.

  Further, as a seventh invention, obtaining the azimuth difference in the position calculation method of the sixth invention includes averaging the moving direction in the stable time and using the averaged moving direction. It is good also as comprising a position calculation method including obtaining.

  According to the seventh aspect of the present invention, the azimuth difference between the absolute temporary coordinate system and the absolute coordinate system can be acquired by averaging the moving directions at the stable time and using the averaged moving directions.

The figure which shows the structural example of the whole system of a position calculation apparatus. The figure which shows an example of a function structure of a position calculation apparatus. The figure which shows the functional block of a process part. Illustration of the principle. The flowchart which shows the flow of a position calculation process. The figure which shows an example of the experimental result which performed position calculation. The figure which shows an experimental result. The figure which shows the functional block of the process part in a modification.

  Hereinafter, an example of a preferred embodiment of the present invention will be described with reference to the drawings. However, it goes without saying that embodiments to which the present invention can be applied are not limited to the embodiments described below.

1. System Configuration FIG. 1 is a diagram illustrating a configuration example of an entire system of a position calculation device 1 in the present embodiment. The position calculation device 1 is a small electronic device that is used by being worn on the user's waist (right waist or left waist), for example, and is a type of position calculation device that calculates and displays the position of the user. The position calculation device 1 includes an operation button 3 that is an input device for a user to input various operations related to position calculation, a liquid crystal display 5 on which information such as the calculated position is displayed, and a speaker 7. In addition to the IMU (Inertial Measurement Unit) 10, a processor such as a CPU (Central Processing Unit) responsible for the function of the processing unit 100 in FIG. 2, a memory responsible for the function of the storage unit 600, and a function of the communication unit 500 are provided. It has a communication circuit, a GPS (Global Positioning System) sensor 20, and the like.

  In this embodiment, three types of coordinate systems are defined. The first coordinate system is a local coordinate system that is a three-dimensional orthogonal coordinate system (sensor coordinate system) associated with the IMU 10 included in the position calculation device 1. In this specification, the local coordinate system is also referred to as an L (Local) frame. In the present embodiment, the three axes of the local coordinate system are denoted as x-axis, y-axis, and z-axis.

  The second coordinate system is a three-dimensional orthogonal coordinate system (moving body coordinate system) associated with the moving body. In this specification, the moving body coordinate system is also referred to as a V (Vehicle) frame. In this embodiment, the front / rear direction with the front of the user as positive is the roll axis (R axis), the left / right direction with the right side as positive is the pitch axis (P axis), and the vertical direction with the vertical down is positive is the yaw axis ( Q axis).

  The third coordinate system is a three-dimensional orthogonal coordinate system (absolute coordinate system) that is a coordinate system that defines a moving space of a moving object. In this specification, the absolute coordinate system is also referred to as an A (Absolute) frame. The A frame is defined, for example, as an NED (North East Down) coordinate system known as a northeast lower coordinate system or an ECEF (Earth Centered Earth Fixed) coordinate system known as an earth-centered earth fixed coordinate system. In the present embodiment, the three axes of the absolute coordinate system are expressed as an X axis, a Y axis, and a Z axis.

  Further, in the present embodiment, the inertial navigation calculation can be started even when the accurate azimuth of the absolute coordinate system (A frame) is not recognized and the moving body starts moving. While the azimuth of the absolute coordinate system cannot be recognized, the position is calculated by the “absolute temporary coordinate system” in which the absolute coordinate system is temporarily set. The position of the moving moving body in the absolute temporary coordinate system corresponds to the moving position in the absolute temporary coordinate system.

  The IMU 10 is a sensor unit mounted on the position calculation device 1 and is known as an inertial navigation unit. The IMU 10 includes an acceleration sensor 11 and a gyro sensor 13. As shown in FIG. 1, the z-axis of the IMU 10 is the vertical axis.

  The acceleration sensor 11 detects the acceleration vector of the user. As the acceleration sensor 10, for example, a MEMS (Micro Electro Mechanical Systems) sensor is used. The value measured by the acceleration sensor 10 is the acceleration of the moving object measured in the local coordinate system.

  In this specification, a scalar and a vector will be appropriately distinguished and described. In principle, acceleration and speed refer to acceleration and speed magnitude (scalar amount), and acceleration vector and speed vector refer to acceleration and speed considering direction and magnitude.

  The gyro sensor 13 is a sensor that detects an angular velocity of the user, and detects an angular velocity (hereinafter referred to as “local coordinate angular velocity”) in the same local coordinate system as the acceleration sensor 11 included in the traveling direction velocity calculation device 1.

  The GPS sensor 20 is a sensor that receives a GPS satellite signal transmitted from a GPS satellite, which is a kind of positioning satellite, and calculates a user's position and velocity vector using the GPS satellite signal. In addition, since the method of calculating a position and a velocity vector using GPS is conventionally well-known, detailed description is abbreviate | omitted.

  In addition, in order to clarify various quantities defined in each coordinate system, a description will be given with the type of the coordinate system added to the head of the word representing each quantity. For example, an acceleration vector expressed in the local coordinate system is referred to as “local coordinate acceleration vector”, and an acceleration vector expressed in the absolute coordinate system is referred to as “absolute coordinate acceleration vector”. The same applies to other quantities. In addition, the relative posture (relative posture) of the sensor with respect to the user is expressed as “sensor posture”, and the posture angle (relative posture angle) is referred to as “sensor posture angle”.

  In addition, in the mathematical expression in this specification, the component of the velocity vector is represented by a small letter “v”. In the notation of the velocity vector, the type of the coordinate system is indicated by a superscript uppercase suffix. “L” means a local coordinate system, “V” means a moving object coordinate system, and “A” means an absolute coordinate system. In addition, each axis component of the corresponding coordinate system is indicated by a subscript in lower case. For example, in the case of three axes in the local coordinate system, the notations “x”, “y”, and “z” are used.

2. Functional Configuration FIG. 2 is a diagram illustrating an example of a functional configuration of the position calculation device 1.
The position calculation device 1 includes an IMU 10, a GPS sensor 20, a processing unit 100, an operation unit 200, a display unit 300, a sound output unit 400, a communication unit 500, and a storage unit 600. The

  The processing unit 100 comprehensively controls each unit of the position calculation device 1 according to various programs such as a system program stored in the storage unit 600.

FIG. 3 is a functional block diagram illustrating an outline of a functional configuration of the processing unit 100.
The processing unit 100 includes an inertial navigation calculation unit 110, a heading difference acquisition unit 120, a correction unit 130, and a Kalman filter processing unit (hereinafter referred to as “KF (Kalman Filter) processing unit”) 140 as functional units. Have.

  The inertial navigation calculation unit 110 performs a known inertial navigation calculation process using the local coordinate acceleration vector output from the acceleration sensor 11 and the local coordinate angular velocity output from the gyro sensor 13, and the user in the absolute coordinate system. Position (absolute coordinate position), velocity vector (absolute coordinate velocity vector) and posture angle (absolute posture angle) are calculated. The inertial navigation calculation unit 110 corrects the local coordinate acceleration vector and the local coordinate angular velocity output from the IMU 10 using the acceleration bias and the gyro bias output from the KF processing unit 140. Then, the absolute coordinate position, the absolute coordinate velocity vector, and the absolute attitude angle are calculated using the corrected local coordinate acceleration vector and the local coordinate angular velocity, and the position error, velocity vector error, and attitude angle error output from the KF processing unit 140 are calculated. Is used to correct the absolute coordinate position, the absolute coordinate velocity vector, and the absolute attitude angle.

  The azimuth difference acquisition unit 120 uses the inertial navigation calculation direction output from the inertial navigation calculation unit 110 and the GPS calculation direction (movement direction on the absolute coordinate system: absolute direction) output from the GPS sensor 20, Obtain the azimuth difference between the absolute coordinate system and the absolute temporary coordinate system. For example, the inertial navigation calculation direction is defined on the absolute temporary coordinate system, and the GPS calculation direction is defined on the absolute coordinate system. However, in reality, the inertial navigation calculation direction and the GPS calculation direction are in the same direction. Therefore, by setting the inertial navigation calculation direction and the GPS calculation direction to the same direction, the angle between the axis direction of the absolute provisional coordinate system and the axis direction of the absolute coordinate system (which can be said to be a difference in attitude) is acquired as a direction difference. To do. The GPS calculation direction corresponds to the moving direction based on the satellite signal. Further, the azimuth difference acquisition unit 120 of the present embodiment includes a function of a movement direction calculation unit that calculates a movement direction on the absolute coordinate system.

  The correction unit 130 corrects the movement position to a position on the absolute coordinate system using the calculation start position of the inertial navigation calculation by the inertial navigation calculation unit 110 and the azimuth difference acquired by the azimuth difference acquisition unit 120.

  The KF processing unit 140 is a functional unit that performs Kalman filter processing. An error estimation calculation (hereinafter referred to as “KF error estimation calculation”) is performed using the inertial navigation calculation results (absolute coordinate position, absolute coordinate velocity vector, and absolute attitude angle) output from the inertial navigation calculation unit 110 as control inputs. Thus, an error included in the inertial navigation calculation result (hereinafter referred to as “inertial navigation calculation error”) is estimated. Further, the estimated inertial navigation calculation error is fed back to the inertial navigation calculation unit 110. The KF processing unit 140 is a filter that performs a predetermined filter process on the movement position calculated by the inertial navigation calculation unit 110 before the correction of the coordinate system by the correction unit 130 and on the movement position after the correction of the coordinate system. It corresponds to a processing unit.

  FIG. 4 is an explanatory diagram of the principle of the azimuth difference acquisition by the azimuth difference acquisition unit 120 and the coordinate system correction by the correction unit 130. The position calculation device 1 starts the inertial navigation calculation in a state where the absolute orientation of the user cannot be recognized. That is, the coordinate system calculated by the inertial navigation calculation unit 110 is a temporary absolute coordinate system (absolute temporary coordinate system). The inertial navigation calculation unit 110 calculates the position, velocity vector, and posture of the user using this absolute temporary coordinate system. FIG. 4 illustrates a case where the inertial navigation calculation is started at time t1 and the inertial navigation calculation is performed until time t6.

  It is assumed that absolute azimuth information can be acquired from the GPS sensor 20 at time t6. In this case, the azimuth difference acquisition unit 120 calculates the average moving azimuth by averaging the relative moving azimuths (moving directions) of the users obtained from the velocity vectors calculated from time t1 to t6. Then, an azimuth difference θ between the average moving azimuth and the GPS calculation azimuth obtained from the GPS sensor 20 is calculated. The correction unit 130 uses the azimuth difference θ acquired by the azimuth difference acquisition unit 120 to correct the absolute temporary coordinate system calculated by the inertial navigation calculation unit 110 to an absolute coordinate system. That is, the absolute temporary coordinate system is rotated using the azimuth difference θ. As a result, the coordinate system calculated by the inertial navigation calculation unit 110 is an absolute coordinate system. Thereby, the inertial navigation calculation unit 110 can correct the user's movement position to a position on the absolute coordinate system using the calculation start position of the inertial navigation calculation and the azimuth difference.

  Returning to FIG. 2, the operation unit 200 includes an input device such as a touch panel or a button switch, and outputs an operation signal corresponding to the performed operation to the processing unit 100. By operating the operation unit 200, various instructions such as a position calculation instruction are input. The operation unit 200 corresponds to the operation button 3 in FIG.

  The display unit 300 includes a display device such as an LCD (Liquid Crystal Display), for example, and performs various displays based on a display signal input from the processing unit 100. The display unit 300 displays information such as the calculated position. The display unit 300 corresponds to the liquid crystal display 5 of FIG.

The sound output unit 400 includes a sound output device such as a speaker, for example, and performs various sound outputs based on the audio signal input from the processing unit 100. The sound output unit 400 corresponds to the speaker 7 in FIG.
The communication unit 500 is a communication device for transmitting / receiving information used inside the device, calculated results, and the like to / from an external information communication device.

  The storage unit 600 is realized by a storage device such as a ROM (Read Only Memory), a flash ROM, or a RAM (Random Access Memory), for example, and a system program for the processing unit 100 to centrally control the position calculation device 1; Various programs and data for executing various application processes are stored.

  The storage unit 600 stores a position calculation program 610 that is executed as a position calculation process (see FIG. 5). The position calculation program 610 includes a KF processing program 611 executed as KF processing (see FIG. 5) as a subroutine. The storage unit 600 stores IMU detection data 630, GPS detection data 640, direction difference data 650, inertial navigation calculation data 660, and inertial navigation calculation error data 670.

The IMU detection data 630 stores the local coordinate acceleration vector and local coordinate angular velocity detected by the IMU 10 in time series.
The GPS detection data 640 stores GPS calculation results calculated by the GPS sensor 20 in time series.
The azimuth difference data 650 stores the azimuth difference acquired by the azimuth difference acquisition unit 120.
The inertial navigation calculation data 660 is data acquired by the inertial navigation calculation unit 110 performing the inertial navigation calculation, and includes various amounts such as a position, a velocity vector, and an attitude angle.
Inertial navigation calculation error data 670 is data obtained by the KF processing unit 140 performing KF processing, and includes various amounts such as position error, velocity vector error, attitude angle error, acceleration bias, and gyro bias. .

3. Processing Flow FIG. 5 is a flowchart showing a flow of position calculation processing executed by the processing unit 100 of the position calculation device 1 according to the position calculation program 610 stored in the storage unit 600.

  First, the processing unit 100 starts obtaining detection data from the IMU 10 and the GPS sensor 20, and stores them in the storage unit 600 as IMU detection data 630 and GPS detection data 640 (step A1). Thereafter, the inertial navigation calculation unit 110 starts the inertial navigation calculation process (step A3), and stores the inertial navigation calculation data 660 including the calculation start position of the inertial navigation calculation in the storage unit 600 (step A5).

  Next, the processing unit 100 determines whether the user is in a straight traveling state (step A7). The determination as to whether or not the vehicle is in a straight traveling state can be made from the calculation result of the inertial navigation calculation unit 110, for example. For example, assuming that the state in which the yaw angle is within a predetermined approximate range (for example, within ± 3 degrees) continues for a predetermined time (for example, 3 seconds) as a straight-running state determination condition, the straight-running state is satisfied if this straight-running state determination condition is satisfied. Is determined. Whether or not it has continued for a predetermined time is determined by determining whether or not the moving direction is stable (whether or not it continues to move in the same or substantially the same direction), and further, by acquiring a heading difference at the stable time It leads to things. When the moving direction is not stable, a change in the moving direction becomes an error, and it becomes impossible to appropriately calculate a azimuth difference described later. Therefore, it is important to include in the straight-running state determination condition that it has continued for a predetermined time.

  If it is determined that the vehicle is not in the straight traveling state (step A7; No), the processing unit 100 returns to step A7. On the other hand, if it is determined that the vehicle is in the straight traveling state (step A7; Yes), the processing unit 100 determines whether or not the GPS calculation result has been acquired from the GPS sensor 20 at the predetermined time of step A7 (step A9).

  If it determines with a GPS calculation result not being able to be acquired (step A9; No), the process part 100 will return to step A7. On the other hand, if it is determined that the GPS calculation result has been acquired (step A9; Yes), the azimuth difference acquisition unit 120 performs an azimuth difference acquisition process (step A11). Specifically, the average moving azimuth is calculated by averaging the moving azimuths calculated by the inertial navigation calculation unit 110 when the vehicle is in the straight traveling state. Then, the azimuth difference θ between the calculated average moving azimuth and the absolute azimuth obtained from the GPS sensor 20 is calculated and stored in the storage unit 600 as the azimuth difference data 650. This is equivalent to averaging the moving direction at the stable time and obtaining the azimuth difference using the averaged moving direction. In addition, the inertial navigation calculation unit 110 of the present embodiment includes a function of a movement direction calculation unit that calculates a movement direction at a stable time.

Next, the correction unit 130 performs a coordinate system correction process of correcting the coordinate system using the azimuth difference acquired by the azimuth difference acquisition unit 120 (step A13). At this time, the moving position is corrected by rotating a position vector connecting the calculation start position of the inertial navigation calculation and the current inertial navigation calculation position using the azimuth difference. In addition, the velocity vector is corrected by rotating the velocity vector using the azimuth difference, and the posture angle of the user is corrected by using the converted moving position. Note that the arithmetic expressions relating to these corrections are known per se. Here Arfken is, G. "Mathematical Methods for Physicists, 3 rd ed." Orland, FL: Academic Press, 1985, quoted p.195, to be incorporated herein.

但し、「δＰ」、「δＶ」及び「δＡ」は、それぞれ慣性航法演算部１１０の演算結果に含まれる位置誤差、速度ベクトル誤差及び姿勢角誤差である。また、「ｂ_ａ」及び「ｂ_ｗ」は、それぞれ加速度バイアス及びジャイロバイアスである。 Next, the KF processing unit 140 performs KF processing according to the KF processing program 611 stored in the storage unit 600. In the KF process, the KF processing unit 140 performs a KF error estimation calculation by setting a state vector X as shown in the following equation (1), for example.

However, “δP”, “δV”, and “δA” are a position error, a velocity vector error, and an attitude angle error included in the calculation result of the inertial navigation calculation unit 110, respectively. “B _a ” and “b _w ” are an acceleration bias and a gyro bias, respectively.

  Further, in the KF error estimation calculation, calculation is performed by setting an error covariance matrix P (a mathematical expression is omitted) having the error covariance included in each component of the state vector X as a component.

First, the KF processing unit 140 performs a prediction calculation for predicting the state vector X and the error covariance matrix P according to a predetermined calculation formula based on the theory of the Kalman filter (step A15).
Next, the KF processing unit 140 determines the current movement status of the user (step A17). If the determination result of the movement status is moving (step A17; moving), the GPS sensor 20 calculates the GPS calculation speed V. Is acquired (step A19). Then, the KF processing unit 140 uses the GPS calculation speed V to set a moving observation vector Z _move as shown in the following equation (2), for example (step A21).

The moving observation vector Z _move is an observation vector set based on the assumption that the velocity component in the vertical and horizontal directions (vertical direction and horizontal direction) of the user is zero. That is, it is assumed that the velocity component in the Q-axis direction and the P-axis direction of the moving object coordinate system (V frame) is zero, and a velocity component corresponding to the velocity V is generated in the R-axis direction that is the user's forward direction. .

In this case, the KF processing unit 140 performs a correction calculation of the Kalman filter process using the observation vector Z _move set in step A21, and obtains the state vector X and the error covariance matrix P predicted by the prediction calculation. Correction is performed (step A23). The correction calculation in this case is performed in the moving body coordinate system. The conversion from the absolute coordinate system to the moving body coordinate system is determined by calculating the attitude (sensor attitude) of the IMU 10 with respect to the user using a known method and using the absolute attitude angle obtained from the detection result of the gyro sensor 13. This can be realized by coordinate conversion using a coordinate conversion matrix and a coordinate conversion matrix determined using the sensor attitude angle.

If it is determined in step A17 that the moving state is stopped (step 17; stopped), the KF processing unit 140 sets a stop-time observation vector (step A25). Specifically, using the absolute coordinate velocity vector v ^A = (v ^A _X , v ^A _Y , v ^A _Z ) ^T calculated by the inertial navigation calculation unit 110, for example, at the time of stop as shown in the following equation (3) Set the observation vector Z _stop .

The stop observation vector Z _stop is an observation vector set based on the assumption that the velocity component of the user is zero. That is, in the stopped state, it is assumed that the velocity component in each axis direction in the absolute coordinate system (A frame) becomes zero. In this case, the KF processing unit 140 performs a correction operation based on the Kalman filter theory to which the stop-time observation vector Z _stop set in step A25 is applied, and the state vector X and the error covariance matrix P predicted by the prediction operation are performed. Is corrected (step A27). The inertial navigation calculation error obtained by the correction calculation in step A23 or A27 is stored in the storage unit 600 as the inertial navigation calculation error data 670. Then, the KF processing unit 140 ends the KF processing.

  After the KF process, the inertial navigation calculation unit 110 corrects the inertial navigation calculation result using the inertial navigation calculation error calculated in the KF process (step A29). Next, the inertial navigation calculation unit 110 determines whether or not it is the position output timing (step A31), and if it is determined that it is the output timing (step A31; Yes), outputs the latest inertial navigation calculation result. (Step A33). If it is determined that it is not the output timing (step A31; No), the process proceeds to step A35.

  The processing unit 100 determines whether or not to end the process (step A35). If it is determined to continue the process (step A35; No), the processing unit 100 returns to step A15. On the other hand, if it determines with complete | finishing a process (step A35; Yes), the process part 100 will complete | finish a position calculation process.

4). Experimental Results FIG. 6 is a diagram showing the results of an experiment for calculating the position when the subject wearing the position calculating device 1 moves along a predetermined route (upper right direction from the starting point SP in FIG. 6). . In FIG. 6, the horizontal axis indicates longitude, and the vertical axis indicates latitude. In spite of moving from the start point SP in the upper right direction, the position of the present embodiment is a trajectory that has temporarily moved downward from the start point SP. This is because the calculation is performed in the absolute temporary coordinate system until the GPS calculation direction is acquired. If the GPS calculation direction can be acquired, it is corrected to the correct position, and thereafter, an appropriate position moved in the upper right direction can be calculated. Of course, it is possible to correct all positions calculated until the GPS calculation direction is obtained by correcting the positions.

  FIG. 7 is a diagram showing a time change of an error estimated value (hereinafter referred to as “estimated error value”) expressed as an error covariance of a position error and a speed error when KF processing is performed in the above experiment. is there. FIG. 7 (1) shows the time change of the estimated error value when the conventional method is used, and FIG. 7 (2) shows the time change of the estimated error value when the method of this embodiment is used. Yes. In each figure, the horizontal axis is time, and the vertical axis is an estimation error value. The time t is the time when the GPS calculation direction (absolute direction) is acquired from the GPS sensor 20.

  In the conventional method, since the coordinate system is corrected at time t, the estimated error value is reset (returned to the initial value). For this reason, the KF process that has become stable at the corner is initialized, the accuracy returns to an unstable state, and it takes time until the KF process is stabilized again.

  On the other hand, when the method of the present embodiment is used, it can be seen that the estimation error value is gradually reset to zero without being reset even at time t. This is because the KF process can be performed by directly inheriting the result of the KF process before the coordinate system is corrected. In this embodiment, even if the coordinate system is an absolute temporary coordinate system or an absolute coordinate system, the calculated position is only a relative position from the reference point of the coordinate system. The principle is that the KF process can continue even if the system is modified. According to this embodiment, the KF process can be continued while maintaining the accuracy before the coordinate system is corrected. Therefore, according to the filter processing of the present embodiment, the error estimation calculation can be performed by inheriting the estimated error value before and after the correction of the coordinate system.

5. Action and Effect In the position calculation device 1, the inertial navigation calculation unit 110 calculates the movement position on the absolute temporary coordinate system in which the absolute coordinate system is temporarily set by inertial navigation calculation. Then, the azimuth difference acquisition unit 120 uses the inertial navigation calculation azimuth calculated by the inertial navigation calculation unit 110 and the absolute azimuth output from the GPS sensor 20 to calculate the azimuth difference between the absolute coordinate system and the absolute temporary coordinate system. get. Then, the correction unit 130 corrects the movement position to a position on the absolute coordinate system using the calculation start position of the inertial navigation calculation and the azimuth difference. Therefore, the inertial navigation calculation can be started even when the azimuth of the absolute coordinate system is unknown. Also, the position on the absolute temporary coordinate system calculated with the absolute coordinate system unknown is later corrected to the correct position on the absolute coordinate system using the azimuth difference between the absolute coordinate system and the absolute temporary coordinate system. The For this reason, it is not necessary to wait for the start of the inertial navigation calculation until the direction of the absolute coordinate system is known.

  Further, the KF processing unit 140 performs predetermined filter processing on the movement position calculated by the inertial navigation calculation unit 110 before correction by the correction unit 130 and on the movement position after correction after correction. More specifically, the KF processing unit 140 performs a Kalman filter process including performing an error estimation calculation by inheriting the estimated error value before and after correction by the correction unit 130. As a result, it is possible to appropriately calculate the position of the user while maintaining stable accuracy without resetting the estimated error value.

6). Modifications Embodiments to which the present invention can be applied are not limited to the above-described embodiments, and can of course be changed as appropriate without departing from the spirit of the present invention. Hereinafter, modified examples will be described. In addition, about the structure same as the said embodiment, the same code | symbol is attached | subjected and description for the second time is abbreviate | omitted.

6-1. In the above embodiment, the sensor (position calculation device 1) is attached to the user as a waist, but it may be attached to a part other than the waist. A suitable wearing part is a user's trunk (parts other than limbs). However, it is not limited to the trunk, and may be worn on the user's head or feet other than the arms. Further, the position calculation device 1 may be attached to, for example, a bicycle or a car instead of a human.

6-2. Filter Processing In the above embodiment, the filter processing applied to the user's moving position has been described as Kalman filter processing, but the filter processing is not limited to this. Instead of the Kalman filter, for example, a filter process using a filter such as a particle filter or an H∞ (H Infinity) filter may be applied to the user's moving position.

6-3. Acquiring the azimuth difference In the above embodiment, it has been described that the azimuth difference is acquired using the absolute azimuth acquired from the GPS sensor 20. However, for example, as shown in FIG. The azimuth difference acquisition unit 120 may acquire the azimuth difference using the measurement azimuth measured in step (1).

6-4. Satellite positioning system Moreover, in said embodiment, the satellite positioning system used in order to acquire an absolute azimuth | direction was demonstrated as GPS. However, it goes without saying that satellite positioning systems such as WAAS (Wide Area Augmentation System), GLONASS (GLObal NAvigation Satellite System), GALILEO, and Beidou may be used.

6-5. Straight running state determination In the above-described embodiment, the state where the yaw angle is in a predetermined approximate range continues for a predetermined time has been described as the straight traveling state determination condition. However, the straight traveling state determination condition is not limited to this. Instead of the yaw angle, a change in the GPS calculation direction or a change in the movement direction obtained from the position calculated by the GPS sensor may be used. When a GPS sensor is used for straight-running state determination, determination of whether or not a GPS calculation result has been acquired may be omitted. Moreover, you may use the well-known method other than the above for straight-ahead state determination.

6-6. Average Moving Direction In the above embodiment, the average moving direction is calculated by averaging the relative moving direction of the user obtained from the velocity vector calculated by the inertial navigation calculation unit 110, but the yaw angle may be averaged.

  1 position calculation device, 3 operation buttons, 5 liquid crystal display, 7 speaker, 10 IMU, 11 acceleration sensor, 13 gyro sensor, 20 GPS sensor, 30 compass, 100 processing unit, 200 operation unit, 300 display unit, 400 sound output Part, 500 communication part, 600 storage part

Claims (12)

Calculating the moving position of the moving body by inertial navigation calculation;
Calculating a moving direction of the moving body;
Using the calculated moving direction to obtain an azimuth difference with an absolute coordinate system;
Using the calculation start position of the inertial navigation calculation and the azimuth difference to correct the movement position;
Only including,
Before the correcting step, the calculated movement position is
After the correcting step, the corrected position is
Perform the predetermined filter processing,
Position calculation method. In claim 1 ,
The filtering process is
It is a Kalman filter process for performing an error estimation calculation by inheriting an estimated error value before and after the correcting step.
Position calculation method. In claim 1 or 2 ,
The step of calculating the moving direction calculates a moving direction on the absolute coordinate system based on a satellite signal from a positioning satellite;
The step of obtaining the azimuth difference obtains the azimuth difference using a moving direction based on the satellite signal.
Position calculation method. In claim 1 or 2 ,
The step of obtaining the azimuth difference is to obtain the azimuth difference using a measurement azimuth measured by an azimuth meter.
Position calculation method. In any one of Claims 1-4 ,
Determining a stable time when the moving direction is stable,
The step of acquiring the heading difference acquires the heading difference at the stable time.
Position calculation method. In claim 5 ,
The step of obtaining the orientation difference includes
Average the direction of movement in the stable period,
Obtaining the orientation difference using the averaged moving direction;
Position calculation method. A step of calculating the moving position of the moving body on the absolute temporary coordinate system with the absolute coordinate system temporarily set by inertial navigation calculation;
Calculating a moving direction of the moving body;
A step of using the moving direction the calculated, to obtain the orientation difference between the absolute coordinate system,
Using the calculation start position of the inertial navigation calculation and the azimuth difference to correct the movement position on the absolute temporary coordinate system to a position on the absolute coordinate system;
Position calculation method including In any one of Claims 1-6 ,
The step of calculating by the inertial navigation calculation calculates the moving position of the moving body on the absolute temporary coordinate system temporarily setting the absolute coordinate system by the inertial navigation calculation,
The correcting step corrects the moving position on the absolute temporary coordinate system to a position on the absolute coordinate system.
Position calculation method. An inertial navigation calculation unit for calculating the moving position of the moving object by inertial navigation calculation;
A moving direction calculating unit for calculating a moving direction of the moving body;
Using the calculated moving direction, an azimuth difference acquisition unit that acquires an azimuth difference with an absolute coordinate system ;
A correction unit that corrects the movement position using the calculation start position of the inertial navigation calculation and the heading difference;
Only including, prior to correction by the correction section in the movement position calculated by the inertial navigation operation unit, after the correction in the movement position corrected by said correction unit, the predetermined filtering process, Position calculation device. An inertial navigation calculation unit for calculating the moving position of the moving object on the absolute temporary coordinate system, which is temporarily set as the absolute coordinate system, by inertial navigation calculation;
A moving direction calculating unit for calculating a moving direction of the moving body;
Using the calculated movement direction, an azimuth difference acquisition unit that acquires an azimuth difference with the absolute coordinate system;
A correction unit that corrects the movement position on the absolute temporary coordinate system to a position on the absolute coordinate system using the calculation start position of the inertial navigation calculation and the heading difference;
A position calculation device including: Calculate the moving position of the moving object by inertial navigation calculation,
Calculating the moving direction of the moving body;
Using the calculated moving direction, obtain an azimuth difference with the absolute coordinate system ,
Using the calculation start position of the inertial navigation calculation and the heading difference, the movement position is corrected,
A predetermined filtering process is performed on the calculated movement position before the correction and on the corrected movement position after the correction.
Position calculation device. Calculate the moving position of the moving object on the absolute temporary coordinate system with the absolute coordinate system temporarily set by inertial navigation calculation,
Calculating the moving direction of the moving body;
Using the calculated moving direction, obtain an azimuth difference with the absolute coordinate system,
Using the calculation start position of the inertial navigation calculation and the heading difference, the moving position on the absolute temporary coordinate system is corrected to a position on the absolute coordinate system .
Position calculation device.

Images