Position and posture calibration device, position and posture calibration method, and position and posture calibration program

Abstract

A position / attitude calibration device (100) comprises an initial value calculation unit (110) and a calibration unit (120). The initial value calculation unit (110) calculates a correction value for correcting the azimuth angle by using a first Kalman filter (131) on the basis of the angular velocity of the azimuth angle measured by an angular velocity sensor (42), the velocity and angular velocity measured by a millimeter wave radar (50), and two-dimensional coordinate values included in coordinate values measured by a GNSS receiver (30), and calculates the initial azimuth angle value on the basis of the calculated correction value. The calibration unit (120) calculates a correction value for correcting the position of a moving body by using a second Kalman filter (132) on the basis of the angular velocity of the azimuth angle measured by the angular velocity sensor (42), the initial azimuth angle value, the three-dimensional acceleration measured by an acceleration sensor (41), the initial three-dimensional position value, and three-dimensional coordinate values measured by the GNSS receiver (30), and calibrates the position of the moving body on the basis of the calculated correction value.

Description

[Technical Field]

[0001] The present disclosure relates to a position and attitude location device, a position and attitude location method, and a position and attitude location program. [Background technology]

[0002] Hybrid navigation systems that utilize the Global Positioning System (GPS) and the Inertial Navigation System (INS) to determine position by compensating for the advantages and disadvantages of both systems are employed in aircraft, ships, and vehicle systems. To maintain relatively high accuracy during periods of satellite blindness that last for relatively long periods, a relatively high-precision Inertial Measurement System (IMU) is required. Due to their relatively high cost, IMUs have only been adopted in limited fields, such as the aviation and space fields. Therefore, in order to realize a relatively inexpensive and highly accurate hybrid navigation system, it is conceivable to determine the position, attitude, and direction of a vehicle using multiple GNSS (Global Navigation Satellite System) antennas and vehicle speed pulses. However, there are limitations, such as the need to widen the spacing between GNSS antennas and to attach sensors to the wheels to acquire vehicle speed pulses. The reasons for these constraints are as follows: (1) When determining the direction of a vehicle while it is stopped, using an IMU with relatively low accuracy makes it impossible to measure the rotation of the Earth and perform calculations to determine the direction. (2) In three-dimensional navigation calculations, if initial values ​​(position, attitude, and heading) are not set with a certain degree of accuracy, the calculation results may diverge. Regarding the initial values, the attitude angles (pitch angle and roll angle) corresponding to the attitude can be obtained by gravity observation, and the position can be obtained by GNSS. However, for the heading, an appropriate value cannot be obtained due to the reason in (1). (3) Even if an accurate initial value is obtained by some method, the heading value will drift due to gyro errors when the vehicle is stopped or otherwise stopped.

[0003] Furthermore, it is expected that relatively inexpensive MEMS (Micro Electro Mechanical Systems) IMUs will become more accurate and smaller in size, while the field of view of millimeter-wave radars will become wider and the price of millimeter-wave radars will fall. As a result, it is expected that the application of 3D navigation will expand beyond vehicles to include ships, drones, construction machinery, agricultural machinery, and other equipment that cannot acquire vehicle speed pulses. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Japanese Patent Application Laid-Open No. 2012-193965 Summary of the Invention [Problem to be solved by the invention]

[0005] Millimeter-wave radar can measure the relative velocity (Doppler velocity) of objects with relatively high reflectivity with relatively high accuracy in all weather conditions. However, millimeter-wave radar has a weakness in that its spatial resolution (angular accuracy of Doppler velocity measurement) is relatively low. On the other hand, GNSS / INS integrated navigation systems can detect position with relatively high accuracy in environments where the sky is open and satellites are visible. However, GNSS / INS integrated navigation systems have weaknesses such as being unable to perform positioning under bridges or near buildings, or having large INS positioning errors.

[0006] The present disclosure aims to locate the position of a mobile body with relatively high accuracy, the mobile body being equipped with a relatively low-accuracy IMU, one GNSS antenna, and a millimeter-wave radar, and unable to acquire vehicle speed pulses. [Means for solving the problem]

[0007] A position and attitude localization device according to the present disclosure includes: A position and attitude location device for locating the position of a moving object, which is equipped with a GNSS receiver that performs positioning using a GNSS (Global Navigation Satellite System), a millimeter wave radar that measures velocity and angular velocity, an acceleration sensor that measures acceleration, and an angular velocity sensor that measures the angular velocity of an azimuth angle, an initial value calculation unit that calculates an azimuth angle of the moving object as a first navigation azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculates a change amount in two-dimensional coordinates by integrating the velocity and angular velocity measured by the millimeter-wave radar based on the calculated first navigation azimuth angle, calculates two-dimensional coordinate values ​​of the moving object as first navigation coordinate values ​​by adding the calculated change amount to two-dimensional position initial values, calculates a correction value for correcting the first navigation azimuth angle using a first Kalman filter based on the calculated first navigation coordinate values ​​and the two-dimensional coordinate values ​​included in the coordinate values ​​measured by the GNSS receiver, corrects the first navigation azimuth angle using the calculated correction value, and outputs the corrected first navigation azimuth angle as an azimuth angle initial value; a location unit that calculates an amount of change in the azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculates the azimuth angle of the moving body as a second navigation azimuth angle based on the calculated amount of change in the azimuth angle and the output initial azimuth angle value, converts the three-dimensional acceleration measured by the acceleration sensor into a three-dimensional acceleration of a navigation coordinate system based on the calculated second navigation azimuth angle, calculates an amount of change in the three-dimensional coordinates by integrating the converted three-dimensional acceleration, calculates three-dimensional coordinate values ​​of the moving body as second navigation coordinate values ​​by adding the calculated amount of change to the three-dimensional position initial value, calculates correction values ​​for correcting the second navigation coordinate values ​​using a second Kalman filter based on the calculated second navigation coordinate values ​​and the three-dimensional coordinate values ​​measured by the GNSS receiver, corrects the second navigation coordinate values ​​using the calculated correction values, and outputs the corrected second navigation coordinate values ​​as position location values ​​for positioning the moving body; Equipped with. [Effects of the Invention]

