WO2019159230A1

WO2019159230A1 - Position/attitude estimation device and position/attitude estimation method

Info

Publication number: WO2019159230A1
Application number: PCT/JP2018/004890
Authority: WO
Inventors: 隆博加島
Original assignee: 三菱電機株式会社
Priority date: 2018-02-13
Filing date: 2018-02-13
Publication date: 2019-08-22
Also published as: JPWO2019159230A1; JP6647481B2

Abstract

A position/attitude storage unit (3) retains the values of the present position and the present attitude. A road-end distance storage unit (6) determines a reference road end and a road-end distance. A traveling direction identification unit (4) identifies the traveling direction. A moving amount calculation unit (7) determines the moving distance after a preset period of time has elapsed. A position update unit (8) makes, on the basis of the road-end distance and the reference road end, and on the basis of the traveling direction and the moving distance, a movement by the amount of the moving distance in the traveling direction along the road-end shape of the reference road end from the present position and then updates the present position of the position/attitude storage unit (3) at a position situated at the road-end distance from the reference road end.

Description

Position / orientation estimation apparatus and position / orientation estimation method

The present invention mainly relates to a position and orientation estimation apparatus and a position and orientation estimation method for estimating the position and orientation of a mobile terminal carried by a pedestrian.

There is a pedestrian autonomous navigation technology that measures the displacement of the relative position of a terminal using an acceleration sensor and an angular velocity sensor built in the mobile terminal (see, for example, Non-Patent Document 1). In addition, since such pedestrian autonomous navigation accumulates positioning errors in proportion to the amount of walking, map positioning technology that corrects the positioning results using map information is used to correct the positioning results to the optimal position. I was trying to do it.

However, the conventional map matching technology can prevent positioning of routes that cannot occur in reality, such as passing through inaccessible areas such as walls and buildings, but it is not possible to There was a problem that it was not possible to calculate an accurate position. Therefore, for example, in an augmented reality (AR) application that draws 3D graphics based on position information of a positioning result, even if the conventional map matching technology is applied, an error in the terminal position in the road There was a problem that it could not be drawn accurately.

The present invention has been made to solve such a problem, and an object of the present invention is to provide a position / orientation estimation apparatus that can more accurately estimate a terminal position in a road.

A position / orientation estimation apparatus according to the present invention includes a position / orientation storage unit that holds values of a current position and a current attitude, a reference road end that serves as a reference for a distance based on a road shape on a map, and a current position that is held A road edge distance calculation unit that calculates the distance as a road edge distance, a traveling direction determination unit that determines a traveling direction based on the current posture held and the road edge shape of the reference road edge, and a movement amount after a set time has elapsed The movement amount calculation unit for calculating the current position, the current position held, the road edge distance and the reference road edge calculated by the road edge distance calculation unit, the traveling direction determined by the traveling direction determination unit, and the movement amount calculation unit Based on the travel distance calculated in step 1, the travel distance is moved along the road edge shape of the reference road edge in the direction of travel from the current position held, and the position away from the reference road edge is calculated. Update the current position of the position and orientation storage unit with the calculated position It is those with a door.

The position and orientation estimation apparatus according to the present invention moves based on the road edge distance, the reference road edge, the traveling direction, and the movement amount, by the movement amount along the road edge shape of the reference road edge from the current position to the traveling direction, Since the current position is updated at a position away from the reference road edge by the road edge distance, the terminal position in the road can be accurately estimated.

It is a hardware block diagram of the position and orientation estimation apparatus of Embodiment 1 of this invention. It is a block diagram of the position and orientation estimation apparatus of Embodiment 1 of this invention. It is a flowchart which shows the flow of the absolute position and orientation estimation thread | sled in the position and orientation estimation apparatus of Embodiment 1 of this invention. It is a flowchart which shows the flow of the advancing direction determination thread | sled in the position and orientation estimation apparatus of Embodiment 1 of this invention. It is a flowchart which shows the flow of the attitude | position update thread | sled in the position / orientation estimation apparatus of Embodiment 1 of this invention. It is a flowchart which shows the flow of the position update thread | sled in the position and orientation estimation apparatus of Embodiment 1 of this invention. It is explanatory drawing which plotted the road shape on the map database in the position and orientation estimation apparatus of Embodiment 1 of this invention on the two-dimensional plane. It is explanatory drawing which shows the calculation method of the road direction in the position and orientation estimation apparatus of Embodiment 1 of this invention. It is explanatory drawing which shows the update method of the position in the position and orientation estimation apparatus of Embodiment 1 of this invention. It is explanatory drawing which shows the method of calculating an update position with the distance along the shape of the reference | standard road end in the position and orientation estimation apparatus of Embodiment 1 of this invention. It is explanatory drawing which shows the method of calculating an update position with the linear distance in the position and orientation estimation apparatus of Embodiment 1 of this invention. It is a block diagram of the position and orientation estimation apparatus of Embodiment 2 of this invention.

