JP5512258B2 - Orientation measuring apparatus, orientation measuring system, orientation measuring method, and orientation measuring program - Google Patents

Description

  The present invention relates to an orientation measurement device, an orientation measurement system, an orientation measurement method, and an orientation measurement program.

  In recent years, technologies related to robots that provide services to people in areas where people are active have been known. Patent Document 1 uses a particle filter to estimate the position and moving speed of a person, and uses a human shape model in which the torso and both arms are combined in three circles, A measuring device for estimating is described.

  On the other hand, in the technical field of mobile robots, a technique is known in which a mobile robot detects its own position. For example, in Patent Document 2, a state quantity that changes due to the movement of a mobile robot is sensed, a distance between the mobile robot and a fixed position is sensed to calculate the absolute position of the mobile robot, A mobile robot is described that estimates the optimum position of the current mobile robot using the absolute position as an input of the Kalman filter.

JP 2009-168578 A JP 2007-149088 A

However, in the technique described in the cited document 1, a human shape model can be applied when a person cannot be imaged from directly above or when a person is located far away and an image with sufficient image quality cannot be obtained. Therefore, there was a drawback that the direction of the human body could not be measured. Further, the technique described in the cited document 1 measures the direction of the upper body. For example, when the body is twisted at the waist, there is a disadvantage that the lower body, that is, the direction of the foot cannot be measured.
Further, in the technique described in the cited document 2, if each mobile robot does not have a function for sensing distance and a function for estimating an optimum position, the position cannot be measured accurately, and the direction cannot be measured accurately. was there.
As described above, the conventional technique has a drawback in that the orientation of the mobile device may not be accurately measured.

  The present invention has been made in view of the above points, and provides an orientation measurement device, an orientation measurement system, an orientation measurement method, and an orientation measurement program that can accurately measure the orientation of a moving device.

The present invention has been made in order to solve the above-described problems. The present invention is directed to the amount of change in the position of the device itself (for example, relative position information in the embodiment) and the amount of change in direction (for example, relative in the embodiment). In a direction measuring device (for example, the position / direction correcting unit 116 in the embodiment) mounted on a mobile device (for example, the mobile robot 1 in the embodiment) that detects the direction information), the amount of change in the position is integrated. A position integrator (e.g., a position integrator 1162 in the embodiment) for estimating estimated position information indicating the position of the mobile device, and estimated direction information (e.g., indicating the direction of the mobile device by integrating the amount of change in the direction). A direction integration unit (e.g., direction integration unit 1161 in the embodiment) that estimates the estimated front direction information in the embodiment) and a position measurement device (e.g., actual The position information received from the absolute position measurement device c1) in the embodiment, which is absolute position information indicating the position of the moving device from moment to moment, and the estimated position information from moment to moment are identified, and the deviation in direction is corrected. A direction correction value calculation unit (for example, a position correction unit 1163 in the embodiment) that calculates a direction correction value and a direction correction unit (for example, an embodiment) that corrects the direction indicated by the estimated direction information using the direction correction value. The direction correction unit 1164) and the movement device determines that it has moved 2 to 3 m from the time of correcting the previous direction, the direction correction value calculation unit starts processing to calculate the direction correction value, and the direction correction start command unit for starting the process of correcting the direction indicated by the estimated direction information to the correction unit (for example, the correction start command unit 119 in the embodiment) equipped with a, the position measurement Location is a direction measuring device according to claim Tei Rukoto installed in buildings.

  In the orientation measuring device according to the present invention, the position measuring device is installed on a ceiling, and the absolute position information is position information transmitted by one position measuring device.

In addition, the present invention measures an orientation measurement device mounted on a moving device that detects the amount of change in the position of the device itself and the amount of change in direction, and absolute position information that indicates the position of the moving device every moment. In a direction measurement system (for example, the direction measurement system S1 in the embodiment) that includes a position measurement device that transmits to the direction measurement device, the direction measurement device integrates the amount of change in the position, and A position estimating unit that estimates estimated position information indicating a position, a direction estimating unit that estimates estimated direction information indicating the direction of the mobile device by integrating the amount of change in the direction, and an absolute position received from the position measuring device Information and the estimated position information for each moment are identified, a direction correction value calculation unit for calculating a direction correction value for correcting a shift in direction, and the estimated direction information indicates by using the direction correction value. Direction The direction correcting unit for correcting the, if the mobile device is determined to have 2~3m moved from the time of correcting the previous direction, to start the process of calculating the direction correction value in the direction correction value calculation unit, the direction comprising a correction start command unit for starting the process of correcting the direction indicated by the estimated direction information to the correction unit, wherein the position measuring device is an orientation measuring system characterized Tei Rukoto installed in buildings.