[0008] According to the present disclosure, the position and azimuth angle are corrected based on the velocity and angular velocity measured by the millimeter-wave radar, and the position of a mobile body to which a GNSS receiver, a millimeter-wave radar, an acceleration sensor, and an angular velocity sensor are attached is corrected based on the corrected position and azimuth angle. Therefore, according to the present disclosure, the position of a mobile body that is equipped with a relatively low-accuracy IMU, one GNSS antenna, and a millimeter-wave radar and is unable to acquire vehicle speed pulses can be located with relatively high accuracy. [Brief explanation of the drawings]

[0009] [Figure 1] 1 is a conceptual diagram of a moving body according to a first embodiment. [Figure 2] FIG. 1 is a diagram showing an example of the configuration of a position and attitude localization system 90 according to the first embodiment. [Figure 3] FIG. 2 is a diagram showing an example of the functional configuration of a position and attitude localization system 90 according to the first embodiment. [Figure 4] A diagram explaining the coordinate system. [Figure 5] FIG. 1 is a diagram showing an example of the hardware configuration of a position and attitude localization device 100 according to a first embodiment. [Figure 6] 3 is a flowchart showing the operation of the position and attitude localization device 100 according to the first embodiment. [Figure 7] FIG. 2 is a diagram for explaining an algorithm for three-degree-of-freedom two-dimensional navigation according to the first embodiment. [Figure 8] FIG. 2 is a diagram for explaining an algorithm for six-degree-of-freedom three-dimensional navigation according to the first embodiment. [Figure 9] 1 is a diagram showing an example of the configuration of a millimeter wave radar 50 according to a first embodiment. [Figure 10] FIG. 4 is a diagram for explaining the processing of a speed profile unit 57 according to the first embodiment. [Figure 11] 5 is a diagram for explaining the processing of a radar / body coordinate conversion unit 59 according to the first embodiment. FIG. [Figure 12] FIG. 10 is a diagram showing an example of the functional configuration of a position and attitude localization system 90 according to a modification of the first embodiment. [Figure 13]FIG. 10 is a diagram showing an example of the hardware configuration of a position and attitude localization device 100 according to a modification of the first embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0010] In the description of the embodiments and the drawings, the same elements and corresponding elements are given the same reference numerals. The description of elements given the same reference numerals will be omitted or simplified as appropriate. Arrows in the drawings mainly indicate the flow of data or the flow of processing. Furthermore, "unit" may be read as "circuit," "step," "procedure," "process," or "circuitry" as appropriate.

[0011] Embodiment 1 Hereinafter, this embodiment will be described in detail with reference to the drawings.

[0012] ***Configuration Description*** 1 is a conceptual diagram of a moving body 20 according to the first embodiment. Specific examples of the moving body 20 include an automobile, a train, or a ship. The moving body 20 includes a Global Navigation Satellite System (GNSS) receiver 30, an Inertial Measurement Unit (IMU) 40, a millimeter-wave radar 50, and a position and attitude determining device 100.

[0013] 2 is a schematic diagram of a position and attitude location system 90 according to Embodiment 1. The position and attitude location system 90 includes a GNSS receiver 30, an IMU 40, a millimeter-wave radar 50, and a position and attitude location device 100.

[0014] The GNSS receiver 30 is a device that performs positioning using the GNSS, observes positioning signals transmitted from satellites using an antenna, and determines a position based on the observation results. A specific example of the GNSS receiver 30 is a GPS (Global Positioning System) receiver.

[0015] The IMU 40, also known as an inertial sensor, measures acceleration in three axes and angular velocity in three axes. The three axes refer to the length, width, and height directions of the moving object. Hereinafter, the acceleration in the three axes will be referred to as "three-dimensional acceleration," and the angular velocity in the three axes will be referred to as "three-dimensional angular velocity."

[0016] The millimeter-wave radar 50 is, for example, a radar that employs a frequency-continuous modulation wave (FMCW) method in the 76 to 77 GHz band or the 77 to 81 GHz band, and is capable of measuring distance, velocity, and angular velocity. The millimeter-wave radar 50 is also called a millimeter-wave sensor.

[0017] The position and attitude localization device 100 locates the position of a moving object 20 to which a GNSS receiver 30, a millimeter-wave radar 50, an acceleration sensor 41, and an angular velocity sensor 42 are attached. The position and attitude localization device 100 locates the position, attitude, and orientation of the moving object using data acquired from the GNSS receiver 30, data acquired from the IMU 40, and data acquired from the millimeter-wave radar 50. The position and attitude localization device 100 is also called a self-position and attitude localization device.

[0018] 3 shows an example of the functional configuration of a position and attitude localization system 90 according to embodiment 1. As shown in FIG. 3, the position and attitude localization device 100 includes an initial value calculation unit 110, a localization unit 120, a Kalman filter unit 130, a strapdown calculation unit 140, and a storage unit 190.

[0019] The IMU 40 includes an acceleration sensor 41 and an angular velocity sensor 42 . The acceleration sensor 41 measures the acceleration. The angular velocity sensor 42 measures the angular velocity of the azimuth angle.

[0020] The initial value calculation unit 110 controls the Kalman filter unit 130 to calculate the initial value of the azimuth angle. Specifically, first, the initial value calculation unit 110 calculates the azimuth angle of the moving object 20 as a first navigation azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor 42. Next, the initial value calculation unit 110 calculates a change amount in two-dimensional coordinates by integrating the velocity and angular velocity measured by the millimeter-wave radar 50 based on the calculated first navigation azimuth angle. Next, the initial value calculation unit 110 calculates a two-dimensional coordinate value of the moving object 20 as a first navigation coordinate value by adding the calculated change amount to a two-dimensional position initial value. The two-dimensional position initial value is an initial position in the navigation coordinate system. Next, the initial value calculation unit 110 calculates a correction value for correcting the first navigation azimuth angle using a first Kalman filter based on the calculated first navigation coordinate value and a two-dimensional coordinate value included in the coordinate value measured by the GNSS receiver 30. Next, the initial value calculation unit 110 corrects the first navigation azimuth using the calculated correction value and outputs the corrected first navigation azimuth as an initial azimuth angle value. That is, the initial value calculation unit 110 calculates an initial azimuth angle value, which is an initial value of the azimuth angle of the moving object 20, based on the measurement values ​​of the angular velocity sensor 42, the measurement values ​​of the millimeter-wave radar 50, and the positioning result of the GNSS receiver 30. More specifically, the initial value calculation unit 110 calculates the first navigation azimuth angle based on the angular velocity measured by the angular velocity sensor 42, calculates two-dimensional first navigation coordinate values ​​based on the calculated first navigation azimuth angle, the velocity and angular velocity measured by the millimeter-wave radar 50, and the two-dimensional position initial value, and corrects the first navigation azimuth angle based on the calculated two-dimensional first navigation coordinate values ​​and the positioning result of the GNSS receiver 30 to calculate the initial azimuth angle value.