Hereinafter, in order to explain the present invention in more detail, modes for carrying out the present invention will be described with reference to the accompanying drawings.
Embodiment 1 FIG.
FIG. 1 is a hardware configuration diagram illustrating a position and orientation estimation apparatus according to the first embodiment.
The illustrated position / orientation estimation apparatus 100 includes an arithmetic device 101, a storage device 102, and a sensor device 103. Each device is connected by a bus 104.
The arithmetic device 101 is a processor such as a CPU (Central Processing Unit). The storage device 102 is, for example, a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an HDD (Hard Disk Drive), an SSD (Solid State Drive), or the like. Alternatively, a part or all of the storage device may be installed outside and communicated via the Internet, an intranet, or the like. The sensor device 103 is configured using a triaxial acceleration sensor and a triaxial angular velocity sensor. The angular velocity sensor is also called a gyroscope or a gyro sensor.
Note that the configuration of FIG. 1 is a minimum configuration, and an imaging device or the like may be added, for example, in order to realize a function of an absolute position and orientation estimation unit described later.

FIG. 2 is a configuration diagram of the position and orientation estimation apparatus according to the first embodiment.
As illustrated, the position / orientation estimation apparatus includes a sensor device 103, a map database 1, an absolute position / orientation estimation unit 2, a position / orientation storage unit 3, a traveling direction determination unit 4, a road end distance calculation unit 5, and a road end distance storage unit. 6, a movement amount calculation unit 7, a position update unit 8, and an attitude update unit 9 are provided.
The sensor device 103 is the sensor device 103 in the hardware configuration diagram of FIG. Further, the absolute position / orientation estimation unit 2, the traveling direction determination unit 4, the road edge distance calculation unit 5, the movement amount calculation unit 7, the position update unit 8, and the attitude update unit 9 in FIG. Programs corresponding to the respective functional units are stored, and are realized by executing these programs by the arithmetic unit 101. Alternatively, any of these functional units may be configured by hardware. Further, the map database 1, the position / orientation storage unit 3, and the road edge distance storage unit 6 are provided in the storage device 102. However, these configurations are merely examples, and for example, the map database 1 may be provided outside and transferred to the position and orientation estimation apparatus via the Internet, an intranet, or the like.

In the following description, the position / orientation estimation apparatus is described as being used for position / orientation estimation of a mobile terminal held by a pedestrian, but may be applied to various mobile objects other than pedestrians.
In the following description, “position” indicates a position in a two-dimensional space or a three-dimensional space, and “posture” indicates a posture (also referred to as tilt or rotation) in a two-dimensional space or a three-dimensional space. In the case of a two-dimensional space, only the position and orientation of the horizontal plane are represented. In the case of a three-dimensional space, vertical (height) information is represented in addition to the horizontal plane. The “world coordinate system” in the description is a coordinate system whose origin (reference coordinate) is known. For example, a plane rectangular coordinate system determined by the Geospatial Information Authority of Japan is applicable, but the origin may be determined independently. The unit of the world coordinate system may be, for example, meters or millimeters, but is not limited thereto. “Road edge” means the edge of a road, and in the case of a tunnel, it means a wall surface.

