JP2006031642A - Self-position specification method of mobile object - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a self-position specification method of a mobile object by which a self-position of the mobile object is specified with high precision even at a place where a GPS can not be used and moreover, a map is hard to obtain and when, for example, fire occurs in a building, a highly precise map to a disaster-stricken part can be created. <P>SOLUTION: A position of an inertial coordinate system of a transfer start part of a self-propelled mobile object 1 is determined and a local inertial coordinate system centering around the position is defined. A first target L1 is selected from an image of a CCD camera 3 loaded on the mobile object 1, distance to the first target L1 is measured with a laser ranging device 4, a direction of the first target L1 is specified with an angle sensor 6, after calculating the local inertial coordinate of the first target L1, the mobile object 1 is transferred on the basis of the first target L1, a second target L2 is selected from the image of the CCD camera 3 at a proper position and the last self-position specified on the basis of the first target and afterward, self-positions of the mobile object 1 are calculated on the basis of new targets sequentially updated whenever the mobile object 1 is transferred. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention provides a map that helps fire fighters who are headed to a disaster spot when a disaster such as a fire occurs in a place where GPS cannot be used and it is difficult to obtain a map, such as a building or underground. The present invention relates to a mobile body self-localization method suitable for use in creation.

  Conventionally, as a method for identifying the position of a moving body as described above, for example, there is a known method called dead reckoning. In this method, the surroundings are confirmed using various sensors mounted on the moving body, and the moving body The movement distance is recognized by counting the number of rotations of the small wheels connected to the vehicle, and the self-position is specified while eliminating accumulated errors by referring to a map stored in advance.

  However, in the above-mentioned dead reckoning, it is indispensable to refer to the map, so it cannot be used in places where it is difficult to obtain maps such as buildings and underground, for example, the internal structure is not so complicated, Even if it is a case where it is possible to recognize the self position without referring to the map, depending on the situation of the running surface, the small wheel connected to the moving body may slip and the moving distance may be greatly shifted, Since there is a possibility that the gyro of the various sensors installed will drift and the orientation may be distorted, there is a problem that it is not possible to specify the self position with high accuracy, and it is a conventional problem to solve this problem It was.

  The present invention has been made paying attention to the above-mentioned conventional problems, and even if it is a place where GPS cannot be used and it is difficult to obtain a map, the self-position of the moving object is specified with high accuracy. As a result, for example, in the event of a disaster such as a fire in a building or underground, a self-localization method for moving objects that makes it possible to create a high-precision map to the required disaster point It is intended to provide.

  According to a first aspect of the present invention, there is provided a self-localization method for a mobile object, wherein an image acquisition means, a distance measurement means and various angle sensors are mounted on a self-propelled mobile object, and the inertia coordinates of the point at which the mobile object starts to move. Following the determination of the position in the system, the definition of local inertia coordinates centered on the movement start point, and the selection of the first target from the image of the image acquisition means mounted on the moving body, the first The distance to the target is measured by the distance measuring means, the direction of the first target in the local inertia coordinate system is specified by various angle sensors, and the local inertia coordinates of the first target are obtained from the distance and direction. After that, the mobile unit is moved while specifying its own position with reference to the first target, and then specified with a distance from the first target as appropriate and with reference to the first target. In the last self-position, Following the selection of the second target from the image of the image acquisition means placed, the second target in the local inertial coordinate system specified by the distance to the second target measured by the distance measuring means and various angle sensors. It is characterized in that the local inertia coordinates of the second target are obtained from the direction of the target, and thereafter, the self-position of the moving body is obtained with reference to a new target that is sequentially updated each time the moving body is moved. The configuration of this mobile body self-position specifying method is used as means for solving the above-described conventional problems.

  According to a second aspect of the present invention, there is provided a self-localization method for a mobile body, wherein a plurality of target bodies, distance measuring means and various angle sensors are mounted on a mobile body capable of traveling, and A position in the inertial coordinate system is determined to define local inertial coordinates centered on the movement start point, and one of a plurality of target bodies is arranged at this position as the first target body. Thereafter, the movable body is moved, and then the distance to the first target body is measured at the stop position as appropriate by the distance measuring means, and the direction of the first target body in the local inertial coordinate system is measured by various angle sensors. After specifying the self-position from the distance and the direction, after placing one of the plurality of target bodies at this position as a second target body, Every time after that, every time the moving body is moved and stopped The method is to obtain the self-position of the moving body with reference to the new target body that is sequentially updated in the direction, and the means for solving the above-described conventional problem is the configuration of the self-position specifying method of the moving body It is said.

  Since the mobile body self-localization method of the present invention has the above-described configuration, it is possible to specify the mobile body's self-position with high accuracy even in places where GPS cannot be used and where it is difficult to obtain a map. Therefore, for example, when a disaster such as a fire occurs in a building or underground, by moving the moving body while identifying its own location, highly accurate information up to the disaster point where fire fighters are craving It has a very good effect that it is possible to create a map.

  The self-localization method of the moving body according to claim 1 of the present invention is, for example, a self-positioning method based on an existing article or structure such as a flower pot, a figurine, or a fire extinguisher selected as a target when the moving body enters the building. Since the position is specified, it is a so-called passive type self-positioning method.

  In this case, the selection of the target from the image of the image acquisition means such as a CCD camera mounted on the moving body may be performed by a person who monitors the image or may be performed by a computer.

  On the other hand, according to the self-localization method of the moving body of claim 2 of the present invention, for example, when the moving body enters the building, it is arranged every time a plurality of target bodies mounted on the moving body are stopped. Since the self-position is specified on the basis of the target body, it is an active type self-position specifying method. In this self-localization method of the moving body, it is necessary to mount a front surveillance camera on the moving body or to set a reconnaissance robot ahead of the moving body for the convenience of identifying the self-position with reference to the rear target body. There is.

  A marker that can be recognized by color and / or shape can be adopted as the target body, which is suitable when the field of view can be secured. In addition to the marker, a phosphor or a light emitter can be used as the target body, and these are suitable when the field of view cannot be secured or when the field of view is poor. Furthermore, when the distance measuring means is a laser distance measuring device, it is desirable to adopt a corner reflector having excellent reflectivity as the target body.

  In the mobile body self-localization method of the present invention, the distance measuring means includes, in addition to the laser distance measuring apparatus described above, a stereo distance measuring apparatus that captures a target or a target object by changing the direction with two CCD cameras, An active stereo distance measuring device that irradiates a corner reflector with a laser beam and changes the direction of the reflected light with two CCD cameras to shoot can be adopted. However, a three-dimensional laser distance measuring device is used. desirable.

  Further, in the method for identifying the position of the moving body of the present invention, as the various angle sensors, a pan-tilt gimbal (biaxial gimbal) that supports the laser distance measuring device, a tilt sensor, and a sun sensor are used.

  Hereinafter, the present invention will be described with reference to the drawings.

  FIG. 1 and FIG. 2 are diagrams for explaining an embodiment of the self-localization method for a mobile object according to the present invention. In this embodiment, a GPS cannot be used and a map is difficult to obtain. An example will be described in which when a fire occurs, the moving body is moved into the building and moved to the fire occurrence point while specifying its own position.

  As shown in FIG. 1, the movable body 1 capable of self-running includes a vehicle body 2, a CCD camera 3 as image acquisition means, a laser distance measuring device 4 as distance measurement means, and a shaft 5 erected on the vehicle body 2. A pan-tilt gimbal (biaxial gimbal) 6 as an angle sensor that supports the laser distance measuring device 4 so as to be pivotable in the left-right direction and the up-down direction. The CCD camera 3 is provided integrally with the laser distance measuring device 4.