[0021] Here, the coordinate system will be explained using Fig. 4. In the following, it is assumed that the moving body 20 and the IMU 40 are integrally formed as a rigid body. The initial position of the navigation coordinate system is the center position of the IMU 40 when the power is turned on. Normally, (0,0) is set as the "initial value of two-dimensional position" and (0,0,0) is set as the "initial value of three-dimensional position". 4, the X-axis, Y-axis, and Z-axis of the body coordinate system at the initial attitude angle of the moving body 20 are respectively defined as the X-axis, Y-axis, and Z-axis of the navigation coordinate system. In this case, the position of the moving body 20 in the two-dimensional navigation coordinate system is the amount of movement of the moving body 20, and is calculated by adding up the amount of movement of the moving body 20 on the X-axis and Y-axis in the body coordinate system. Furthermore, the position of the moving body 20 in the three-dimensional navigation coordinate system is the amount of movement of the moving body 20, and is calculated by adding up the amount of movement of the moving body 20 on the X-axis, Y-axis, and Z-axis in the body coordinate system. As shown in FIG. 4, the Body coordinate system is a coordinate system in which the center of the IMU 40 is the origin, the forward direction of the moving body 20 is the X-axis, the right direction relative to the forward direction of the moving body 20 is the Y-axis, and the vertical downward direction of the moving body 20 is the Z-axis.

[0022] The location unit 120 includes a running location unit 121, a stopped location unit 122, and a stop determination unit 123. The location unit 120 locates the position of the moving object 20 based on the measurement value of the angular velocity sensor 42, the initial azimuth angle value, the measurement value of the acceleration sensor 41, and the positioning result of the GNSS receiver 30. More specifically, the location unit 120 calculates the amount of change in azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor 42, calculates a second navigation azimuth angle based on the calculated amount of change in azimuth angle and the initial azimuth angle value, calculates three-dimensional second navigation coordinate values ​​based on the calculated second navigation azimuth angle, the acceleration measured by the acceleration sensor 41, and the initial three-dimensional position value, and corrects the second navigation coordinate values ​​based on the calculated three-dimensional second navigation coordinate values ​​and the positioning result of the GNSS receiver 30, thereby locating the position of the moving object 20. The initial three-dimensional position value is the initial position of the navigation coordinate system. The traveling orientation unit 121 controls the Kalman filter unit 130 and the strapdown calculation unit 140 while the moving body 20 is traveling to orient (calculate) the position, attitude, and direction of the moving body 20. The traveling orientation unit 121 is also called an orientation value calculation unit. The stationary state locating unit 122 controls the Kalman filter unit 130 and the strapdown calculation unit 140 when the moving body 20 is stationary to locate the position, attitude, and direction of the moving body 20. The stationary state locating unit 122 is also called an azimuth angle correction unit. The stoppage determination unit 123 determines whether the moving object 20 has stopped or started moving.

[0023] Specifically, the orientation unit 120 first calculates the amount of change in the azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor 42, and then calculates the azimuth angle of the moving object 20 as a second navigation azimuth angle based on the calculated amount of change in the azimuth angle and the output initial azimuth angle value. Next, the orientation unit 120 converts the three-dimensional acceleration measured by the acceleration sensor 41 into a three-dimensional acceleration in the navigation coordinate system based on the calculated second navigation azimuth angle. Next, the orientation unit 120 integrates the converted three-dimensional acceleration to calculate the amount of change in the three-dimensional coordinates, and then calculates the three-dimensional coordinate values ​​of the moving object 20 as second navigation coordinate values ​​by adding the calculated amount of change to the three-dimensional position initial value. Next, the orientation unit 120 calculates correction values ​​for correcting the second navigation coordinate values ​​using a second Kalman filter based on the calculated second navigation coordinate values ​​and the three-dimensional coordinate values ​​measured by the GNSS receiver 30. Next, the orientation unit 120 corrects the second navigation coordinate values ​​using the calculated correction values, and outputs the corrected second navigation coordinate values ​​as position orientation values ​​that have positioned the position of the moving body 20 . Furthermore, when the moving object 20 stops moving, the location unit 120 uses a third Kalman filter to calculate an azimuth angle correction value for correcting the currently calculated second navigation azimuth angle based on the previously calculated second navigation azimuth angle and the currently calculated second navigation azimuth angle, and then corrects the currently calculated second navigation azimuth angle using the calculated azimuth angle correction value to calculate a corrected second navigation azimuth angle. Here, the third Kalman filter calculates an azimuth angle correction value that matches the previously calculated second navigation azimuth angle with the currently calculated second navigation azimuth angle. The third Kalman filter generates an observation equation that represents the difference between the previously calculated second navigation azimuth angle and the currently calculated second navigation azimuth angle, and calculates the azimuth angle correction value using the generated observation equation. Furthermore, when the stopped moving object 20 resumes movement, the orientation unit 120 first calculates a new second navigation azimuth based on the corrected second navigation azimuth and the amount of change in the new azimuth angle, and then converts the new three-dimensional acceleration measured by the acceleration sensor 41 into a three-dimensional acceleration in the navigation coordinate system based on the calculated new second navigation azimuth. Next, the orientation unit 120 integrates the converted three-dimensional acceleration to calculate the amount of change in the three-dimensional coordinates, and calculates new second navigation coordinate values ​​by adding the calculated amount of change to the previously calculated second navigation coordinate values. Next, the orientation unit 120 uses a second Kalman filter to calculate position correction values ​​that correct the new second navigation coordinate values ​​based on the calculated new second navigation coordinate values ​​and new three-dimensional coordinate values ​​measured by the GNSS receiver 30, corrects the new second navigation coordinate values ​​using the calculated position correction value, and outputs the corrected new second navigation coordinate values ​​as new position orientation values. The location unit 120 determines that the moving body 20 has stopped moving if the speed measured by the millimeter-wave radar 50 is less than a predetermined speed threshold. After determining that the moving body 20 has stopped moving, the location unit 120 determines that the moving body 20 has resumed moving if the speed measured by the millimeter-wave radar 50 is equal to or greater than the speed threshold.