The map database 1 is a database in which road shape information in the world coordinate system is recorded. The road shape information represents the shape of the road edge.
The absolute position / orientation estimation unit 2 is configured to estimate an absolute position and orientation in the world coordinate system of the position / orientation estimation apparatus. An existing method can be used as the position and orientation estimation means. For example, a dedicated marker whose installation position is known or other image recognizable object may be photographed by an imaging device (not shown), the marker or object may be recognized by image processing, and the position and orientation of the position and orientation estimation device may be estimated. . Alternatively, the position / orientation estimation device may be a positioning device that uses radio waves such as GPS (Global Positioning System), BLE (Bluetooth (registered trademark) Low Energy), UWB (Ultra Wide Band), and wireless LAN (also referred to as Wi-Fi). A method of estimating the position and estimating the posture of the position and posture estimation apparatus using an acceleration sensor and a geomagnetic sensor may be used.

The position and orientation storage unit 3 is a storage unit for storing the position and orientation values calculated by the absolute position and orientation estimation unit 2, the position update unit 8, and the posture update unit 9. That is, the position / orientation storage unit 3 is a storage unit that holds the current position and orientation (hereinafter referred to as the current position and current orientation) in the position / orientation estimation apparatus.
The traveling direction determination unit 4 determines the traveling direction of the position and orientation estimation device on the road based on the position and orientation of the position and orientation estimation device stored in the position and orientation storage unit 3, and notifies the position update unit 8 of the determination result. It is configured to
Based on the current position of the position / orientation estimation device estimated by the absolute position / orientation estimation unit 2, the road edge distance calculation unit 5 is a distance from the road end of the position / orientation estimation device (hereinafter referred to as a road end distance), and a distance. The position of a road end (hereinafter referred to as a reference road edge) serving as a reference of the vehicle is calculated. The road edge distance storage unit 6 is a storage unit that stores the road edge distance calculated by the road edge distance calculation unit 5 and the value of the reference road edge.
The movement amount calculation unit 7 calculates the value of the current position after the set time has elapsed based on the value of the acceleration sensor of the sensor device 103. As the movement amount, the movement of the user who owns the position / orientation estimation device is calculated. It is configured to calculate the quantity. As a calculation method, existing means based on pedestrian autonomous navigation technology can be used. The movement amount calculation unit 7 may output the walking amount in units of one step, or may output the walking amount for one step divided by time.
The position update unit 8 includes a current position / posture of the position / posture estimation device stored in the position / posture storage unit 3, a road end distance stored in the road end distance storage unit 6, a travel direction by the travel direction determination unit 4, and movement Based on the walking amount calculated by the amount calculation unit 7, the position of the position and orientation estimation device is updated, and the current position stored in the position and orientation storage unit 3 is updated.
The posture update unit 9 updates the value of the current posture of the position and posture estimation device based on the value of the angular velocity sensor of the sensor device 103 and the current posture of the position and posture estimation device stored in the position and posture storage unit 3. The current posture stored in the storage unit 3 is updated.

Next, the operation of the position / orientation estimation apparatus according to Embodiment 1 will be described.
The position / orientation estimation apparatus performs parallel processing by the absolute position / orientation estimation thread of FIG. 3, the traveling direction determination thread of FIG. 4, the attitude update thread of FIG. 5, and the position update thread of FIG.
First, the processing of the absolute position / posture estimation thread by the absolute position / posture estimation unit 2 and the road edge distance calculation unit 5 shown in FIG. 3 will be described.
First, the absolute position / orientation estimation unit 2 estimates an absolute position / orientation in the world coordinate system of the position / orientation estimation apparatus (step ST301). As the absolute position / orientation estimation method, an existing method can be used as described above. If the estimation is successful (step ST302-Yes), the process proceeds to the next step ST303. If the estimation is unsuccessful (step ST302-No), step ST301 is executed again.