Further, according to the present invention, in the orientation measurement method in the orientation measurement device mounted on the moving device that detects the change amount of the position of the device itself and the change amount of the direction, the position integration unit integrates the change amount of the position. A process of estimating estimated position information indicating the position of the mobile device, a process of estimating the estimated direction information indicating the direction of the mobile device by a direction integrating unit integrating the amount of change in the direction, and direction correction The value calculating unit identifies the absolute position information indicating the position of the moving device from moment to moment and the position information received from the position measuring device outside the moving device, and the estimated position information from moment to moment. A process of calculating a direction correction value for correcting the deviation, a process of the direction correction unit correcting the direction indicated by the estimated direction information using the direction correction value, and a correction start command unit, 2 to 3m from the time of correcting the direction If it is determined that the dynamic, chromatic said to initiate the process of calculating the direction correction value in the direction correction value calculation unit, and a process of starting the process of correcting the direction indicated by the estimated direction information in the direction correction unit, the and, wherein the position measuring device is oriented measurement method characterized by Tei Rukoto installed in buildings.

Further, the present invention provides a computer of an orientation measuring device mounted on a moving device that detects the amount of change in the position of the device and the amount of change in the direction, and integrates the amount of change in the position to determine the position of the moving device. Position estimation means for estimating the estimated position information, direction estimation means for integrating the amount of change in the direction to estimate estimated direction information indicating the direction of the mobile device, and position information received from a position measurement device outside the mobile device. Direction correction value calculating means for identifying the absolute position information indicating the position of the moving device from moment to moment and the estimated position information from moment to moment, and calculating a direction correction value for correcting a deviation in direction, the direction Direction correction means that corrects the direction indicated by the estimated direction information using the correction value, and when it is determined that the moving device has moved 2 to 3 m from the time when the previous direction was corrected, the direction correction value calculation means receives the direction correction To initiate the process of calculating the said direction correction means and the estimated direction information to function as a correction start command means for starting the process of correcting the direction indicated in the position measuring device, the orientation measurement Ru Tei is installed in a building It is a program.

According to the present invention, the orientation measuring device estimates the estimated position information indicating the position of the moving device by integrating the amount of change in the position, and the estimated direction information indicating the direction of the moving device by integrating the amount of change in the direction. presume. In addition, the orientation measuring device identifies the positional information received from a device outside the moving device, which is the absolute position information indicating the position of the moving device every moment, and the estimated position information every moment, and the direction deviation is detected. A direction correction value to be corrected is calculated, and the direction indicated by the estimated direction information is corrected using the direction correction value. Thereby, the direction measuring device can accurately measure the direction of the moving device.

Further, according to the present invention, the direction measuring device calculates a direction correction value using the absolute position information and the estimated position information at a plurality of times. Thereby, the direction measuring device can measure the direction of the moving device with higher accuracy than the case of using one piece of absolute position information and estimated position information.

Further, according to the present invention, the direction measuring device determines the timing for starting the process of calculating the direction correction value based on the distance moved by the moving device. Thereby, the orientation measuring device can correct the orientation of the mobile station device according to the distance traveled by the mobile device.

It is the schematic which shows direction measurement system S1 which concerns on embodiment of this invention. It is a schematic block diagram which shows the structure of the mobile robot which concerns on this embodiment. It is the schematic which shows the relationship between the estimated position which concerns on this embodiment, an absolute position, and an estimated front direction. It is the schematic showing an example of the correction value calculation process which concerns on this embodiment. It is the schematic which shows an example of the timing which performs the position direction correction process which concerns on this embodiment.

(First embodiment)
Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
FIG. 1 is a schematic diagram showing an orientation measurement system S1 according to an embodiment of the present invention. The direction measuring system S1 includes a mobile robot 1 (moving device) and an absolute position measuring device c1.
FIG. 1 is a diagram of the interior of a building room. In this figure, the mobile robot 1 moves in a room. Here, the mobile robot 1 denoted by reference numerals 11, 12, and 13 includes a mobile robot 1 whose operation time T is the target time t, a mobile robot 1 one second before the target time t, and n seconds before the target time t. The mobile robot 1 is shown. In this figure, the locus | trajectory which attached | subjected the arrow shows the locus | trajectory which the mobile robot 1 moved.