[0024] The Kalman filter unit 130 generates at least one of an observation equation and a state equation, and calculates correction values ​​for various data using the generated at least one of the observation equation and the state equation. The Kalman filter unit 130 includes a first Kalman filter 131, a second Kalman filter 132, and a third Kalman filter 133 as a plurality of Kalman filters having different observation equations.

[0025] The strapdown calculation unit 140 calculates the velocity, position, attitude, and direction based on the acceleration data 193 and the angular velocity data 194. A specific example of the strapdown calculation unit 140 is described in Patent Document 1.

[0026] The storage unit 190 stores GNSS data 191 , velocity and angular velocity data 192 , acceleration data 193 , and angular velocity data 194 . The GNSS data 191 consists of data observed by the GNSS receiver 30 . The velocity and angular velocity data 192 is made up of data indicating the velocity and angular velocity observed by the millimeter wave radar 50 . The acceleration data 193 is made up of data observed by the acceleration sensor 41 . Angular velocity data 194 consists of data observed by angular velocity sensor 42 .

[0027] 5 shows an example of the hardware configuration of the position and attitude localization device 100 according to this embodiment. The position and attitude localization device 100 is made up of a computer. The position and attitude localization device 100 may be made up of multiple computers.

[0028] As shown in the figure, the position and attitude localization device 100 is a computer including hardware such as a processor 11, a memory 12, an auxiliary storage device 13, an input / output IF (Interface) 14, and a communication device 15. These hardware components are appropriately connected via signal lines 19.

[0029] The processor 11 is an integrated circuit (IC) that performs arithmetic processing and controls the hardware of the computer. Specific examples of the processor 11 include a central processing unit (CPU), a digital signal processor (DSP), or a graphics processing unit (GPU). The position and attitude localization device 100 may include a plurality of processors that replace the processor 11. The plurality of processors share the role of the processor 11.

[0030] The memory 12 is typically a volatile storage device, specifically a random access memory (RAM). The memory 12 is also called a primary storage device or a main memory. Data stored in the memory 12 is saved in the secondary storage device 13 as needed.

[0031] The auxiliary storage device 13 is typically a non-volatile storage device, and specific examples thereof include a ROM (Read Only Memory), an HDD (Hard Disk Drive), or a flash memory. Data stored in the auxiliary storage device 13 is loaded into the memory 12 as needed. The memory 12 and the auxiliary storage device 13 may be integrated into one unit.

[0032] The input / output IF 14 is a port to which an input device and an output device are connected. A specific example of the input / output IF 14 is a USB (Universal Serial Bus) terminal. Specific examples of the input device are a keyboard and a mouse. A specific example of the output device is a display.

[0033] The communication device 15 is a receiver and a transmitter, and is, for example, a communication chip or a network interface card (NIC).

[0034] Each unit of the position and attitude localization device 100 may use the input / output IF 14 and the communication device 15 as appropriate when communicating with other devices.

[0035] The auxiliary storage device 13 stores a position and attitude localization program. The position and attitude localization program is a program that causes a computer to realize the functions of each unit included in the position and attitude localization device 100. The position and attitude localization program is loaded into the memory 12 and executed by the processor 11. The functions of each unit included in the position and attitude localization device 100 are realized by software.

[0036] Data used when executing the position and attitude localization program and data obtained by executing the position and attitude localization program are stored in a storage device as appropriate. Each part of the position and attitude localization device 100 uses a storage device as appropriate. Specific examples of the storage device include at least one of the memory 12, the auxiliary storage device 13, a register in the processor 11, and a cache memory in the processor 11. Note that the terms "data" and "information" may have the same meaning. The storage device may be independent of the computer. The functions of the memory 12 and the auxiliary storage device 13 may be realized by other storage devices.

[0037] The position and attitude localization program may be stored in a computer-readable non-volatile recording medium. Specific examples of the non-volatile recording medium include an optical disk and a flash memory. The position and attitude localization program may be provided as a program product.

[0038] ***Explanation of Operation*** The operation procedure of the position and attitude localization device 100 corresponds to a position and attitude localization method, and the program that realizes the operation of the position and attitude localization device 100 corresponds to a position and attitude localization program.

[0039] 6 is a flowchart showing an example of a position and attitude localization method according to Embodiment 1. The position and attitude localization method will be described with reference to FIG.

[0040] (Step S101) The initial value calculation unit 110 calculates the initial value of the azimuth angle by three-degree-of-freedom two-dimensional navigation when the moving body 20 departs.

[0041] (Step S102) The traveling orientation unit 121 orients the position, attitude, and orientation of the moving body 20 by six-degree-of-freedom three-dimensional navigation using the initial value of the azimuth angle or the most recently orientated azimuth angle until the moving body 20 stops.

[0042] (Step S103) If the moving body 20 is stationary, the position and attitude localization device 100 proceeds to step S104, otherwise, the position and attitude localization device 100 proceeds to step S102.

[0043] (Step S104) The stationary positioning unit 122 locates the position, attitude, and orientation of the mobile object 20 by six-degree-of-freedom three-dimensional navigation based on an extended ZUPT (Zero Velocity Update) algorithm while the mobile object 20 is stationary. An overview of the extended ZUPT algorithm is disclosed in Patent Document 1.

[0044] (Step S105) If the moving body 20 is stationary, the position and attitude localization device 100 proceeds to step S104, otherwise, the position and attitude localization device 100 proceeds to step S102.

[0045] 7 shows an algorithm for three-degree-of-freedom two-dimensional navigation according to the first embodiment. This algorithm is the same as the algorithm shown in FIG. 7 of Patent Document 1. FIG. 7 will be explained below. Note that the initial value calculation unit 110 includes an input correction unit 111, an orientation update unit 112, a coordinate conversion unit 113, a latitude and longitude update unit 114, and an output correction unit 115.

[0046] The input correction unit 111 corrects the velocity (velocity vector) indicated by the velocity / angular velocity data 192 by adding the correction value calculated by the Kalman filter unit 130 to the velocity indicated by the velocity / angular velocity data 192 . Furthermore, the input correction unit 111 corrects the angular velocity of the azimuth angle by adding the correction value calculated by the Kalman filter unit 130 to the angular velocity of the "azimuth angle" indicated by the angular velocity data 194.