Next, the absolute position / posture estimation unit 2 updates the current position and the current posture of the position / posture estimation apparatus stored in the position / posture storage unit 3 to the position / posture estimated in step ST301 (step ST303). In addition, when the position and orientation are not stored in the position and orientation storage unit 3, the estimation result of the absolute position and orientation estimation unit 2 is stored (position and orientation storage step).
Next, the road edge distance calculation unit 5 determines a reference road edge based on the position and orientation estimated in step ST301 (step ST304). A method for calculating the reference road edge will be described with reference to FIG. FIG. 7 is a diagram in which road shapes on the map database 1 are plotted on a two-dimensional plane. A road 701 is a road on the map database 1, and a road edge 702 and a road edge 703 are road edges of the road 701. Since the road direction is generally called “up” or “down”, the direction of the road is also called “up” or “down” in this description. In order to distinguish the road edge 702 from the road edge 703, when standing on the road facing upward, the road edge on the left side is called the left road edge, and the road edge on the right side is called the right road edge. And In FIG. 7, the road edge 702 is the left road edge, and the road edge 703 is the right road edge. A current position 704 indicates the current position of the position / orientation estimation apparatus stored in the position / orientation storage unit 3. In step ST303, the road edge distance calculation unit 5 determines whether the road edge closest to the current position 704 is the left road edge or the right road edge. Therefore, the position of both road ends closest to the current position 704, the road edge position 705 and the road edge position 706 are calculated, and the distance from the current position 704 is calculated (road edge distance calculating step). In FIG. 7, since the left road end position 705 is closer to the current position 704 than the right road end position 706, the reference road end is determined to be the left road end. Next, the road edge distance calculation unit 5 stores the distance between the reference road edge determined in step ST303 and the current position 704 and the reference road edge in the road edge distance storage unit 6 (step ST305). Thereafter, the process proceeds to step ST301, and the processes in steps ST301 to ST305 are repeated.
The above is the processing of the absolute position / posture estimation thread.

Next, processing of a traveling direction determination thread by the traveling direction determination unit 4 illustrated in FIG. 4 will be described.
First, the traveling direction determination unit 4 acquires the current position and orientation of the position and orientation estimation apparatus stored in the position and orientation storage unit 3 (step ST401). If the current position / orientation value cannot be acquired because the position / orientation is not yet stored in the position / orientation storage unit 3 (step ST402-No), the process returns to step ST401. On the other hand, when the current position and orientation can be acquired in step ST402 (step ST402-Yes), the traveling direction determination unit 4 acquires the reference road edge stored in the road edge distance storage unit 6 (step ST403).
Since the road edge distance storage unit 6 has not yet stored the reference road edge, when the traveling direction determination unit 4 cannot acquire the current reference road edge (step ST404-No), the process returns to ST401. On the other hand, when the current reference road edge can be acquired in step ST404 (step ST404-Yes), the traveling direction determination unit 4 calculates the road direction (hereinafter referred to as road direction) at the current position of the position / orientation estimation apparatus. (Step ST405).

The road direction is represented by an angle (azimuth) in the up or down direction. In this description, the angle is an upward direction angle. A road direction calculation method will be described with reference to FIG. FIG. 8 is a diagram in which the road shape on the map database 1 is plotted on a two-dimensional plane, as in FIG. In this description, the reference road edge acquired in step ST403 is assumed to be the road edge 702 of the left road edge. In step ST405, first, the position of the reference road end closest to the current position is calculated. In FIG. 8, it is assumed that the road edge position 705 is the position of the reference road edge closest to the current position. And the angle 801 to the up direction in the road edge position 705 is calculated, and this angle is made into a road direction. In FIG. 8, the x-axis direction is used as a reference for the angle, but in practice, an arbitrary angle can be used as a reference.
Next, the advancing direction determination unit 4 calculates the direction of the position / orientation estimation apparatus (hereinafter referred to as apparatus direction) based on the current attitude of the position / orientation estimation apparatus (step ST406). For example, if the user's mobile terminal is a tablet terminal or a smartphone terminal, the device direction is defined as the device direction. The device direction is represented by an angle (azimuth) in the same manner as the road direction in step ST405.

Next, the traveling direction determination unit 4 determines whether the position / orientation estimation device is facing upward or downward with respect to the road (step ST407: traveling direction determination step). As this determination method, first, the difference between the road direction calculated in step ST405 and the device direction calculated in step ST406 is calculated. Since the difference between the road direction and the device direction is an angle, the calculation result is normalized from -180 ° to 180 °. Next, if the difference between the two directions is within the range of −90 ° to 90 °, the position / orientation estimation apparatus determines that the vehicle is in the upward direction with respect to the road. Or, if the difference between the two directions is outside the range of -90 ° to 90 °, that is, within the range of -180 ° to -90 °, or within the range of 90 ° to 180 °, the position / orientation estimation device On the other hand, it is determined that it faces the down direction. The position update unit 8 is notified of the determination result of the traveling direction determination unit 4. After the traveling direction is determined in step ST407, the process proceeds to step ST401, and the processes in steps ST401 to ST407 are repeated.
The above is the process of the traveling direction determination thread by the traveling direction determination unit 4.