When the above moving body 1 enters the building where the fire has occurred and goes to the point where the fire occurs, first, as shown in FIG. 2, for example, the entrance of the building is set as the movement start point P0. A position (X ₀ , Y ₀ , Z ₀ ) in the inertial coordinate system is determined, and local inertia coordinates (X _i , Y _i , Z _i ) centered on this movement start point P0 are defined.

Subsequently, from the image of the CCD camera 3 mounted on the moving body 1, an object that can be a target (a figurine in the illustrated example) such as a flower pot, a figurine, or a fire extinguisher is selected as the first target L1. 1 of the distance R ₁ to the target L1 identifies the direction of the first target L1 in the local inertial frame in various angle sensor 6 as well as measured by the laser distance measuring device 4, from the distance R ₁ and direction The local inertial coordinates (X ₁₁ , Y ₁₁ , Z ₁₁ ) of the first target L1 are obtained. The selection of the first target L1 from the image of the CCD camera 3 may be performed by an operator who monitors the image, or may be performed by a computer.

Thereafter, when the moving body 1 moves while specifying its own position with reference to the first target L1, the local inertia coordinates (X, Y, Z) are obtained at the arbitrary position P. When there is a possibility that it is difficult to capture the target L1, at the last self-position P1 specified with reference to the first target L1, a flowerpot, a figurine, those that can be a target, such as a fire extinguisher (not shown) followed in selecting a second target L2, the distance R ₂ and various angles up to a second target L2 measured at the laser distance measuring device 4 obtains a local inertial coordinate _{(X l2, Y l2, Z} l2) of the second target L2 from the direction of the second target L2 in the identified local inertial coordinate system by the sensor 6, thereafter, the moving body 1 and the second Locating the self-position based on the target L2 Move one.

  Thereafter, the mobile body 1 always moves while grasping its own position on the basis of the new target L sequentially updated as described above, and collects position information obtained by the movement of the mobile body 1. A highly accurate information map up to the fire point will be created outside the building.