[0047] The azimuth update unit 112 calculates the amount of change in the azimuth angle by once integrating the angular velocity of the azimuth angle corrected by the input correction unit 111. The azimuth update unit 112 calculates a new azimuth angle by adding the calculated amount of change in the azimuth angle to the initial value of the azimuth angle or the azimuth angle previously calculated by the output correction unit 115. Here, the initial value of the azimuth angle may be any appropriate value.

[0048] The coordinate conversion unit 113 converts the velocity corrected by the input correction unit 111 from a value in the sensor coordinate system to a value in the navigation coordinate system using a predetermined conversion formula based on the attitude and orientation calculated by the orientation update unit 112. This results in a two-dimensional velocity in the navigation coordinate system. The two-dimensional velocity consists of a velocity in the latitudinal direction and a velocity in the longitudinal direction.

[0049] The latitude and longitude update unit 114 calculates the amount of change in the two-dimensional coordinate by integrating once the two-dimensional velocity obtained by the coordinate conversion unit 113. The two-dimensional coordinate consists of a latitude component and a longitude component. The latitude and longitude update unit 114 calculates new two-dimensional coordinate by adding the calculated amount of change in the two-dimensional coordinate to the initial value of the two-dimensional coordinate or to the two-dimensional coordinate previously calculated by the output correction unit 115. As a specific example, the latitude and longitude update unit 114 uses the two-dimensional coordinate values ​​measured by the GNSS receiver 30 before the moving object 20 departs, that is, when the moving object 20 is stopped, as the initial values ​​of the two-dimensional coordinates.

[0050] First, the Kalman filter unit 130 calculates the difference between the two-dimensional coordinates indicated by the GNSS data 191 and the two-dimensional coordinates calculated by the latitude and longitude update unit 114 as a "coordinate residual." Next, the Kalman filter unit 130 calculates the difference between the two-dimensional velocity indicated by the GNSS data 191 and the two-dimensional velocity obtained by the coordinate conversion unit 113 as a "velocity residual." Next, the Kalman filter unit 130 executes the first Kalman filter 131 using the calculated coordinate residual and velocity residual as input, thereby calculating a correction value for the velocity, a correction value for the angular velocity, a correction value for the two-dimensional coordinate, and a correction value for the azimuth angle.

[0051] The output correction unit 115 corrects the two-dimensional coordinate values ​​calculated by the latitude and longitude update unit 114 by adding the correction value calculated by the Kalman filter unit 130 to the two-dimensional coordinate values, and outputs the corrected two-dimensional coordinate values. Furthermore, the output correction unit 115 corrects the azimuth angle by adding the correction value calculated by the Kalman filter unit 130 to the azimuth angle calculated by the azimuth update unit 112, and outputs the corrected azimuth angle.

[0052] As described above, in the three-degree-of-freedom two-dimensional navigation, the velocity measured by the millimeter-wave radar 50 is used instead of the acceleration measured by the acceleration sensor 41, so that an integral error does not occur in the velocity. Furthermore, even if the initial value of the azimuth angle is inappropriate, an appropriate value can be quickly obtained as the azimuth angle. This is because the angular velocity measured by the millimeter-wave radar 50 is used instead of the angular velocity measured by the angular velocity sensor 42. Therefore, with three-degree-of-freedom two-dimensional navigation, only one integral calculation is required to determine the two-dimensional coordinate, the influence of errors in the initial value of the azimuth angle used in the coordinate conversion of the velocity is relatively small, and the two-dimensional coordinate value and the azimuth angle can each be appropriately corrected using a Kalman filter.

[0053] 8 shows an algorithm for six-degree-of-freedom three-dimensional navigation according to the first embodiment. This algorithm is the same as the algorithm shown in FIG. 9 of Patent Document 1. FIG. 8 will be explained below. The orientation unit 120 includes an input correction unit 124 and an output correction unit 125.

[0054] The input corrector 124 corrects the three-dimensional acceleration by adding the correction value calculated by the Kalman filter 130 to the three-dimensional acceleration indicated by the acceleration data 193 . Furthermore, the input correction unit 124 corrects the three-dimensional angular velocity by adding the correction value calculated by the Kalman filter unit 130 to the three-dimensional angular velocity indicated by the angular velocity data 194.

[0055] The strapdown calculation unit 140 calculates the position, attitude, and orientation of the moving body 20 using the three-dimensional acceleration corrected by the input correction unit 124 and the three-dimensional angular velocity corrected by the input correction unit 124.

[0056] The output correction unit 125 corrects the position calculated by the strapdown calculation unit 140 by adding the correction value calculated by the Kalman filter unit 130 to the position calculated by the strapdown calculation unit 140, and outputs the corrected position as the position of the location result. In addition, the output correction unit 125 corrects the attitude calculated by the strapdown calculation unit 140 by adding the correction value calculated by the Kalman filter unit 130 to the attitude, and outputs the corrected attitude as the attitude angle of the orientation result. Furthermore, the output correction unit 125 corrects the azimuth calculated by the strapdown calculation unit 140 by adding the correction value calculated by the Kalman filter unit 130 to the azimuth calculated by the strapdown calculation unit 140, and outputs the corrected azimuth as the azimuth angle of the location result.

[0057] The stoppage determination unit 123 determines whether the moving object 20 is stopped or not based on the speed calculated from the observation results of the millimeter wave radar 50.

[0058] When the moving object 20 is not stopped, the Kalman filter unit 130 calculates the difference between the position indicated by the GNSS data 191 and the position calculated by the strapdown calculation unit 140 as a "position residual." Next, the Kalman filter unit 130 calculates the difference between the velocity indicated by the velocity / angular velocity data 192 and the velocity calculated by the strapdown calculation unit 140 as a "velocity residual," and calculates the difference between the angular velocity indicated by the velocity / angular velocity data 192 and the angular velocity calculated by the strapdown calculation unit 140 as an "angular velocity residual." Next, the Kalman filter unit 130 executes the second Kalman filter 132 using the calculated position residual and velocity residual as input values, thereby calculating a correction value for the three-dimensional acceleration, a correction value for the three-dimensional angular velocity, a correction value for the position, and a correction value for the attitude.