Next, processing of the posture update thread by the posture update unit 9 shown in FIG. 5 will be described.
The posture update unit 9 first acquires the current posture of the position / posture estimation apparatus stored in the position / posture storage unit 3 (step ST501). If the current posture cannot be acquired because the posture is not yet stored in the position / posture storage unit 3 (step ST502-No), the process returns to step ST501. On the other hand, when the current posture can be acquired in step ST502 (step ST502-Yes), the posture update unit 9 acquires the angular velocity from the angular velocity sensor of the sensor device 103 (step ST503). Next, the posture update unit 9 updates the value of the current posture based on the current angular velocity acquired in step ST503 with respect to the current posture acquired in step ST501, and notifies the position / posture storage unit 3 of the result. (Step ST504). Thereafter, the process returns to step ST501, and the processes of step ST501 to step ST504 are repeated.
The above is the processing of the posture update thread by the posture update unit 9.

Next, processing of the location update thread by the location update unit 8 shown in FIG. 6 will be described.
First, the position update unit 8 acquires the current position of the position / orientation estimation apparatus stored in the position / orientation storage unit 3 (step ST601). If the current position cannot be acquired because the current position is not yet stored in the position / orientation storage unit 3 (step ST602-No), the process returns to ST601. On the other hand, when the current position can be acquired in step ST602 (step ST602-Yes), the movement amount calculation unit 7 acquires the acceleration value from the acceleration sensor of the sensor device 103 (step ST603). Next, the movement amount calculation unit 7 calculates a walking amount based on the acceleration acquired in step ST603 (step ST604: movement amount calculation step).

As described above, the existing method based on the pedestrian autonomous navigation technology can be used to calculate the walking amount. When the movement amount calculation unit 7 determines in step ST605 that the user is not walking (step ST605-No), the process returns to step ST601. On the other hand, when the movement amount calculation unit 7 determines in step ST605 that the user is walking (step ST605-Yes), the position update unit 8 stores the reference road edge and the road stored in the road edge distance storage unit 6. An end distance is acquired (step ST606). If the reference road edge and the road edge distance cannot be acquired because the reference road edge and the road edge distance are not yet stored in the road edge distance storage unit 6 (step ST607—No), the process returns to step ST601. On the other hand, when the position update unit 8 can acquire the reference road edge and the road edge distance in step ST607 (step ST607-Yes), the position update unit 8 determines the travel direction of the position and orientation estimation device determined by the travel direction determination unit 4. Is acquired (step ST608). If the traveling direction determination unit 4 has not yet determined the traveling direction, and the traveling direction cannot be acquired (step ST609-No), the process returns to ST601. On the other hand, when the traveling direction can be acquired in step ST609 (step ST609-Yes), the position update unit 8 determines the walking amount calculated by the movement amount calculation unit 7 in step ST604, the reference road edge and the road acquired in step ST606. Based on the end distance and the traveling direction acquired in step ST608, the position of the position / orientation estimation apparatus is updated (step ST610: position update step).

The position updating method in step ST610 will be described with reference to FIG. FIG. 9 is a diagram in which road shapes on the map database 1 are plotted on a two-dimensional plane. A road 901 is a road on the map database 1, and a road edge 902 and a road edge 903 are road edges of the road 901. In the example of FIG. 9, the reference road edge is the road edge 902 of the left road edge. A position 904 is a current position of the position / orientation estimation apparatus stored in the position / orientation storage unit 3. The road edge distance 905 is a road edge distance stored in the road edge distance storage unit 6. In the example of FIG. 9, it is assumed that the traveling direction is the upward direction. In this situation, the position update unit 8 moves the distance of the walking amount 906 from the current position 904 in the upward direction, and calculates the update position 907 that is the road edge distance 905 from the reference road edge 902. Then, the update position 907 is notified to the position / orientation storage unit 3 to update the position of the position / orientation estimation apparatus.