Also, an absolute position measuring device c1 is installed on the ceiling of the room in FIG. The absolute position measuring device c1 has an imaging function, and also has a function of measuring position information (referred to as absolute position information) of the mobile robot 1 based on the captured image. Here, the absolute position measuring device c1 uses the measured absolute position information of the mobile robot 1 as a value on coordinates (X axis, Y axis; global coordinates in FIG. 1) perpendicular to the direction of gravity (horizontal to the floor). taking measurement.
Specifically, the absolute position measurement device c1 is installed at a sufficiently high position, and images the mobile robot 1 from almost directly above. The absolute position measuring device c1 performs circle approximation on the mobile robot 1 portion of the captured image, and sets the center of the circle as absolute position information of the mobile robot 1. The absolute position measurement device c1 moves information including the measured absolute position information and the absolute position measurement time indicating the measured time every unit time (in this embodiment, 1 second) using wireless communication or the like. Transmit to the robot 1.
A sensor that is installed in a building or the like and measures the position information of an object, such as the absolute position measurement device c1, is referred to as an infrastructure sensor. The absolute position measurement device c1 may be any device that can measure the position information of an object, and may be a device that measures using laser light, for example. Further, it may be installed in a place other than the ceiling, or may be a plurality of devices installed in different positions.

Further, marks M11 to M14 are attached to the floor of the room in FIG. Each of the marks M11 to M14 is two lines parallel to the Y axis of the global coordinates, and each line has a different color (for example, a line having a large value on the X axis is a red line and a line having a small value) Is the blue line). The mobile robot 1 captures an image of the floor with a camera provided on the side surface of the waist portion, and detects any of the marks M11 to M14 by image recognition. The mobile robot 1 stores in advance an image of a line that is perpendicular to the front direction (traveling direction) in the image to be captured, and the angle difference between the image of the line and the detected line image is expressed as X on the global coordinates. Calculated as the angle (θ _z ) with respect to the axis. That is, this angle is an angle indicating the front direction (traveling direction) in the global coordinates of the mobile robot 1.
In the global coordinates, the direction perpendicular to the line and going from the blue line to the red line is the X axis direction, and the axis obtained by rotating the X axis by 90 degrees is the Y axis. The marks M11 to M14 may be any marks as long as the mobile robot 1 can detect the direction in the global coordinates used by the absolute position measuring device c1, and may be marks having other shapes. Further, the mobile robot 1 may be provided with an azimuth magnetic needle and calculate the front direction based on the relationship between a predetermined azimuth and global coordinates.

<Configuration of mobile robot 1>
FIG. 2 is a schematic block diagram showing the configuration of the mobile robot 1 according to the present embodiment. In this figure, the mobile robot 1 includes a gyroscope 101, a drive unit 102, a communication unit 103, a direction detection unit 104, and a position / direction control unit 11. The position / direction control unit 11 includes an encoder unit 111, a relative direction information storage unit 112, a relative position information storage unit 113, an absolute position information storage unit 114, an initial direction information storage unit 115, a position / direction correction unit 116, and position / direction information. A storage unit 117, a drive control unit 118, and a correction start command unit 119 are included. Here, the position / direction correction unit 116 (orientation measurement device) includes a direction integration unit 1161, a position integration unit 1162, a position correction unit 1163, and a direction correction unit 1164.

The gyroscope 101 is a measuring instrument that detects an angle and an angular velocity. The gyroscope 101 outputs the detected angle and angular velocity per unit time to the encoder unit 111.
The drive unit 102 moves the mobile robot 1 by moving the moving means of the mobile robot 1 (both legs in the case of a biped robot) under the control of the drive control unit 118. The drive unit 102 outputs the operation time T and the operation result information to the encoder unit 111 at predetermined time intervals (referred to as drive detection time; for example, 1/200 second).
Here, the motion result information is the travel direction and travel distance at the drive detection time at the motion time T. For example, if the mobile robot 1 is a biped robot, which direction the foot is moved at the motion time T It is information indicating which distance has been stepped on. Note that the traveling direction of the operation time T (the direction in which the foot is stepped on) is a direction relative to the front direction. For example, when the mobile robot 1 is a wheel-type mobile robot, the operation result information is information indicating the rotation direction and the rotation speed for each wheel.

The communication unit 103 performs wireless communication with the absolute position measurement device c1. The communication unit 103 receives information including the absolute position information and the absolute position measurement time from the absolute position measurement device c1, and stores the information in the absolute position information storage unit 114.
The direction detection unit 104 calculates the front direction of its own device based on the global coordinates used by the absolute position measurement device c1 based on the marks M11 to M14 in FIG. The direction detection unit 104 stores information including the initial direction information indicating the calculated front direction and the initial direction calculation time indicating the time when the initial direction information is calculated in the initial direction information storage unit 115.

Based on the operation result information input from the drive unit 102, the encoder unit 111 has relative direction information indicating a difference in direction at the previous and subsequent operation times T and the position at the previous and subsequent operation times T for the mobile robot 1. The relative position information indicating the difference is calculated. That is, the encoder unit 111 detects the change amount of the position of the mobile robot 1 and the change amount of the direction.
Note that the encoder unit 111 may increase the accuracy of the relative direction information using the angle and the angular velocity input from the gyroscope 101. The encoder unit 111 stores information including the operation time T and the calculated relative direction information in the relative direction information storage unit 112, and stores information including the operation time and the calculated relative position information in the relative position information storage unit 113. To do.