[0059] In step S104, the third Kalman filter 133 to which the extended ZUPT algorithm is applied is executed in the six-degree-of-freedom three-dimensional navigation.

[0060] Fig. 9 shows an example of the configuration of a millimeter-wave radar 50. As shown in Fig. 9, the millimeter-wave radar 50 includes a synthesizer 51, an oscillator 52, a millimeter-wave transmitting / receiving antenna 53, a mixer 54, a signal processing unit 55, a symbol acquisition and tracking unit 56, a speed profile unit 57, a moving average unit 58, and a radar / body coordinate conversion unit 59.

[0061] The synthesizer 51 generates a chirp signal that represents the amplitude of the signal from the oscillator 52 as a function of time.

[0062] The millimeter wave transmitting / receiving antenna 53 includes a transmitting antenna and a receiving antenna. The transmitting antenna emits a chirp signal, which, when reflected by an object, generates a reflected chirp signal that is captured by the receiving antenna.

[0063] The mixer 54 generates an intermediate frequency signal by appropriately combining the transmission signal and the reception signal.

[0064] The signal processor 55 detects the beat frequency by performing a Fourier transform on the intermediate frequency signal. Since the beat frequency correlates with distance, the distance to the target is calculated based on the beat frequency. Furthermore, the Fourier transform also provides phase information, which can be used to calculate the velocity Doppler. Therefore, the relative velocity to the target can be calculated based on the results of the Fourier transform.

[0065] The symbol acquisition and tracking unit 56 calculates the correlation between the symbolized target and the symbol candidate indicated by the scan result based on the intensity of the radar echo (reflecting object). If a symbol candidate highly correlated with the symbolized target is found, the symbol acquisition and tracking unit 56 maintains tracking of the target by setting a prediction gate for the next scan based on the velocity information of the found symbol candidate. Each radar echo is assigned information indicating the symbol ID, Doppler angle of arrival, Doppler velocity, echo intensity, and distance calculated from the integral of velocity from the previous frame.

[0066] The velocity profile unit 57 randomly selects two points from the symbols output by the symbol acquisition and tracking unit 56 and calculates the Doppler velocity Vs and the Doppler angle of arrival α, which are coefficients of y = Vs cos(x-α), using the least squares method. Specifically, the velocity profile unit 57 selects the coefficient with the largest number of points whose residual error with respect to the fitted curve is within a threshold. A specific example of a known fitting method is the Random Sample Consensus (RANSAC) algorithm, as shown in [Reference 1]. By fitting the velocity profile in this way, it is possible to compensate for the coarse angular resolution and the coarse velocity resolution of the millimeter-wave radar 50, thereby improving the accuracy of both the velocity and angle.

[0067] [Reference 1] M. Fischler, R. Bolles. “Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography.” Communications of the ACM, 1981.

[0068] Fig. 10 shows a specific example of the results of actual symbol fitting. In Fig. 10, each point represents a symbol, and the curve represents the fitting result. Although there is a relatively large variation in the symbols, the value on the X-axis at the extreme value of the fitting result is obtained as the Doppler angle of arrival, and the value on the Y-axis at that extreme value is obtained as the Doppler velocity. Therefore, it can be seen that, despite the fact that the angular resolution and velocity resolution of the millimeter-wave radar 50 are both coarse, it is possible to reduce the variation in angle and velocity.

[0069] The moving average unit 58 reduces random angle measurement errors by taking a moving average of multiple epochs. Below, a specific example will be shown in which the moving average unit 58 uses a moving average of five epochs as each of the Doppler velocity Vs and the Doppler angle of arrival α, as shown in [Equation 1]. As a specific example, when the millimeter-wave symbol is updated in units of 10 Hz, the moving average unit 58 holds data for the past four epochs, calculates a moving average of a total of five pieces of data, including the current epoch and the past four epochs, by the calculation shown in [Equation 1], and outputs the calculated value in units of 10 Hz.

[0070]

number

[0071] The radar / body coordinate conversion unit 59 performs coordinate conversion as shown in Fig. 11. In Fig. 11, the X-axis and Y-axis are the X-axis and Y-axis of the body coordinate system, and the center of the IMU 40 is located at a position where the X-axis and Y-axis intersect at right angles. The radar / body coordinate conversion unit 59 converts the velocity Vs and azimuth angle α of the phase shift center of the radar antenna into the body coordinate system of the moving body, converting them from polar coordinates to Vb (X-axis velocity) and ω (Z-axis angular velocity) on the XY axes. Here, β indicates the angle formed by the nose and the center line of the antenna. ly indicates the Y-axis lever arm between the nose and the center of the antenna. lx indicates the X-axis lever arm between the center of the antenna and the center of the IMU 40. The center of the XY axes is the center of the IMU 40. A specific example of the model of the moving body is the Ackermann model.

[0072]

number

[0073] ***Explanation of the effect of the first embodiment*** As described above, according to this embodiment, in a mobile body equipped with a millimeter-wave radar and a GNSS / INS hybrid navigation system, by combining both the millimeter-wave radar and the GNSS / INS hybrid navigation system, state estimation and observation updating are performed using a Kalman filter, using as observables the difference between the velocity and angular velocity of the mobile body measured by the millimeter-wave radar and the velocity and angular velocity estimated by the GNSS / INS hybrid navigation system. Therefore, according to this embodiment, it is possible to realize a position and attitude locating device that can locate with relatively high accuracy the position of a mobile body that is equipped with a low-accuracy IMU, one GNSS antenna, and a millimeter-wave radar that does not have high spatial resolution, or a mobile body that is unable to acquire vehicle speed pulses.

[0074] ***Other Configurations*** <Variation 1> 12 shows an example of the functional configuration of a position and attitude location system 90 according to Embodiment 1. The position and attitude location system 90 includes a magnetic direction sensor 60 instead of the millimeter-wave radar 50. The magnetic direction sensor 60 is, as a specific example, a relatively simple sensor that determines direction using the Earth's geomagnetism. The moving body 20 according to this modification is a ship, as a specific example. The moving body 20 is equipped with a magnetic direction sensor 60 for measuring direction instead of the millimeter wave radar 50. The position and attitude localization device 100 according to this modification uses orientation data 195 instead of the velocity and angular velocity data 192. That is, the position and attitude localization device 100 uses angular velocity calculated from the time change in orientation measured by the magnetic orientation sensor 60 instead of the velocity and angular velocity measured by the millimeter wave radar 50. The orientation data 195, which is the output of the magnetic orientation sensor 60, is converted into angular velocity data by performing the calculation shown in [Equation 3]. Here, ΔΨ indicates angular velocity data at time t+1, and Ψ t denotes the direction data at time t, and Ψ t+1 indicates the direction data at time t+1. ΔΨ corresponds to the angular velocity calculated from the change in the direction measured by the magnetic direction sensor 60 over time.