In step ST610, regarding the handling of the walking amount 906 between the current position 904 and the update position 907, either a distance along the shape of the reference road edge 902 or a simple linear distance can be considered. It may be adopted. This difference will be described with reference to FIGS. 10 and 11 are diagrams in which road shapes on the map database 1 are plotted on a two-dimensional plane, as in FIG. 9, but FIGS. 10 and 11 illustrate roads having shapes different from those in FIG. . FIG. 10 shows a method of calculating the update position with the distance along the shape of the reference road edge 902. When calculating the position at a distance along the shape of the reference road edge 902, first, a shape 1001 moved from the reference road edge 902 to the center side of the road by the distance of the road edge distance 905 is calculated. Then, an update position 1003 that is moved from the current position 904 by the walking amount 1002 on the shape 1001 is calculated. On the other hand, FIG. 11 shows a method of simply calculating the update position with a linear distance. When the update position is calculated based on the straight line distance, the update position 1102 that is the road edge distance 905 moved from the current position 904 by the walking distance 1101 by the straight line distance is calculated. If the road edge shape is a straight line, there is no difference in either calculation method, but if the road edge distance is a curve, the update position differs depending on the distance calculation method. When the update frequency by the position update thread is one step of walking or updating by a unit obtained by dividing one step, there is no significant difference, so either method is practical. On the other hand, when the update frequency by the position update thread is low, for example, when updating with a walking unit of two steps or more, it is closer to the actual walking distance to calculate the position with the distance along the shape of the reference road edge 902 preferable.
After updating the current position in step ST610, the position updating unit 8 moves to step ST601 and repeats the processes in steps ST601 to ST610 described above.
The above is the process of the position update thread by the position update unit 8.

With the position / orientation estimation apparatus configured as described above, the absolute position / orientation is first estimated by the absolute position / orientation estimation thread and stored in the position / orientation storage unit 3. It is stored in the end distance storage unit 6. Thereafter, the traveling direction and posture are constantly updated by the traveling direction determination thread and the posture update thread. In addition, based on the reference road edge and the road edge distance stored when the absolute position and orientation are estimated by the position update thread, the amount of walking from the current position along the road edge shape of the reference road edge toward the traveling direction is calculated. It moves, calculates the position separated from the reference road edge by the road edge distance, and updates the position stored in the position and orientation storage unit 3. Therefore, even when the absolute position and orientation cannot be obtained by the absolute position and orientation estimation unit 2, the position and orientation can be updated to the current values. In addition, the position / orientation estimation apparatus configured in this way applies the existing map matching to calculate the position along the road edge shape of the reference road edge and separated from the reference road edge by the road edge distance. Compared with the pedestrian autonomous navigation technology, the actual walking position on the road can be estimated accurately. In order to estimate an accurate position and orientation with this position and orientation estimation device, a user who possesses the position and orientation estimation device walks along the road edge in the upward or downward direction while keeping the distance from the road edge constant. However, in general, when a pedestrian walks along a roadside belt on an expressway, he / she must walk under such restrictions. The effect of being able to do is not lost.
Further, the value of the angular velocity sensor is used for the determination of the traveling direction. However, since the determination is either ascending or descending, it is acceptable even if a slight error is added to the angular velocity. Accordingly, the present invention can also be applied to an application that requires a highly accurate position and orientation of a mobile terminal, such as augmented reality.

As described above, according to the position / orientation estimation apparatus of the first embodiment, the position / orientation storage unit that holds the current position and the value of the current attitude, and the reference path that serves as a reference for the distance based on the road shape on the map A road edge distance calculation unit that obtains the distance between the end and the current position held as a road edge distance, and a traveling direction determination unit that determines the traveling direction based on the current posture held and the road edge shape of the reference road edge And a movement amount calculation unit that calculates a movement amount after the set time has elapsed, a current position that is held, a road edge distance calculated by the road edge distance calculation unit, a reference road edge, and a traveling direction determination unit. Based on the travel direction and the travel amount calculated by the travel amount calculation unit, the travel amount is moved from the current position held in the travel direction by the travel amount along the road end shape of the reference road end, and from the reference road end. Calculate the position away from the road edge distance, and the position and orientation storage unit Since a position updating unit that updates the standing position, it is possible to accurately estimate the terminal location in the road.

Further, according to the position / orientation estimation apparatus of the first embodiment, since the absolute position / orientation estimation unit that estimates the absolute position / orientation as the current position and the current attitude is provided, the terminal position in the road can be estimated more accurately. it can.