Here, as shown in FIG. 3, a vehicle coordinate system is used for the moving body 1, and a sensor coordinate system is used for the laser distance measuring device 4 and various angle sensors 6 mounted on the moving body 1. the distance to the first target L1 as measured by the coordinate system the laser light applied to the Xs axis direction together with the R _1, coordinate transformation matrix C _vi from the vehicle coordinate system to the local inertial _frame, sensor coordinates If the coordinate transformation matrix from the system to the vehicle coordinate system is C _sv , the local inertia coordinates (X ₁₁ , Y ₁₁ , Z ₁₁ ) of the first target L1 can be obtained from Equation 1. In addition, the angle of the sensor necessary for calculating C _vi and C _sv using these coordinate transformation matrices is measured as shown in Table 1.

  Further, the local inertia coordinates (X, Y, Z) at the arbitrary position P of the moving body 1 that moves on the basis of the first target L1 can be obtained by Expression 2.

但し、Ｒは、移動体１の任意位置Ｐから第１の目標Ｌ１までの距離であり、角度パラメータは任意位置Ｐで計測した値を使用する。

However, R is the distance from the arbitrary position P of the moving body 1 to the first target L1, and the angle parameter uses a value measured at the arbitrary position P.

Then, a position selected as the second target L2 from the image of the CCD camera 3, that is, the local inertia coordinates (X ₁ , Y ₁ , Z ₁ ) at the last self-position P1 specified with reference to the first target L1. Can be obtained by Equation 3.

但し、Ｒ_ｌ１は、移動体１の任意位置Ｐ１から第１の目標Ｌ１までの距離であり、第２の目標Ｌ２の局所慣性座標（Ｘ_ｌ２，Ｙ_ｌ２，Ｚ_ｌ２）は、式４により得ることができる。

_{However, R l1} is the distance from an arbitrary position P1 of the movable member 1 to the first target L1, local inertial coordinate of the second target _{L2 (X l2, Y l2,} Z l2) is obtained by Equation 4 be able to.

但し、Ｒ_２は、移動体１の任意位置Ｐ１から更新した新しい第２の目標Ｌ２までの距離である。

However, R ₂ is the distance from an arbitrary position P1 of the moving body 1 to the new second target L2 has been updated.

  Similarly to the above, the local inertia coordinates (X, Y, Z) at the arbitrary position P of the moving body 1 that moves on the basis of the second target L2 can be obtained by Expression 5.

但し、Ｒは、移動体１の任意位置Ｐから第２の目標Ｌ２までの距離である。