The direction integration unit 1161 receives the reference direction information and the reference direction measurement time from the direction correction unit 1164. The reference direction information is information indicating a direction in global coordinates, and is information indicating the front direction of the mobile robot 1 at the reference direction measurement time.
The direction integration unit 1161 reads information including the operation time T and the relative direction information from the relative direction information storage unit 112. The direction integration unit 1161 estimates the front direction at the operation time T in the global coordinates of the mobile robot 1 by adding the relative direction information from the reference direction measurement time to the operation time T using the reference direction information as an initial value. . The direction integration unit 1161 outputs information including estimated front direction information (estimated direction information) indicating the estimated front direction (estimated front direction) and the operation time T for each operation time T of the drive detection time, and a direction correction unit 1164. Output to.

The position integration unit 1162 receives the reference position information and the reference position measurement time from the position correction unit 1163. The reference position information is position information in global coordinates, and is information indicating the position of the mobile robot 1 at the reference position measurement time.
The position integration unit 1162 reads information including the operation time and the relative position information from the relative position information storage unit 113. The position integration unit 1162 estimates the position of the mobile robot 1 at the operation time T in the global coordinates by adding the relative position information from the reference position measurement time to the operation time T using the reference position information as an initial value. The direction integration unit 1161 outputs, to the position correction unit 1163 and the correction start command unit 119, information including estimated position information indicating the estimated position (referred to as an estimated position) and the operation time T for each operation time T of the drive detection time. To do.

The position correction unit 1163 (direction correction value calculation unit) is estimated position information for each unit time out of the information input from the position integration unit 1162, and the operation time T is in the period from the target time t to the time t−n. Extract estimated position information. In addition, the position correction unit 1163 selects and reads out the absolute position information in the period from the target time t to the time t−n from the information stored in the absolute position information storage unit 114.
The position correction unit 1163 identifies the extracted estimated position information and the read absolute position information, and calculates the deviation between the estimated position and the absolute position for the period from the target time t to the time t−n. The position correction unit 1163 calculates a correction value (ΔX, ΔY) for correcting the deviation of the estimated position and a correction value (Δθ _z ; direction correction value) for correcting the deviation of the direction with respect to the calculated deviation. The details of the correction value calculation processing will be described together with the position / direction correction processing described later. The position correction unit 1163 outputs the calculated direction correction value to the direction correction unit 1164.

  Further, the position correction unit 1163 outputs the absolute position information and the target time t whose absolute position measurement time is the target time t to the position integration unit 1162 as the reference position information and the reference position measurement time, respectively. Here, when the position integration unit 1162 starts processing for the first time, the position correction unit 1163, as the reference position information and the reference position measurement time, respectively stores the absolute position information and the absolute position measurement stored in the absolute position information storage unit 114. The time is output to the position integration unit 1162. Further, the position correction unit 1163 outputs information read from the absolute position information storage unit 114 to the direction correction unit 1164.

The direction correction unit 1164 extracts the estimated front direction information during the period from the target time t to the time t−n from the information input from the direction integration unit 1161. The direction correction unit 1164 adds the direction correction value input from the position correction unit 1163 to the estimated front direction indicated by the extracted estimated front direction information. That is, the direction correction unit 1164 corrects the direction indicated by the estimated front direction information using the direction correction value. Hereinafter, the direction after the addition is referred to as a correction direction, and information indicating the correction direction is referred to as correction direction information.
The direction correction unit 1164 calculates the correction direction information, the operation time T (from the target time t to the time t-n), and the absolute position information at the operation time T input from the position correction unit 1163, as the position at the operation time T. The direction information is stored in the position / direction information storage unit 117 as direction information. That is, the position / direction information storage unit 117 stores the position / direction information in which the position and the direction are corrected for the operation time T in the period from the target time t to the time t−n.

Further, the direction correction unit 1164 outputs the correction direction information and the target time t whose operation time T is the target time t to the direction integration unit 1161 as the reference direction information and the reference direction measurement time, respectively. Here, when the direction integration unit 1161 starts processing for the first time, the direction correction unit 1164 calculates initial direction information and initial direction calculation stored in the initial direction information storage unit 115 as reference direction information and reference direction measurement time, respectively. The time is output to the direction integration unit 1161.
Note that the processing performed by the position / direction correction unit 116, that is, the direction integration unit 1161, the position integration unit 1162, the position correction unit 1163, and the direction correction unit 1164 is referred to as a position / direction correction process.