Further, according to the position / orientation estimation apparatus of the first embodiment, a new current attitude is calculated based on the current attitude stored in the position / orientation storage unit and the angular velocity value obtained by the angular velocity sensor, and the position is determined based on the new current attitude. Since the posture update unit for updating the current posture of the posture storage unit is provided, the current posture is also updated, so that the traveling direction of the terminal can be estimated more accurately.

Further, according to the position / orientation estimation method of the first embodiment, the position / orientation storage unit in which the position / orientation storage unit holds the current position and the value of the current attitude, and the road edge distance calculation unit uses the road shape on the map as a source. The road edge distance calculating step for obtaining the distance between the reference road edge serving as the reference of the distance and the current position held as the road edge distance, and the traveling direction determination unit, the road between the current posture held and the reference road edge A traveling direction determination step for determining a traveling direction based on the end shape, a movement amount calculation step for the movement amount calculation unit to calculate a movement amount after the set time has elapsed, and a position update unit for the current position held The current position is stored based on the road edge distance and reference road edge calculated in the road edge distance calculation step, the traveling direction determined in the traveling direction determination step, and the movement amount calculated in the movement amount calculation step. From position to direction of travel A position update step that moves by the amount of movement along the road edge shape of the road edge, calculates a position away from the reference road edge by the road edge distance, and updates the current position of the position and orientation storage step at the calculated position. Therefore, the terminal position in the road can be accurately estimated.

Embodiment 2. FIG.
In the second embodiment, the altitude of the current position is estimated.
FIG. 12 is a configuration diagram of the position / orientation estimation apparatus according to the second embodiment.
The illustrated position and orientation estimation apparatus includes a map database 1, an absolute position and orientation estimation unit 2 a, a position and orientation storage unit 3 a, a traveling direction determination unit 4, a road end distance calculation unit 5, a road end distance storage unit 6, and a movement amount calculation unit 7. , A position update unit 8a, a posture update unit 9, a ground height calculation unit 10, and a ground height storage unit 11.
The absolute position / posture estimation unit 2a has a function of estimating the three-dimensional position of the current position. The position / orientation storage unit 3a stores the value of the three-dimensional position of the current position including the altitude. The ground height calculation unit 10 calculates the ground height of the current position based on the height of the absolute position estimated by the absolute position / orientation estimation unit 2a and the altitude on the map in the map database 1. The ground height storage unit 11 is a storage unit that stores the ground height calculated by the ground height calculation unit 10. When updating the current position, the position update unit 8a calculates a new altitude based on the altitude on the map and the ground height of the current position stored in the ground height storage unit 11, and the position / posture storage unit 3a Is configured to update the altitude stored in Since the other configuration is the same as the configuration of the first embodiment shown in FIG. 2, the same reference numerals are given to the corresponding portions, and the description thereof is omitted.

In the second embodiment, “altitude” means a value representing an absolute height. Generally, the height is based on the sea level, but other standards may be used. The “elevation” means the altitude of the ground surface stored on the map database 1. “Ground height” means a height based on the altitude. For example, a ground height of 1 meter indicates a height of 1 meter from the ground surface.

Next, the operation of the position / orientation estimation apparatus according to the second embodiment will be described.
When the absolute position / orientation estimation unit 2a succeeds in estimating the absolute position / orientation, the absolute position / orientation estimation unit 2a outputs the estimated altitude to the position / orientation storage unit 3a and the ground height calculation unit 10. The ground height calculation unit 10 calculates the ground height of the position / orientation estimation device based on the altitude acquired from the absolute position / orientation estimation unit 2 a and the altitude on the map database 1, and stores it in the ground height storage unit 11.
When updating the current position, the position updating unit 8a obtains an altitude on the map database 1 based on the position of the two-dimensional plane obtained by the processing of the first embodiment. Then, the position update unit 8a sets the altitude obtained by adding the ground height stored in the ground height storage unit 11 to the altitude as the height of the position and orientation estimation device, and stores the height in the position and orientation storage unit 3a. Since other operations are the same as those in the first embodiment, description thereof is omitted here.