However, R is the distance from the arbitrary position P of the moving body 1 to the second target L2.

  This expression 5 further becomes the following expression 6.

  Then, local inertia coordinates (X, Y, Z) at an arbitrary position P of the moving body 1 that moves with reference to the kth target can be obtained by Expression 7.

  That is, the self-position at the arbitrary position P of the moving body 1 can be specified by the above formula 7.

  FIG. 4 is a diagram for explaining another embodiment of the mobile body self-localization method according to the present invention. As shown in FIG. 23, a laser distance measuring device 24 as a distance measuring means, and various angle sensors (not shown), and the vehicle body 22 carries a plurality of target bodies.

  In this case, a corner reflector 29 having excellent reflectivity is adopted as the target body because the distance measuring means is the laser distance measuring device 24. If the field of view can be secured, a marker that can be recognized by color and / or shape can be adopted as the target body. If the field of view cannot be secured or the field of view is poor, the target body is a phosphor. Or a luminescent material can be used.

  When the above-mentioned moving body 21 enters the building where the fire has occurred and is directed to the point where the fire occurs, first, the position of the point where the moving body 21 starts moving is determined in the inertial coordinate system, and the movement is started. Define local inertia coordinates around the point.

  Next, one of the plurality of corner reflectors 29 is arranged at this position to form a first corner reflector (first target body) 29, and then the moving body 21 is moved.

  Then, at an appropriate stop position, the distance to the first corner reflector 29 is measured by the laser distance measuring device 24, and the direction of the first corner reflector 29 in the local inertial coordinate system is specified by various angle sensors, After the self-position is specified from the distance and direction, after one of the plurality of corner reflectors 29 is arranged at this position, the second corner reflector (second target body) 29 is used. The moving body 21 is moved again.

  Thereafter, each time the moving body 21 is moved and stopped, the self-position is grasped with reference to the new corner reflector 29 that is sequentially updated backward, and the position information obtained by stopping the moving body 1 is collected. For example, a highly accurate information map to the fire point is created outside the building.

  The self-localization method of the moving body of the present invention is not limited to fire and emergency activities when a fire occurs in a building where GPS cannot be used and it is difficult to obtain a map. In this case, it is possible to mount an information collecting camera and a sound collecting microphone on the moving body.

  Further, in the above-described embodiment, the case where the moving body moves two-dimensionally is shown, but it goes without saying that the self-position can be grasped similarly when the moving body moves three-dimensionally.

  Furthermore, in the mobile body self-localization method of the present invention, when moving the mobile body, it is possible to arrange an RFID (Radio Frequency Identification) tag storing route information to that point as necessary. It is.

It is perspective explanatory drawing which shows simply the condition where the mobile body used for the self-position specifying method by one Example of this invention has specified the self-position. (Example 1) It is a conceptual explanatory drawing of the self-localization method of the moving body in FIG. It is a figure explaining the coordinate system of the self-localization method of the moving body of this invention. It is perspective explanatory drawing which shows simply the condition where the mobile body used for the self-position specifying method by the other Example of this invention has specified the self-position. (Example 2)

Explanation of symbols

1,21 Moving body 3 CCD camera (image acquisition means)
4,24 Laser distance measuring device (ranging means)
6 Pan tilt gimbal (angle sensor)
29 Corner reflector (first target body, second target body, ...)
L1 1st goal L2 2nd goal

Claims (6)

  A self-propelled moving body is equipped with image acquisition means, distance measuring means and various angle sensors, and the position in the inertial coordinate system where the moving body starts moving is determined, and this moving start point is set as the center. After defining the local inertia coordinates and selecting the first target from the image of the image acquiring means mounted on the moving body, the distance to the first target is measured by the distance measuring means and various types are selected. The direction of the first target in the local inertia coordinate system is specified by the angle sensor, the local inertia coordinates of the first target are obtained from the distance and direction, and the self-position is specified with reference to the first target. Then, the moving body is moved, and then the image acquisition means mounted on the moving body is positioned at an appropriate distance from the first target and at the last self-position specified with reference to the first target. Select second target from image Subsequently, the local inertia coordinates of the second target are obtained from the distance to the second target measured by the distance measuring means and the direction of the second target in the local inertia coordinate system specified by the various angle sensors. Thereafter, the self-position specifying method of the moving body, characterized in that the self-position of the moving body is obtained based on a new target that is sequentially updated each time the moving body is moved.   A plurality of target bodies, ranging means, and various angle sensors are mounted on a mobile body that can run on its own, and the position of the point where the mobile body starts moving is determined in the inertial coordinate system. And defining one of the plurality of target bodies at this position as the first target body, and then moving the movable body, The distance to one target is measured by the distance measuring means, the direction of the first target in the local inertial coordinate system is specified by various angle sensors, and the self position is specified from the distance and the direction. Subsequently, after placing one of the plurality of target bodies at this position to form a second target body, each time the movable body is moved and thereafter the movable body is moved and stopped. Move with reference to a new target that is sequentially updated backward Self localization method of a moving body and obtains the self-position.   The self-localization method of the moving body according to claim 2, wherein the target body is a marker that can be recognized by color and / or shape.   The self-positioning method of a moving body according to claim 2, wherein the target body is a phosphor.   The method for identifying the position of a moving body according to claim 2, wherein the target body is a light emitter.   3. The method of claim 2, wherein the distance measuring means is a laser distance measuring device and the target object is a corner reflector.

Images