The drive control unit 118 controls the drive unit based on the position / direction information stored in the position / direction information storage unit 117.
Based on the information read from the relative position information storage unit 113, the correction start command unit 119 determines whether or not the mobile robot 1 has moved a predetermined distance since the previous position / direction correction process. If the correction start command unit 119 determines that the predetermined distance has been moved, the correction start command unit 119 outputs a signal for starting the position / direction correction process to the position correction unit 1163 and the direction correction unit 1164 of the position / direction correction unit 116.
That is, the correction start command unit 119 determines the timing for starting the process of calculating the direction correction value based on the distance traveled by the mobile robot 1. Here, the predetermined distance is preferably about 2 to 3 m. When this distance is large, the difference between the shape of the internal odometry trajectory (see FIG. 3) of the mobile robot 1 and the shape of the infrastructure sensor trajectory of the mobile robot 1 becomes large. This is because the error becomes larger.

<Position and direction correction processing>
Hereinafter, the position / direction correction processing performed by the position correction unit 1163 will be described.
FIG. 3 is a schematic diagram illustrating the relationship between the estimated position, the absolute position, and the estimated front direction according to the present embodiment. In this figure, the coordinates are global coordinates, the horizontal axis is the X axis, and the vertical axis is the Y axis.
In FIG. 3, a trajectory L11 denoted by reference symbol L11 is an estimated position in the period from the target time t to the time t−n and indicates an estimated position per unit time (start point of a white arrow) (the mobile robot 1 of the mobile robot 1). Internal odometry trajectory). Moreover, the white arrow in each estimated position shows the estimated front direction in each estimated position. On the other hand, a locus L12 denoted by reference numeral L12 is a locus (infrastructure sensor locus of the mobile robot 1; the locus shown in FIG. 1) indicating the absolute position (black circle) from the target time t to the time t−n. Further, in this figure, the estimated position and the absolute position associated with the dotted arrows are those of the same operation time T.

  For example, in FIG. 3, positions P111 and P121 denoted by reference signs P111 and P121 respectively indicate an estimated position and an absolute position at the operation time T = t−n. An arrow D121 with a reference sign D121 indicates the estimated front direction at the operation time T = t−n. Further, for example, the positions P112, P122, and the arrow D121 with the symbol D122 indicate the estimated position, the absolute position, and the estimated front direction at the operation time T = t, respectively. As shown in this figure, the estimated position may deviate from the absolute position, but the trajectories L11 and L12 have almost the same shape.

FIG. 4 is a schematic diagram illustrating an example of a correction value calculation process according to the present embodiment. In this figure, the coordinates are global coordinates, the horizontal axis is the X axis, and the vertical axis is the Y axis.
FIG. F41 denoted by reference numeral F41 is the same diagram as FIG. 3 and shows the relationship between the estimated position, the absolute position, and the estimated front direction before performing the correction value calculation process. FIG. F41 shows that the position correction unit 1163 calculates the affine transformation amounts (ΔX, ΔY, Δθ _z ).

Specifically, the position correction unit 1163 calculates the average value of the absolute position and the average value of the relative position for the period from the target time t to the time t−n, and the difference between these average values (ΔX, ΔY; position) (Referred to as a correction value). The position correction unit 1163 adds the calculated position correction values (ΔX, ΔY) to the relative position during the period from the target time t to the time t−n.
The position correction unit 1163 calculates the slope of the linear equation approximated using the least square method for the absolute position during the period from the target time t to the time t−n. Further, the position correction unit 1163 calculates the slope of the linear equation approximated using the least square method for the relative position to which the position correction value is added. Position correcting unit 1163, the difference between the angle indicated by the calculated slope (angle a variable theta _z from the X-axis), is calculated as the direction correction value [Delta] [theta] _z. However, the present invention is not limited to this, the position correcting unit 1163, [Delta] X as the sum is the minimum distance between the estimated position and the absolute position associated with an arrow, [Delta] Y, may be determined [Delta] [theta] _z.

FIG. F42 denoted by reference numeral 42 is a diagram illustrating an absolute position, an estimated position obtained by adding a position correction value, and a correction direction.
For example, in FIG. F42, a white arrow D221 with a symbol D221 indicates the correction direction at the target time t. The arrow D221 in the direction indicated by the arrow D121 of FIG. F 41, indicating the direction obtained by adding the direction correction value [Delta] [theta] _z.

FIG. 5 is a schematic diagram illustrating an example of timing for performing the position / direction correction processing according to the present embodiment. In this figure, the coordinates are global coordinates, the horizontal axis is the X axis, and the vertical axis is the Y axis. In this figure, trajectories (both broken lines and solid lines) including positions P211, P212, P213, P214, and P215 are trajectories indicating estimated positions. The trajectory including the positions P220, P221, P222, P223, P224, and P225 is a trajectory indicating an absolute position. For example, positions P211, P212, P211 and P222 are the positions P111, P112, P121 and P122 in FIG. 3, respectively.
FIG. 5 shows that the position / direction correction processing has been performed at the times P222, P223, and P225. Further, in this figure, white arrows ID1 and ID2 with reference numerals ID1 and ID2 represent directions indicated by the initial direction information detected by the direction detecting unit 104.