[0075]

number

[0076] In this modification, velocity errors cannot be corrected as compared with the first embodiment. However, the Kalman filter unit 130 corrects angular velocity errors of the strapdown calculation unit 140. Therefore, according to this modification, it is possible to avoid the phenomenon in which the orientation value drifts due to gyro errors or the like, and it is possible to locate the position of the moving body 20 with relatively high accuracy.

[0077] There are few magnetic objects around the ship, such as steel towers, buildings, and automobiles. Therefore, according to this modification, by using a magnetic direction sensor 60 instead of the millimeter-wave radar 50, it is possible to obtain the same effect as that of the first embodiment. The position and attitude locating system 90 may use the millimeter wave radar 50 and the magnetic direction sensor 60 in combination.

[0078] <Variation 2> FIG. 13 shows an example of the hardware configuration of the position and attitude localization device 100 according to this modified example. The position and attitude localization device 100 includes a processing circuit 18 in place of the processor 11, the processor 11 and memory 12, the processor 11 and auxiliary storage device 13, or the processor 11, memory 12, and auxiliary storage device 13. The processing circuitry 18 is hardware that realizes at least a part of the components of the position and attitude localization device 100 . The processing circuitry 18 may be dedicated hardware or may be a processor that executes a program stored in the memory 12 .

[0079] When processing circuitry 18 is dedicated hardware, processing circuitry 18 may be, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or a combination thereof. The position and attitude localization device 100 may include a plurality of processing circuits that replace the processing circuit 18. The plurality of processing circuits share the role of the processing circuit 18.

[0080] In the position and attitude localization device 100, some functions may be realized by dedicated hardware, and the remaining functions may be realized by software or firmware.

[0081] Processing circuitry 18 is illustratively implemented in hardware, software, firmware, or a combination thereof. The processor 11, memory 12, auxiliary storage device 13, and processing circuit 18 are collectively referred to as "processing circuitry." In other words, the functions of the functional components of the position and attitude localization device 100 are realized by the processing circuitry.

[0082] ***Other embodiments*** Although the first embodiment has been described, it is also possible to combine multiple parts of this embodiment. Alternatively, it is also possible to implement this embodiment in part. In addition, this embodiment may be modified in various ways as needed, and may be implemented in any combination, either as a whole or in part. The above-described embodiments are essentially preferred examples and are not intended to limit the scope of the present disclosure, its applications, and uses. The procedures described using flowcharts and the like may be modified as appropriate. [Explanation of symbols]

[0083] 11 processor, 12 memory, 13 auxiliary storage device, 14 input / output IF, 15 communication device, 18 processing circuit, 19 signal line, 20 moving body, 30 GNSS receiver, 40 IMU, 41 acceleration sensor, 42 angular velocity sensor, 50 millimeter wave radar, 51 synthesizer, 52 oscillator, 53 millimeter wave transmitting / receiving antenna, 54 mixer, 55 signal processing unit, 56 symbol acquisition and tracking unit, 57 velocity profile unit, 58 moving average unit, 59 radar / body coordinate conversion unit, 60 magnetic direction sensor, 90 position and attitude location system, 100 position and attitude location device, 110 initial value calculation unit, 111 input correction unit, 112 direction update unit, 113 coordinate conversion unit, 114 latitude and longitude update unit, 115 output correction unit, 120 location unit, 121 A running orientation unit, 122 a stopped orientation unit, 123 a stop determination unit, 124 an input correction unit, 125 an output correction unit, 130 a Kalman filter unit, 131 a first Kalman filter, 132 a second Kalman filter, 133 a third Kalman filter, 140 a strapdown calculation unit, 190 a memory unit, 191 GNSS data, 192 speed and angular velocity data, 193 acceleration data, 194 angular velocity data, and 195 orientation data.

Claims

1. A position and attitude locating device for locating the position of a moving object, the position and attitude locating device being equipped with a GNSS receiver that performs positioning using a GNSS (Global Navigation Satellite System), a millimeter wave radar that measures velocity and angular velocity, an acceleration sensor that measures acceleration, and an angular velocity sensor that measures the angular velocity of an azimuth angle, an initial value calculation unit that calculates an azimuth angle of the moving body as a first navigation azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculates a change amount in two-dimensional coordinates by integrating the velocity and angular velocity measured by the millimeter-wave radar based on the calculated first navigation azimuth angle, calculates two-dimensional coordinate values ​​of the moving body as first navigation coordinate values ​​by adding the calculated change amount to two-dimensional position initial values, calculates a correction value for correcting the first navigation azimuth angle using a first Kalman filter based on the calculated first navigation coordinate values ​​and the two-dimensional coordinate values ​​included in the coordinate values ​​measured by the GNSS receiver, corrects the first navigation azimuth angle using the calculated correction value, and outputs the corrected first navigation azimuth angle as an azimuth angle initial value; a location unit that calculates an amount of change in the azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculates the azimuth angle of the moving body as a second navigation azimuth angle based on the calculated amount of change in the azimuth angle and the output initial azimuth angle value, converts three-dimensional acceleration measured by the acceleration sensor into a three-dimensional acceleration of a navigation coordinate system based on the calculated second navigation azimuth angle, calculates an amount of change in the three-dimensional coordinates by integrating the converted three-dimensional acceleration, calculates three-dimensional coordinate values ​​of the moving body as second navigation coordinate values ​​by adding the calculated amount of change to the three-dimensional position initial value, calculates correction values ​​for correcting the second navigation coordinate values ​​using a second Kalman filter based on the calculated second navigation coordinate values ​​and the three-dimensional coordinate values ​​measured by the GNSS receiver, corrects the second navigation coordinate values ​​using the calculated correction values, and outputs the corrected second navigation coordinate values ​​as position location values ​​for positioning the moving body; A position and attitude localization device comprising:

2. The orientation unit includes: when the moving body stops moving, calculate an azimuth angle correction value for correcting the currently calculated second navigation azimuth angle using a third Kalman filter based on the previously calculated second navigation azimuth angle and the currently calculated second navigation azimuth angle, and correct the currently calculated second navigation azimuth angle using the calculated azimuth angle correction value to calculate a corrected second navigation azimuth angle; 2. The position and attitude localization device according to claim 1, wherein, when the moving object that has stopped resumes movement, the device calculates a new second navigation azimuth angle based on the corrected second navigation azimuth angle and an amount of change in the new azimuth angle, converts a new three-dimensional acceleration measured by the acceleration sensor into a three-dimensional acceleration in a navigation coordinate system based on the calculated new second navigation azimuth angle, calculates an amount of change in the three-dimensional coordinates by integrating the converted three-dimensional acceleration, and adds the calculated amount of change to the previously calculated second navigation coordinate value to calculate a new second navigation coordinate value, calculates a position correction value that corrects the new second navigation coordinate value based on the calculated new second navigation coordinate value and new three-dimensional coordinate value measured by the GNSS receiver, corrects the new second navigation coordinate value using the calculated position correction value, and outputs the corrected new second navigation coordinate value as a new position location value.

3. The orientation unit includes: If the speed measured by the millimeter wave radar is less than a predetermined speed threshold, it is determined that the moving object has stopped moving; 3. The position and attitude locating device according to claim 2, wherein after it is determined that the moving body has stopped moving, if the speed measured by the millimeter wave radar is equal to or greater than the speed threshold, it is determined that the moving body has resumed moving.

4. 4. The position and attitude localization device according to claim 2, wherein the third Kalman filter calculates, as the azimuth angle correction value, a correction value that makes the second navigation azimuth calculated last time coincide with the second navigation azimuth calculated this time.

5. 4. The position and attitude localization device according to claim 2, wherein the third Kalman filter generates an observation equation that represents a difference between a second navigation azimuth calculated previously and a second navigation azimuth calculated currently, and calculates the azimuth correction value using the generated observation equation.

6. a magnetic bearing sensor that measures a bearing is attached to the moving object instead of the millimeter wave radar; 4. The position and attitude locating device according to claim 1, wherein the position and attitude locating device uses an angular velocity calculated from a change over time in the orientation measured by the magnetic orientation sensor, instead of the velocity and angular velocity measured by the millimeter-wave radar.

7. A position and attitude locating method for locating the position of a moving object equipped with a GNSS (Global Navigation Satellite System) receiver for performing positioning using the GNSS, a millimeter wave radar for measuring velocity and angular velocity, an acceleration sensor for measuring acceleration, and an angular velocity sensor for measuring angular velocity of an azimuth angle, comprising: a computer calculates an azimuth angle of the moving body as a first navigation azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculates a change amount in two-dimensional coordinates by integrating the velocity and angular velocity measured by the millimeter-wave radar based on the calculated first navigation azimuth angle, calculates a two-dimensional coordinate value of the moving body as a first navigation coordinate value by adding the calculated change amount to a two-dimensional position initial value, calculates a correction value for correcting the first navigation azimuth angle using a first Kalman filter based on the calculated first navigation coordinate value and the two-dimensional coordinate value included in the coordinate value measured by the GNSS receiver, corrects the first navigation azimuth angle using the calculated correction value, and outputs the corrected first navigation azimuth angle as an azimuth angle initial value; a position and attitude location method in which the computer calculates an amount of change in the azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculates the azimuth angle of the moving body as a second navigation azimuth angle based on the calculated amount of change in the azimuth angle and the output initial azimuth angle value, converts the three-dimensional acceleration measured by the acceleration sensor into a three-dimensional acceleration of a navigation coordinate system based on the calculated second navigation azimuth angle, calculates an amount of change in the three-dimensional coordinates by integrating the converted three-dimensional acceleration, calculates the three-dimensional coordinate values ​​of the moving body as second navigation coordinate values ​​by adding the calculated amount of change to the three-dimensional initial position values, calculates a correction value for correcting the second navigation coordinate values ​​using a second Kalman filter based on the calculated second navigation coordinate values ​​and the three-dimensional coordinate values ​​measured by the GNSS receiver, corrects the second navigation coordinate values ​​using the calculated correction value, and outputs the corrected second navigation coordinate values ​​as position location values ​​for location of the moving body.

8. A position and attitude location program executed by a position and attitude location device, which is a computer that locates the position of a moving object equipped with a GNSS receiver that performs positioning using a GNSS (Global Navigation Satellite System), a millimeter wave radar that measures velocity and angular velocity, an acceleration sensor that measures acceleration, and an angular velocity sensor that measures the angular velocity of an azimuth angle, an initial value calculation process of calculating an azimuth angle of the moving body as a first navigation azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculating a change amount in two-dimensional coordinates by integrating the velocity and angular velocity measured by the millimeter-wave radar based on the calculated first navigation azimuth angle, calculating a two-dimensional coordinate value of the moving body as a first navigation coordinate value by adding the calculated change amount to a two-dimensional position initial value, calculating a correction value for correcting the first navigation azimuth angle using a first Kalman filter based on the calculated first navigation coordinate value and the two-dimensional coordinate value included in the coordinate value measured by the GNSS receiver, correcting the first navigation azimuth angle using the calculated correction value, and outputting the corrected first navigation azimuth angle as an azimuth angle initial value; an orientation process of calculating an amount of change in an azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculating an azimuth angle of the moving body as a second navigation azimuth angle based on the calculated amount of change in the azimuth angle and the output initial azimuth angle value, converting three-dimensional acceleration measured by the acceleration sensor into a three-dimensional acceleration of a navigation coordinate system based on the calculated second navigation azimuth angle, integrating the converted three-dimensional acceleration to calculate an amount of change in the three-dimensional coordinates, calculating three-dimensional coordinate values ​​of the moving body as second navigation coordinate values ​​by adding the calculated amount of change to the three-dimensional position initial value, calculating a correction value for correcting the second navigation coordinate value using a second Kalman filter based on the calculated second navigation coordinate value and the three-dimensional coordinate value measured by the GNSS receiver, correcting the second navigation coordinate value using the calculated correction value, and outputting the corrected second navigation coordinate value as a position orientation value for positioning the moving body; a position and attitude locating program that causes the position and attitude locating device to execute the above;

Images