When the three-dimensional absolute position / orientation is estimated by the absolute position / orientation estimation unit 2a by the position / orientation estimation apparatus configured as described above, the ground height of the position / orientation estimation apparatus is stored. Further, when the position is updated by the position update unit 8a, the altitude on the map database 1 is updated to the altitude obtained by adding the ground height stored when the absolute position and orientation is estimated. Therefore, the position update unit 8a calculates the actual altitude unless the user who owns the position / orientation estimation apparatus changes the height of the position / orientation estimation apparatus. Further, since the position updating unit 8a calculates the altitude based on the altitude on the map database 1, it can calculate the accurate altitude even if the road is not flat and has irregularities and gradients.

As described above, according to the position / orientation estimation apparatus of the second embodiment, the absolute position / orientation estimation unit estimates the absolute position including the altitude, and based on the absolute position altitude and the altitude on the map, A ground height calculation unit that calculates the ground height of the position is provided, and the position update unit calculates a new altitude based on the altitude on the map and the ground height of the current position when updating the current position. Therefore, the terminal position in the road including the altitude can be accurately estimated.

In the present invention, within the scope of the invention, any combination of each embodiment, any component of each embodiment can be modified, or any component in each embodiment can be omitted. .

As described above, the position / orientation estimation apparatus and position / orientation estimation method according to the present invention relate to a configuration for estimating the position and orientation of a mobile terminal or the like carried by a pedestrian, and are used, for example, for drawing in an augmented reality application. Suitable for

1 Map database, 2, 2a Absolute position / orientation estimation unit, 3, 3a Position / orientation storage unit, 4 Travel direction determination unit, 5 Road end distance calculation unit, 6 Road end distance storage unit, 7 Travel amount calculation unit, 8, 8a Position update unit, 9 attitude update unit, 10 ground height calculation unit, 11 ground height storage unit.

Claims

A position and orientation storage unit that holds values of the current position and current posture;
A road edge distance calculation unit for obtaining a distance between a reference road edge serving as a distance reference based on the road shape on the map and the current position held as the road edge distance;
A traveling direction determination unit that determines a traveling direction based on the current posture held and the road edge shape of the reference road edge;
A movement amount calculation unit for calculating a movement amount after the set time has elapsed;
The current position held, the road edge distance calculated by the road edge distance calculation unit, the reference road edge, the traveling direction determined by the traveling direction determination unit, and the movement amount calculation unit are calculated. A position that moves by the amount of movement along the road end shape of the reference road end in the advancing direction from the held current position based on the amount of movement that has been held, and is separated from the reference road end by the road end distance And a position update unit that updates the current position of the position and orientation storage unit with the calculated position.
The position / orientation estimation apparatus according to claim 1, further comprising an absolute position / orientation estimation unit configured to estimate an absolute position / orientation as the current position and the current attitude.
A posture update unit that calculates a new current posture based on the current posture stored in the position and posture storage unit and the angular velocity value obtained by the angular velocity sensor, and updates the current posture of the position and posture storage unit with the new current posture. The position / orientation estimation apparatus according to claim 1, further comprising:
The absolute position and orientation estimation unit estimates an absolute position including altitude,
Based on the absolute position altitude and the altitude on the map, a ground height calculation unit that calculates the ground height of the current position,
The position / orientation estimation apparatus according to claim 2, wherein the position update unit calculates a new altitude based on an altitude on a map and a ground height of the current position when updating the current position.
A position and orientation storage step in which the position and orientation storage unit holds the current position and the value of the current orientation;
A road edge distance calculation unit, wherein a road edge distance calculation unit obtains a distance between a reference road edge serving as a distance reference based on a road shape on a map and the current position held as a road edge distance; and
A traveling direction determination unit, wherein the traveling direction determination unit determines a traveling direction based on the current posture held and the road edge shape of the reference road edge;
A movement amount calculating unit that calculates a movement amount after the set time has elapsed; and
The position update unit is configured to store the current position, the road edge distance calculated in the road edge distance calculation step, the reference road edge, the traveling direction determined in the traveling direction determination step, and the movement amount. Based on the movement amount calculated in the calculation step, the movement amount moves along the road edge shape of the reference road edge in the traveling direction from the held current position, and from the reference road edge to the road. A position / orientation estimation method comprising: a position update step of calculating a position separated by an end distance and updating the current position of the position / orientation storage step at the calculated position.