Hereinafter, an example of the operation of the position / direction control unit 11 will be described with reference to FIGS. 5 and 2.
The position correction unit 1163 outputs the absolute position information and the absolute position measurement time indicating the position P220 to the position integration unit 1162 as the reference position information and the reference position measurement time. Further, the direction correction unit 1164 outputs the initial direction information indicating the direction indicated by the arrow ID1 and the initial direction calculation time to the direction integration unit 1161 as the reference direction information and the reference direction measurement time.

The direction integration unit 1161 and the position integration unit 1162 calculate the estimated front direction and the estimated position, respectively, based on the input initial values. For example, the position integration unit 1162 calculates the positions P211 and 212 as the estimated positions. The communication unit 103 receives absolute position information. This absolute position information is, for example, positions P221 and P222.
The correction start command unit 119 determines whether or not the distance along the locus of the estimated position from the position P220 to the position P212 exceeds a predetermined distance. FIG. 5 is a diagram when it is determined that a predetermined distance has been exceeded at the time of the estimated position P212 (target time t = t1). The correction start command unit 119 outputs a signal for starting the position / direction correction process to the position correction unit 1163 and the direction correction unit 1164 at the target time t1.

  The position correction unit 1163 calculates the direction correction value based on the estimated position information and the absolute position information during the period from the time t1-n to the target time t1 (in FIG. 5, at the time of the positions P211 and P221). The direction correction unit 1164 adds the direction correction value calculated by the position correction unit 1163 to the estimated direction to calculate a correction direction. The position correction unit 1163 outputs reference position information indicating the absolute position P222 at the target time t1 to the position integration unit 1162. Further, the direction correcting unit 1164 outputs reference direction information indicating the correction direction at the target time t1 to the direction integrating unit 1161.

  The direction integration unit 1161 calculates an estimated front direction based on the input reference direction information, and the position integration unit 1162 calculates an estimated position based on the input reference position information. Similarly, the correction start command unit 119 outputs a signal for starting the position / direction correction process at the estimated position P213. The position / direction correction unit 116 performs position / direction correction processing based on this signal.

FIG. 5 shows that the direction detection unit 104 has detected the direction indicated by the arrow ID2 at the time of the position P224 (time of the estimation P214; time t2). In this case, the direction correction unit 1164 outputs the initial direction information indicating the direction and the initial direction calculation time to the direction integration unit 1161 as the reference direction information and the reference direction measurement time.
FIG. 5 shows a case where it is determined that the distance along the locus of the estimated position from the position P223 to the position P215 exceeds the predetermined distance at the estimated position P213 (target time t = t3). FIG. Here, in the example of FIG. 5, time t2 is later than time t3-n. In this case, the position correction unit 1163 calculates the direction correction value based on the estimated position information and the absolute position information for the period from the time t2 to the target time t3. That is, the position correction unit 1163 is based on the estimated position information and the absolute position information from the later time of the time t−n and the time when the direction detection unit 104 detects the direction to the target time t. The direction correction value is calculated.

Thus, according to the present embodiment, the position / direction control unit 11 of the mobile robot 1 integrates the amount of change in position to estimate estimated position information indicating the position of the mobile robot, and integrates the amount of change in direction. Then, estimated front direction information indicating the front direction of the mobile robot is estimated. In addition, the position / direction control unit 11 identifies the position information received from the absolute position measurement device c1 and the absolute position information indicating the position of the moving device from moment to moment and the estimated position information from moment to moment. A direction correction value for correcting the deviation is calculated, and the front direction indicated by the estimated front direction information is corrected using the direction correction value. Thereby, the position direction control part 11 can measure the direction of a moving apparatus correctly.
Further, in the present embodiment, it is only necessary that a device other than the mobile robot 1 (the absolute position measuring device c1) can measure only the position of the mobile robot 1, so that even if the absolute position measuring device c1 cannot detect the direction, the position / direction control unit. 11 can accurately measure the orientation of the moving device. Further, the position / direction control unit 11 can accurately measure the direction of the mobile device without having a function of sensing distance or a function of estimating an optimum position.
Furthermore, since the position / direction control unit 11 can accurately measure the front direction, that is, the traveling direction, by detecting the relative direction between the traveling direction and the direction of another part (for example, the front direction of the upper body), The direction of the part can also be accurately measured.

