Position / attitude calibration device, position / attitude calibration method, and position / attitude 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

Position / attitude orientation device, position / attitude orientation method, and position / attitude orientation program

[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.

[0002] A hybrid navigation system that uses a Global Positioning System (GPS) and an Inertial Navigation System (INS) to determine a position by compensating for the advantages and disadvantages of both systems is adopted in aircraft, ships, vehicle systems, and the like. Here, in order to maintain relatively high accuracy during a relatively long period of satellite blindness, a relatively high-precision Inertial Measurement System (IMU) is required. Due to its relatively high cost, IMUs have only been adopted in limited fields such as aviation and space. Therefore, in order to realize a relatively low-cost, relatively high-precision hybrid navigation system, it is conceivable to determine the position, attitude, and orientation of a vehicle using multiple Global Navigation Satellite System (GNSS) antennas and vehicle speed pulses. However, there are limitations, such as the need to widen the spacing between GNSS antennas and attach sensors to the wheels to acquire vehicle speed pulses. These limitations arise due to the following reasons: (1) When determining the direction of a vehicle while it is stopped, using a relatively low-precision IMU makes it impossible to measure the Earth's rotation and perform calculations to determine the direction. (2) In three-dimensional navigation calculations, if initial values ​​(position, attitude, and direction) 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 from the GNSS. However, for the direction, it is impossible to obtain an appropriate value due to reason (1). (3) Even if an accurate initial value is obtained by some method, the direction value will drift due to gyro errors and other factors when the vehicle is subsequently stopped, etc.

[0003] Furthermore, it is expected that relatively inexpensive MEMS (Micro Electro Mechanical Systems) IMUs will become more precise 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.

[0004] JP 2012-193965 A

[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 positions with relatively high accuracy in environments where the sky is clear and satellites are visible. However, GNSS / INS integrated navigation systems have weaknesses such as inability to perform positioning under bridges or near buildings, or 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 being unable to acquire vehicle speed pulses.

[0007] A position and attitude localization device according to the present disclosure is a position and attitude localization device that locates the position of a moving body, and is equipped with a GNSS (Global Navigation Satellite System) receiver that performs positioning using the GNSS, 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; and an orientation 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 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 value, 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 orientation values ​​for positioning the moving body.

[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.

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

[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] First Embodiment Hereinafter, the present embodiment will be described in detail with reference to the drawings.

[0012] 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 GNSS (Global Navigation Satellite System) receiver 30, an IMU (Inertial Measurement Unit) 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 is also called an inertial sensor and measures acceleration in three axial directions and angular velocity in three axial directions. The three axial directions refer to the length, width, and height directions of the moving object. Hereinafter, the acceleration in the three axial directions will be referred to as "three-dimensional acceleration," and the angular velocity in the three axial directions 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) system 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 the moving body 20 to which the GNSS receiver 30, the millimeter-wave radar 50, the acceleration sensor 41, and the angular velocity sensor 42 are attached. The position and attitude localization device 100 locates each of the position, attitude, and direction of the moving body 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 acceleration, and 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 an initial value of the azimuth angle. Specifically, the initial value calculation unit 110 first 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 two-dimensional coordinate values ​​of the moving object 20 as first navigation coordinate values ​​by adding the calculated change amount to a two-dimensional position initial value. The two-dimensional position initial value is the initial position of 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 values ​​and two-dimensional coordinate values ​​included in the coordinate values ​​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 that 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 described using FIG. 4 . Hereinafter, 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. Typically, (0,0) is set as the "two-dimensional position initial value," and (0,0,0) is set as the "three-dimensional position initial value." Also, as shown in FIG. 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 set as the X-axis, Y-axis, and Z-axis of the navigation coordinate system, respectively. 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. Also, 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 Figure 4, the Body coordinate system is a coordinate system with the center of the IMU 40 as the origin, the forward direction of the moving body 20 as the X axis, the right direction relative to the forward direction of the moving body 20 as the Y axis, and the vertical downward direction of the moving body 20 as the Z axis.

[0022] The location unit 120 includes a traveling location unit 121, a stopped location unit 122, and a stop determination unit 123. The location unit 120 locates the position of the moving body 20 based on the measurement values ​​of the angular velocity sensor 42, the initial azimuth angle value, the measurement values ​​of the acceleration sensor 41, and the positioning results of the GNSS receiver 30. More specifically, the location unit 120 calculates an 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 three-dimensional position initial value, and corrects the second navigation coordinate values ​​based on the calculated three-dimensional second navigation coordinate values ​​and the positioning results of the GNSS receiver 30, thereby locating the position of the moving body 20. The three-dimensional position initial 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 to orient (calculate) the position, attitude, and orientation of the mobile object 20 while the mobile object 20 is traveling. The traveling orientation unit 121 is also called an orientation value calculation unit. The stopped orientation unit 122 controls the Kalman filter unit 130 and the strapdown calculation unit 140 to orient the position, attitude, and orientation of the mobile object 20 while the mobile object 20 is stopped. The stopped orientation unit 122 is also called an azimuth angle correction unit. The stop determination unit 123 determines whether the mobile object 20 has stopped or started moving.

[0023] Specifically, the location 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 mobile 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 location 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 location 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 mobile object 20 as second navigation coordinate values ​​by adding the calculated amount of change to the three-dimensional position initial value. Next, the location 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 located the position of the moving body 20. Furthermore, when the moving body 20 stops moving, the orientation unit 120 calculates an azimuth angle correction value for correcting the currently calculated second navigation azimuth using a third Kalman filter based on the previously calculated second navigation azimuth angle and the currently calculated second navigation azimuth angle, and 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 a 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. In addition, when the stopped moving body 20 resumes movement, the positioning unit 120 first calculates a new second navigation azimuth angle based on the corrected second navigation azimuth angle 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 angle.Next, the location unit 120 integrates the converted three-dimensional acceleration to calculate a change in three-dimensional coordinates, and adds the calculated change to the previously calculated second navigation coordinate value to calculate a new second navigation coordinate value. Next, the location unit 120 uses a second Kalman filter to calculate a position correction value for correcting the new second navigation coordinate value based on the calculated new second navigation coordinate value and the new three-dimensional coordinate value measured by the GNSS receiver 30, 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. If the speed measured by the millimeter-wave radar 50 is less than a predetermined speed threshold, the location unit 120 determines that the moving body 20 has stopped moving. If the speed measured by the millimeter-wave radar 50 is equal to or greater than the 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.

[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 multiple 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 / 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 / angular velocity data 192 consists of data indicating the velocity and angular velocity observed by the millimeter-wave radar 50. The acceleration data 193 consists of data observed by the acceleration sensor 41. The angular velocity data 194 consists of data observed by the 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 composed of a computer. The position and attitude localization device 100 may be composed 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 pieces of hardware 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 a 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 multiple processors that replace the processor 11. The multiple 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 configured integrally.

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

[0033] The communication device 15 is a receiver and a transmitter, and is specifically a communication chip or a NIC (Network Interface Card).

[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 recorded on 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] ***Description of Operation*** The operating procedure of the position and attitude localization device 100 corresponds to a position and attitude localization method. Also, 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 locates the position, attitude, and orientation of the moving body 20 using six-degree-of-freedom three-dimensional navigation, using the initial value of the azimuth angle or the most recently determined azimuth angle, until the moving body 20 stops.

[0042] (Step S103) If the moving body 20 is stopped, 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) While the moving body 20 is stopped, the stationary state locating unit 122 locates the position, attitude, and orientation of the moving body 20 using six-degree-of-freedom three-dimensional navigation based on an extended ZUPT (Zero Velocity Update) algorithm. An overview of the extended ZUPT algorithm is disclosed in Patent Document 1.

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

[0045] Fig. 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. 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. The input correction unit 111 also 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 coordinates by integrating the two-dimensional velocity obtained by the coordinate conversion unit 113 once. The two-dimensional coordinates consist of a latitude component and a longitude component. The latitude and longitude update unit 114 calculates new two-dimensional coordinates by adding the calculated amount of change in the two-dimensional coordinates to the initial values ​​of the two-dimensional coordinates or the two-dimensional coordinates 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 mobile object 20 departs, that is, when the mobile object 20 is stopped, as the initial values ​​of the two-dimensional coordinates.

[0050] First, the Kalman filter unit 130 calculates, as a "coordinate residual," 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. Next, the Kalman filter unit 130 calculates, as a "velocity residual," 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. 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 velocity correction value, an angular velocity correction value, a two-dimensional coordinate correction value, and an azimuth correction value.

[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 calculated by the azimuth update unit 112 by adding the correction value calculated by the Kalman filter unit 130 to the azimuth angle, and outputs the corrected azimuth angle.

[0052] As described above, three-degree-of-freedom two-dimensional navigation uses the velocity measured by the millimeter-wave radar 50, rather than the acceleration measured by the acceleration sensor 41, so no integral error is carried over into the velocity. Furthermore, even if the initial value of the azimuth angle is inappropriate, an appropriate value for the azimuth angle can be quickly determined. This is because the angular velocity measured by the millimeter-wave radar 50, rather than the angular velocity measured by the angular velocity sensor 42, is used. Therefore, three-degree-of-freedom two-dimensional navigation requires only one integral calculation to determine the two-dimensional coordinate, is relatively less affected by errors in the initial value of the azimuth angle used in the coordinate conversion of the velocity, and can appropriately correct both the two-dimensional coordinate value and the azimuth angle using a Kalman filter.

[0053] Fig. 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 unit 130 to the three-dimensional acceleration indicated by the acceleration data 193. The input corrector 124 also 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. The output correction unit 125 also 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 location result. The output correction unit 125 also corrects the orientation calculated by the strapdown calculation unit 140 by adding the correction value calculated by the Kalman filter unit 130 to the orientation, and outputs the corrected orientation as the azimuth angle of the location result.

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

[0058] When the mobile 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 strap-down 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 strap-down 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 strap-down 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 three-dimensional acceleration, a correction value for three-dimensional angular velocity, a correction value for position, and a correction value for 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. When an object reflects the chirp signal, a reflected chirp signal is generated, and the generated chirp signal 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, and the velocity Doppler can be calculated based on the obtained phase information. Therefore, the relative velocity to the target can also 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 residuals with respect to the fitted curve are within a threshold. A specific example of a known fitting method is the Random Sample Consensus (RANSAC) algorithm shown in [Reference 1]. By fitting the velocity profile in this manner, it is possible to compensate for the roughness of the angular resolution and the roughness of the velocity resolution of the millimeter-wave radar 50, thereby improving the accuracy of both the velocity and the angle.

[0067] [Reference 1] M. Fischler, R. Bolles. “Random sample consensus: a paradigm for model fitting with applications to image analysis and automation ”, 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 adopting a moving average of multiple epochs. A specific example will be shown below in which the moving average unit 58 adopts 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 10 Hz increments, the moving average unit 58 stores data for the past four epochs, calculates a moving average of a total of five pieces of data (the current epoch and the past four epochs) by the operation shown in [Equation 1], and outputs the calculated value in 10 Hz increments.

[0070]

[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 the 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 between 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. 1x 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 a moving body model is the Ackermann model.

[0072]

[0073] ***Description of Effects of First Embodiment*** As described above, according to the present 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 differences between the velocity and angular velocity of the mobile body measured by the millimeter-wave radar and the velocity and angular velocity estimates by the GNSS / INS hybrid navigation system. Therefore, according to the present 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*** <Modification 1> FIG. 12 shows an example of the functional configuration of a position and attitude location system 90 according to the first embodiment. 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 mobile body 20 according to this modification is, as a specific example, a ship. The mobile body 20 is equipped with a magnetic direction sensor 60 that measures direction instead of the millimeter-wave radar 50. The position and attitude location device 100 according to this modification uses direction data 195 instead of the velocity and angular velocity data 192. That is, the position and attitude location device 100 uses angular velocity calculated from the time change in direction measured by the magnetic direction sensor 60 instead of the velocity and angular velocity measured by the millimeter-wave radar 50. The direction data 195, which is the output of the magnetic direction sensor 60, is converted into angular velocity data by executing the calculation shown in [Equation 3]. where ΔΨ represents the angular velocity data at time t+1, and Ψ ｔ denotes the direction data at time t, and Ψ ｔ＋１ indicates the direction data at time t+1. ΔΨ corresponds to the angular velocity calculated from the change over time in the direction measured by the magnetic direction sensor 60.

[0075]

[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 in the strapdown calculation unit 140. Therefore, according to this modification, it is possible to avoid the phenomenon in which the heading 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 a 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. Note that the position and attitude locating system 90 may use both the millimeter-wave radar 50 and the magnetic direction sensor 60.

[0078] 13 shows an example of the hardware configuration of a position and attitude localization device 100 according to this modification. The position and attitude localization device 100 includes a processing circuitry 18 instead 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 included in 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 the processing circuitry 18 is dedicated hardware, the 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 multiple processing circuits that replace the processing circuitry 18. The multiple processing circuits share the role of the processing circuitry 18.

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

[0081] The processing circuitry 18 is realized by, for example, hardware, software, firmware, or a combination of these. The processor 11, memory 12, auxiliary storage device 13, and processing circuitry 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 and implement it. 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. Note that 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, etc. may be modified as appropriate.

[0083] 11 Processor, 12 Memory, 13 Auxiliary storage device, 14 Input / output IF, 15 Communication device, 18 Processing circuit, 19 Signal line, 20 Mobile object, 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 Speed ​​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 Orientation update unit, 113 Coordinate conversion unit, 114 Latitude and longitude update unit, 115 Output correction unit, 120 Location unit, 121 Traveling positioning unit, 122 Stop positioning unit, 123 Stop determination unit, 124 Input correction unit, 125 Output correction unit, 130 Kalman filter unit, 131 First Kalman filter, 132 Second Kalman filter, 133 Third Kalman filter, 140 Strapdown calculation unit, 190 Memory unit, 191 GNSS data, 192 Speed / angular velocity data, 193 Acceleration data, 194 Angular velocity data, 195 Orientation data.

Claims

1. A position and attitude locating device for locating a position of a moving object, the position and attitude locating device including a GNSS receiver for performing positioning using a Global Navigation Satellite System (GNSS), a millimeter wave radar for measuring speed and angular velocity, an acceleration sensor for measuring acceleration, and an angular velocity sensor for measuring 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 an amount of change in two-dimensional coordinates by integrating the velocity and the 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 amount of change 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 a 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 positioning unit that calculates an amount of change in an azimuth angle based on an angular velocity of the azimuth angle measured by the angular velocity sensor, calculates 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 value of the azimuth angle, converts a 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 a three-dimensional coordinate value of the moving body as a second navigation coordinate value by adding the calculated amount of change to a three-dimensional position initial value, calculates 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, corrects the second navigation coordinate value using the calculated correction value, and outputs the corrected second navigation coordinate value as a position positioning value for positioning the moving body; A position and attitude determination device comprising:

2. The orientation unit includes: when the moving body stops moving, calculating 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 correcting 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 location device according to claim 1, wherein, when the moving body that has stopped resumes movement, a new second navigation azimuth is calculated based on the corrected second navigation azimuth and an amount of change in the new azimuth angle, a new three-dimensional acceleration measured by the acceleration sensor is converted into a three-dimensional acceleration of a navigation coordinate system based on the calculated new second navigation azimuth, an amount of change in the three-dimensional coordinates is calculated by integrating the converted three-dimensional acceleration, and a new second navigation coordinate value is calculated by adding the calculated amount of change to a previously calculated second navigation coordinate value, a position correction value for correcting the new second navigation coordinate value is calculated using the second Kalman filter based on the calculated new second navigation coordinate value and new three-dimensional coordinate values ​​measured by the GNSS receiver, the new second navigation coordinate value is corrected using the calculated position correction value, and the corrected new second navigation coordinate value is output 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 object 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 object has resumed moving.

4. 4. The position and attitude locating device according to claim 2, wherein the third Kalman filter calculates, as the azimuth angle correction value, a correction value that causes a previously calculated second navigation azimuth angle to coincide with a currently calculated second navigation azimuth angle.

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

6. a magnetic bearing sensor that measures a bearing instead of the millimeter wave radar is attached to the moving object; 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 time change in a direction measured by the magnetic direction sensor, instead of the velocity and angular velocity measured by the millimeter wave radar.

7. A position and attitude locating device for locating a position of a moving object, the position and attitude locating device including a GNSS receiver for performing positioning using a Global Navigation Satellite System (GNSS), a millimeter wave radar for measuring speed and angular velocity, an acceleration sensor for measuring acceleration, and an angular velocity sensor for measuring the angular velocity of an azimuth angle, an initial value calculation unit that calculates an azimuth angle initial value, which is an initial value of an azimuth angle of the moving object, based on the measurement value of the angular velocity sensor, the measurement value of the millimeter wave radar, and the positioning result of the GNSS receiver; a positioning unit that determines a position of the moving body based on the measurement value of the angular velocity sensor, the initial value of the azimuth angle, the measurement value of the acceleration sensor, and a positioning result of the hybrid navigation of the GNSS receiver; A position and attitude determination device comprising:

8. the initial value calculation unit calculates a first navigation azimuth based on the angular velocity measured by the angular velocity sensor, calculates a two-dimensional first navigation coordinate value based on the calculated first navigation azimuth, the velocity and angular velocity measured by the millimeter wave radar, and a two-dimensional position initial value, and calculates the azimuth angle initial value by correcting the first navigation azimuth based on the calculated two-dimensional first navigation coordinate value and a positioning result of the GNSS receiver; 8. The position and attitude location device according to claim 7, wherein the location unit calculates an amount of change in azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor, calculates a second navigation azimuth angle based on the calculated amount of change in azimuth angle and the initial azimuth angle value, calculates a three-dimensional second navigation coordinate value based on the calculated second navigation azimuth angle, the acceleration measured by the acceleration sensor, and a three-dimensional position initial value, and corrects the second navigation coordinate value based on the calculated three-dimensional second navigation coordinate value and a positioning result of the GNSS receiver, thereby location of the moving body.

9. A position and attitude locating method for locating a position of a moving object equipped with a GNSS receiver that performs positioning using a Global Navigation Satellite System (GNSS), 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, 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 an amount of change 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 amount of change 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 a 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 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, 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 an initial three-dimensional position value, calculates 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, corrects the second navigation coordinate value using the calculated correction value, and outputs the corrected second navigation coordinate value as a position location value for location of the position of the moving body.

10. 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 to which a GNSS receiver that uses a Global Navigation Satellite System (GNSS) to perform positioning, 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 are attached, the position and attitude location program comprising: 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 an amount of change in two-dimensional coordinates by integrating the velocity and the 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 amount of change 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 a 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 an 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 a 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 a three-dimensional coordinate value of the moving body as a second navigation coordinate value by adding the calculated amount of change to a 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 location program that causes the position and attitude location device to execute the above.