Further, according to the present embodiment, the position / direction control unit 11 calculates the direction correction value using the absolute position information and the estimated position information at a plurality of times. As a result, the position / direction control unit 11 can measure the orientation of the moving device with higher accuracy than in the case of using one piece of absolute position information and estimated position information.
Further, according to the present embodiment, the position / direction control unit 11 determines the timing for starting the process of calculating the direction correction value based on the distance traveled by the mobile robot 1. Accordingly, the position / direction control unit 11 can correct the orientation of the mobile station robot 1 according to the distance traveled by the mobile robot 1.

In addition, in the said embodiment, although the case where the mobile robot 1 was provided with the position direction control part 11 was demonstrated, this invention is not restricted to this, The position direction control part 11 is a vehicle (a motorcycle etc. is included) and a wheelchair. It may be provided for a device that moves automatically.
Moreover, although the case where the mobile robot 1 moves in the room has been described in the above embodiment, the present invention is not limited to this, and any environment that can measure the absolute position of the mobile robot 1 (or vehicle) may be used. For example, when the position of the mobile robot 1 can be measured by GPS (Global Positioning System), it may be outdoors.

Moreover, in the said embodiment, although the correction start command part 119 demonstrated the case where the timing which starts the process which calculates a direction correction value based on the distance which the mobile robot 1 moved was demonstrated, this invention does this. Not limited to this, the correction start command unit 119 may determine the timing based on the time. For example, the correction start command unit 119 determines whether or not a predetermined time has elapsed from the time when the previous position / direction correction process was performed, and when determining that the predetermined time has elapsed, the correction start command unit 119 performs the position / direction correction process. A signal to be started may be output to the position correction unit 1163 and the direction correction unit 1164 of the position / direction correction unit 116.
Moreover, in the said embodiment, the correction start command part 119 demonstrated the case where the timing which starts the process which calculates a direction correction value was determined based on the distance along the locus | trajectory of an estimated position. However, the present invention is not limited to this, and may be a distance between estimated positions. Further, the correction start command unit 119 may set the timing to start the process of calculating the direction correction value when it is determined that either the distance in the X coordinate or the Y coordinate exceeds a predetermined value.

  In the above embodiment, the case where the position correction unit 1163 calculates the direction correction value by identifying the estimated position information and the absolute position information for each unit time has been described. However, the present invention is not limited to this, and the unit time Information for each smaller time may be used, or information for each larger time may be used. For example, the position correction unit 1163 calculates the time interval (1 second in the above embodiment) of the absolute position measurement time received by the communication unit 103 from the absolute position measurement device c1, and calculates the estimated position information and absolute value for each calculated time interval. The direction correction value may be calculated by identifying the position information.

In the above embodiment, the position correction unit 1163 has a singular point where the numerical value indicated by the estimated position information or the absolute position information changes extremely (the difference from the numerical value at the previous time becomes larger than a predetermined threshold value. When there is a time numerical value), the direction correction value may be calculated by identifying the estimated position information and the absolute position information excluding the time information of the singular point. As a result, the position / direction control unit 11 can measure the orientation of the moving device with higher accuracy than in the case of using one piece of absolute position information and estimated position information.
Moreover, in the said embodiment, although the front direction is calculated based on the relationship between a predetermined azimuth | direction and global coordinates, this process may be abbreviate | omitted.

  Note that a part of the mobile robot 1 in the embodiment described above, for example, the encoder unit 111, the direction integration unit 1161, the position integration unit 1162, the position correction unit 1163, the direction correction unit 1164, the drive control unit 118, and the correction start command unit. 119 may be realized by a computer. In that case, the program for realizing the control function may be recorded on a computer-readable recording medium, and the program recorded on the recording medium may be read by a computer system and executed. Here, the “computer system” is a computer system built in the mobile robot 1 and includes an OS and hardware such as peripheral devices. The “computer-readable recording medium” refers to a storage device such as a flexible medium, a magneto-optical disk, a portable medium such as a ROM and a CD-ROM, and a hard disk incorporated in a computer system. Furthermore, the “computer-readable recording medium” is a medium that dynamically holds a program for a short time, such as a communication line when transmitting a program via a network such as the Internet or a communication line such as a telephone line, In such a case, a volatile memory inside a computer system serving as a server or a client may be included and a program that holds a program for a certain period of time. The program may be a program for realizing a part of the functions described above, and may be a program capable of realizing the functions described above in combination with a program already recorded in a computer system.

  As described above, the embodiment of the present invention has been described in detail with reference to the drawings. However, the specific configuration is not limited to the above, and various design changes and the like can be made without departing from the scope of the present invention. It is possible to

  DESCRIPTION OF SYMBOLS 1, 11, 12, 13 ... Mobile robot (mobile device), c1 ... Absolute position measuring device, 101 ... Gyroscope, 102 ... Drive part, 103 ... Communication part, 104 ... Direction detection unit, 11 ... position / direction control unit, 111 ... encoder unit, 112 ... relative direction information storage unit, 113 ... relative position information storage unit, 114 ... absolute position information storage unit 115 ... Initial direction information storage unit, 116 ... Position / direction correction unit (orientation measuring device), 117 ... Position / direction information storage unit, 118 ... Drive control unit, 119 ... Correction start command 1161, ... direction integration part, 1162 ... position integration part, 1163 ... position correction part (direction correction value calculation part), 1164 ... direction correction part

Claims (5)

In the orientation measurement device mounted on the moving device that detects the amount of change in the position of the device itself and the amount of change in the direction,
A position integrator that integrates the amount of change in the position and estimates estimated position information indicating the position of the mobile device;
A direction integration unit that estimates the estimated direction information indicating the direction of the mobile device by integrating the amount of change in the direction;
A direction for identifying positional information received from a position measuring device outside the mobile device and indicating the position of the mobile device from time to time and the estimated position information from time to time, and correcting the direction deviation A direction correction value calculation unit for calculating a correction value;
A direction correction unit that corrects the direction indicated by the estimated direction information using the direction correction value;
When it is determined that the moving device has moved 2 to 3 m from the time of correcting the previous direction, the direction correction value calculation unit starts processing to calculate the direction correction value, and the direction correction unit receives the estimated direction information. A correction start command part for starting a process of correcting the direction shown,
With
The position measuring device, a direction measuring device according to claim Tei Rukoto installed in buildings. The position measuring device is installed on a ceiling,
The orientation measurement apparatus according to claim 1, wherein the absolute position information is position information transmitted by one of the position measurement apparatuses. An orientation measuring device mounted on a moving device that detects the amount of change in the position of the device itself and the amount of change in direction, and absolute position information indicating the position of the moving device every moment is measured and transmitted to the orientation measuring device. An orientation measuring system comprising: a position measuring device;
The orientation measuring device is
A position estimation unit that estimates estimated position information indicating the position of the mobile device by integrating the amount of change in the position;
A direction estimation unit that estimates estimated direction information indicating the direction of the mobile device by integrating the amount of change in the direction;
A direction correction value calculation unit that identifies the absolute position information received from the position measurement device and the estimated position information every moment, and calculates a direction correction value for correcting a shift in direction;
A direction correction unit that corrects the direction indicated by the estimated direction information using the direction correction value;
When it is determined that the moving device has moved 2 to 3 m from the time of correcting the previous direction, the direction correction value calculation unit starts processing to calculate the direction correction value, and the direction correction unit receives the estimated direction information. A correction start command part for starting a process of correcting the direction shown,
With
The position measuring device includes:
Orientation measurement system, characterized in Tei Rukoto installed in buildings. In the direction measuring method in the direction measuring device mounted on the moving device that detects the amount of change in the position of the own device and the amount of change in the direction,
A process in which a position integration unit integrates the amount of change in the position to estimate estimated position information indicating the position of the mobile device;
A process in which the direction integration unit estimates the estimated direction information indicating the direction of the mobile device by integrating the amount of change in the direction;
The direction correction value calculating unit identifies the position information received from the position measuring device outside the moving device and the absolute position information indicating the position of the moving device from moment to moment, and the estimated position information from moment to moment, A process of calculating a direction correction value for correcting a direction shift;
A process in which the direction correction unit corrects the direction indicated by the estimated direction information using the direction correction value;
When the correction start command unit determines that the moving device has moved 2 to 3 m from the time of correcting the previous direction, the direction correction value calculation unit starts processing to calculate the direction correction value, and the direction correction unit Starting the process of correcting the direction indicated by the estimated direction information,
Have
The position measuring device, a direction measuring method comprising Tei Rukoto installed in buildings. A computer of a direction measuring device mounted on a moving device that detects the amount of change in the position of the device itself and the amount of change in the direction,
Position estimation means for estimating estimated position information indicating the position of the mobile device by integrating the amount of change in the position;
Direction estimation means for estimating estimated direction information indicating the direction of the mobile device by integrating the amount of change in the direction;
A direction for identifying positional information received from a position measuring device outside the mobile device and indicating the position of the mobile device from time to time and the estimated position information from time to time, and correcting the direction deviation Direction correction value calculating means for calculating a correction value;
Direction correcting means for correcting the direction indicated by the estimated direction information using the direction correction value;
When it is determined that the moving device has moved 2 to 3 m from the time when the previous direction was corrected, the direction correction value calculation unit starts processing to calculate the direction correction value, and the direction correction unit receives the estimated direction information. Function as a correction start command means for starting the process of correcting the direction shown ,
The position measuring device, Tei Ru orientation measurement program installed in the building.

Images