JP4806435B2 - Self-position recognition method and self-position recognition system using 3D model - Google Patents

Description

  The present invention relates to a self-position recognition method and system for a moving body that moves in a moving space in which target objects forming a passage are installed.

In distribution warehouses, automatic guided vehicles carry packages to improve efficiency and labor saving. In general, there are methods (electromagnetic induction type / magnetic induction type) for automatically driving an automated guided vehicle by detecting electromagnetic waves and magnetism. In this method, a magnetic tape serving as guiding means is laid on the floor (see Patent Document 1), or a magnetic material is embedded on the floor (see Patent Document 2, etc.). The automatic guided vehicle automatically travels by detecting electromagnetic waves and magnetism from the guiding means. Therefore, since it takes a lot of labor and expense to change the position of the guiding means such as a magnetic material, it is very difficult to change the layout in the warehouse. Moreover, since this method embeds a magnetic material, the introduction cost is high.
JP 2002-278622 A JP 2005-122568 A

  Therefore, the problem to be solved by the present invention can be easily applied even if the layout in the movement space is changed, the introduction cost is low, and the self-position recognition method of the mobile body capable of self-position recognition at high speed and accurately, and Is to provide a system.

In order to solve the above problem, the self-position recognition method according to the present invention is:
A self-position recognition method for a moving object that moves in a moving space along a pre-stored target trajectory, the step of creating a three-dimensional model of the moving space, and a camera image from the position and direction of the moving object at a predetermined time A step of acquiring, a step of calculating a target position and direction on a target trajectory at a predetermined time, a step of forming model images of a plurality of positions and directions within a predetermined range of the target position and direction from a three-dimensional model, The step of calculating the degree of coincidence of the color information between the plurality of model images and the camera image, and the step of recognizing the position and direction of the moving body based on the degree of coincidence.

  Preferably, the step of installing the target object in the moving space, the step of defining the boundary vertical line of the target object in the camera image as a reference line, the step of specifying the position of the reference line in the camera image, and the reference in the three-dimensional model The step of specifying the position of the line and the step of forming a plurality of model images at arbitrary positions and directions from the three-dimensional model based on the position of each specified reference line are included.

  Preferably, the target object is a passage forming body in which a passage is formed.

  Preferably, the method includes a step of binarizing the camera image and the model image.

  Preferably, the method includes a step of approximating a quadratic or cubic curve from each coincidence and recognizing both or one of the position and direction of the moving object based on the maximum value of the curve.

  Preferably, the method includes a step of changing the shape of the target object in the three-dimensional model based on information from the storage management system for moving space.

  Preferably, a step of extracting a detection area other than the target object in the camera image and the model image, a step of comparing the detection areas, and an obstacle in the direction of the moving object based on the comparison value and a predetermined threshold value Recognizing.

The self-position recognition system according to the present invention is
A self-position recognition system for a moving body that moves in a moving space along a pre-stored target trajectory, comprising: means for creating a three-dimensional model of the moving space; and a camera image from the position and direction of the moving body at a predetermined time. Means for acquiring, means for calculating a target position and direction on a target trajectory at a predetermined time, means for forming model images of a plurality of positions and directions within a predetermined range of the target position and direction from a three-dimensional model, Means for calculating the degree of coincidence of the color information between the plurality of model images and the camera image and means for recognizing the position and direction of the moving body based on the degree of coincidence.

  Preferably, means for installing the target object in the moving space, means for defining the boundary vertical line of the target object in the camera image as a reference line, means for specifying the position of the reference line in the camera image, and reference in the three-dimensional model Means for specifying the position of the line, and means for forming a plurality of model images at arbitrary positions and directions from the three-dimensional model based on the position of each specified reference line.

  Preferably, the target object is a passage forming body in which a passage is formed.

  Preferably, a means for binarizing the camera image and the model image is provided.

  Preferably, there is provided means for approximating a quadratic or cubic curve from each degree of coincidence and recognizing both or one of the position and direction of the moving body based on the maximum value of the curve.

  Preferably, there is provided means for changing the shape of the target object in the three-dimensional model based on information from the moving-in / out management system.

  Preferably, a means for extracting a detection area other than the target object in the camera image and the model image, a means for comparing the detection areas, and an obstacle in the direction of the moving object based on the comparison value and a predetermined threshold value And means for recognizing

  A self-position recognition method and system using a three-dimensional model according to the present invention is based on a camera image acquired by a camera of a moving object and a three-dimensional model of a moving space created in advance, and the self-position and direction of the moving object. Enable recognition. Therefore, even if the moving space is changed, it can be dealt with by simply changing the three-dimensional model according to the moving space. Therefore, even if the layout in the moving space is changed, it can be easily applied and the introduction cost is low. Furthermore, the target position and direction on the target trajectory at a predetermined time is calculated, and a model image within a predetermined range of the target position and direction is formed from the three-dimensional model and compared with the camera image. The number of model images can be limited and processed at high speed. Further, since the model image and the camera image are identified based on the degree of coincidence of the color information rather than the pixel position information based on the contour shape, the reliability of the estimated position and direction is very high.

  In the present invention, the boundary vertical line of the target object in the camera image can be defined as the reference line. Then, the self position of the moving body is recognized based on the position of the reference line in the camera image and the three-dimensional model. Accordingly, the feature quantity for recognizing the self position is the position of the reference line that is the boundary vertical line, and since there are few reference lines for recognizing the position, the self position can be recognized at high speed and accurately.

  Hereinafter, a self-position recognition method and system using a three-dimensional model according to the present invention will be described with reference to the accompanying drawings.

[overall structure]
FIG. 1 is a diagram showing a mobile object equipped with a self-position recognition system. In the present embodiment, the moving body 1 is an automatic guided vehicle. As shown in FIG. 1, the moving body 1 includes a camera 2 facing forward. The camera 2 is installed at a certain camera height H from the floor surface F. The moving body 1 includes a controller 3.

  The moving body 1 includes a steering wheel 4 and a traveling wheel 5. The steering wheel 4 changes the steering angle based on the drive of the steering motor 40. The traveling wheel 5 moves forward and backward based on driving of the traveling motor 50. As will be described in detail later, the controller 3 recognizes its own position and direction based on the image from the camera 2 so that the moving body 1 travels along a pre-stored target trajectory ST (traveling path). The driving of the steering motor 40 and the traveling motor 50 is controlled.

[Target trajectory]
FIG. 2 is a diagram for explaining the target trajectory. As shown in FIG. 2, in the moving body 1, the controller 3 controls the driving of the steering motor 40 and the traveling motor 50, and the target trajectory ST (two coordinates in FIG. 2) composed of two-dimensional coordinates (XY coordinates) and directions stored in advance. Steering, decelerating, accelerating, stopping and traveling along the dotted line). Therefore, the controller 3 sets the target position (on the target trajectory) for the moving body 1 to move along the target trajectory ST at predetermined time intervals t−1, t, t + 1. X _t , Y _t ) and direction θ _t can be calculated. In the present embodiment, the predetermined time interval Δt is, for example, about 0.3 seconds.

[First Embodiment]
A first embodiment will be described.
FIG. 3 is a block diagram showing the self-position recognition system of the first embodiment. FIG. 4 is a flowchart for explaining the self-position recognition method according to the first embodiment. FIG. 5 is a diagram for explaining model images to be compared in the first embodiment. FIG. 6 is a conceptual diagram showing a three-dimensional model in the movement space.

  The self-position recognition system includes a camera 2 and a controller 3 (FIG. 3). The controller 3 includes a computer equipped with a CPU and the like, and processes a camera image acquired from the camera 2 to recognize the position and direction of the moving body 1.

  In the present embodiment, the moving body 1 moves as a moving space 6 in a physical distribution warehouse (FIG. 6). In this movement space 6, a plurality of racks (shelves) 60 for arranging luggage and the like are arranged. The moving body 1 travels along the target track ST with the path 61 between the racks 60.

  First, the same three-dimensional model (model space) (FIG. 6) as the moving space 6 (real space) in which the mobile body 1 actually travels is created by the three-dimensional model creating means 21 (step S01 in FIG. 4). Then, the three-dimensional model storage unit 11 stores the three-dimensional model. Note that the three-dimensional model is created with substantially the same color as the real space. The three-dimensional model creation means 21 is composed of, for example, a CAD system.

  The three-dimensional model creating means 21 has a shape in which the parcel is arranged on the target object 60 in the three-dimensional model based on a storage / inventory management system (WMS) 22 that manages the parcels arranged on the target object (rack) 60. Change from time to time. As a result, the shape and color information in the moving space 6 between the actual (real space) and the three-dimensional model (model space) always coincide. The color information includes hue (chromaticity) H, saturation S, and lightness (luminance) V.

  Then, a target trajectory ST on which the mobile body 1 travels is created, and the target trajectory ST is stored in the target trajectory storage means 17 (step S02 in FIG. 4).

FIG. 7 is a diagram illustrating a camera image. The camera image acquisition unit 10, at predetermined time t, acquiring a camera image Ri _t by camera 2 (step S11 in FIG. 4). Camera image Ri _t is the predetermined time t, the position of the moving body 1 _{(X M, Y} M) is an image for and the direction theta _M.

FIG. 8 is a diagram illustrating a binarized camera image. All pixels of the camera image Ri _t (total pixels) and HSV conversion, the brightness value V of the pixel composed of the hue H of the floor and the wall to "0", the luminance value V of the other pixels to "1" It converted to binarized to generate a camera image Ri _t obtained by extracting the object 60 (step S3 in FIG. 3). Since the hue H of the floor surface and the wall surface is set in advance, binarization by extracting the target object 60 is possible.

Then, at a predetermined time t, the target position (X _t , Y _t ) and the direction θ _t on the target trajectory for the moving body 1 to move along the target trajectory ST are calculated (step S12 in FIG. 4).

Then, a model image Vi _t having a plurality of positions (X _tn , Y _tn ) and a direction θ _tn within a predetermined range of the target position (X _t , Y _t ) and the direction θ _{t for} a predetermined time _t is created (FIG. 4). Step S13). As shown in FIG. 5, in the present embodiment, the predetermined range (W1-W2) is a rectangular range (may be a circular range or the like) centered on the target position (X _t , Y _t ), and every predetermined interval Δw. A plurality of positions (X _tn , Y _tn ) are set (Δ mark in FIG. 5). Further, for each position (X _tn , Y _tn ), a plurality of θ _tn is set in the left and right predetermined angle range θw of the target direction θ _t for each predetermined angle interval Δθ (marked with x in FIG. 5). In the present embodiment, for example, W1 = W2 = 200 to 600 mm, Δw = 10 to 50 mm, θw = 1 to 10 °, and Δθ = 0.1 to 1.0 °.

FIG. 9 is a diagram illustrating a model image. The model image creating means 15 creates a two-dimensional model image viewed from the camera height H at an arbitrary position and direction of the moving body 1 based on a previously stored three-dimensional model. Therefore, the model image creating means 15 creates a plurality of positions of the _{(X tn, Y} tn) a and a direction theta _tn model image Vi _t of.

Also in the model image Vi _t, similar to the camera image Ri _t, all the pixels (all pixels) and HSV conversion, the brightness value V of the pixel composed of the hue H of the floor and the wall to "0", otherwise The luminance value V of the pixel is converted to “1” and binarized to create a model image Vi _{θi from} which the target object 60 is extracted (FIG. 10). FIG. 10 is a diagram illustrating a binarized model image.

Then, the matching degree calculating unit 16, and compares the camera image Ri _t and the model image Vi _t (step S14 in FIG. 4). FIG. 11 is a diagram illustrating a state in which the camera image and the model image are compared. Then, a matching degree CVi the camera image Ri _t and the model image Vi _t (step S15 in FIG. 4). Matching degree CVi is feature quantity at the camera image Ri _t and the model image Vi _t based on the pixel of the (luminance value V "0"), employing a ratio of pixels to match for all the pixels.

And the most matching degree higher one employs a model image Vi _t of the _{CVi, (X tn, Y tn} ) in the model image Vi _t and direction theta _tn, the self-position of the moving body 1 at a predetermined time t (X _R , Y _R ) and direction θ _R are determined (step S16 in FIG. 4). Based on the self-position (X _R , Y _R ) and the direction θ _R , the steering motor 40 and the traveling motor 50 are controlled so that the moving body 1 follows along the previously stored target trajectory ST (FIG. 4). Step S17). That is, the moving body 1 is controlled to travel toward the target position (X _{t + 1} , Y _{t + 1} ) on the target trajectory and the direction θ _{t + 1} at the predetermined time t + 1.

In the above description, the identification is performed based on the completely matching hue H, but it may be calculated for pixels matching the hue H ± Δh having a certain width. Further, in the above, by binarizing based on a camera image Ri _t and the model image Vi _t to the hue H, it was calculated matching degree CVi, without binarizing the color information in each pixel The degree of coincidence CVi may be calculated. In that case, one of hue H, saturation S, and brightness V, or a combination of them, depending on the type of color, such as identifying with gray (brightness) V, for example, instead of identifying with hue H alone. They can also be identified.

[Second Embodiment]
A second embodiment will be described. Detailed description of the same contents as those in the first embodiment will be omitted.
FIG. 12 is a block diagram illustrating a self-position recognition system according to the second embodiment. 13 and 14 are flowcharts for explaining the self-position recognition method of the second embodiment.

  First, the same three-dimensional model (FIG. 6) as the moving space 6 in which the moving body 1 travels is created by the three-dimensional model creating means 21 (step S01 in FIG. 13). Then, the target trajectory ST on which the mobile body 1 travels is stored in the target trajectory storage means 17 (step S02 in FIG. 13).

Camera image acquisition unit 10, at predetermined time t, acquiring a camera image Ri _t by camera 2 (step S21 in FIG. 13). Camera image Ri _t is the predetermined time t, the position of the moving body 1 _(X M, _{Y M)} and the direction theta _M image (FIG. 7).

[Position of reference lines rRL _t , lRL _t ]
FIG. 15 is a diagram illustrating a camera image that defines a reference line. The reference line defining means 12 defines the boundary vertical lines formed by the boundary between the target object (rack) 60 and the front wall in the binarized camera image Ri _t (FIG. 8) as the reference lines rRL _t and lRL _t. (Step S23 in FIG. 13). In the present embodiment, the target object 60 is a rack (shelf) formed by assembling columns and the like extending in the vertical direction and the horizontal direction, so that a boundary vertical line appears. As Figure 15, the coordinate system in the camera image Ri _t and IX-IY coordinates, the reference line _RRL t, the position of the LRL _t is the coordinate value of the IX-axis direction.

  Note that there may be only one boundary vertical line depending on the position of the moving object. Further, the setting of the boundary vertical line differs depending on the shape of the target object 60. For example, when the boundary between the target object 60 and the front wall is an arc, the boundary vertical line may be a vertical line in contact with the tip portion.

In the camera image Ri _t, it sets the passage center line Cp _t-1. The passage center line Cp _t-1 is based on the position (X _M , Y _M ) and the direction θ _M of the moving body 1 determined at the predetermined time t−1 and the center position (IX axis) of the passage 61 at the time t−1. The center position between each reference line rRL _t-1 and lRL _{t-1 in} the direction is adopted.

The reference line position specifying means 13 in the camera image specifies the positions of the reference lines rRL _t and lRL _t by the following procedure. In the camera image Ri _t, it sets a search height range Ra present the target object 60 to the left and right of the passage center line Cp _t-1. In the search height range Ra, from the passage center line Cp _t-1 in the camera image Ri _t, for each predetermined pixel interval in the IY direction, carries out the search towards the IX axis direction is identified as the target object 60 ( Identify left and right positions (identified as luminance value V "1").

Then, the respective positions specified for the left and right are sorted, and the average value for the position of the intermediate amount 1/2 is excluded except for the position of the upper amount 1/4 and the position of the lower amount 1/4 with low reliability. And the value is specified as the position of the left and right reference lines rRL _t and lRL _t .

Based on the position of the passage center line Cp _t-1 described above, the reference line _RRL t of the left and right from the image center vertical line in the camera image Ri _t, distance LRL _t Wpa, identifies the Wpb (step S24 in FIG. 13) .

Then, at a predetermined time t, the target position (X _t , Y _t ) and the direction θ _t on the target trajectory for the moving body 1 to move along the target trajectory ST are calculated (step S22 in FIG. 13).

FIG. 16 is a plan view showing the positional relationship between the reference line and the moving object. The reference line position specifying means 14 in the three-dimensional model specifies the position coordinates (Xq, Yq), (Xp, Yp) of the reference lines rRL _t and lRL _t in the model space (step S25 in FIG. 13). The position coordinates (Xq, Yq) and (Xp, Yp) are calculated by calculating the target position (X _t , Y _t ) and the direction θ _t at a predetermined time t (step S22 in FIG. 13). The position can be estimated and specified.

Based on the positional relationship between the reference lines rRL _t and lRL _t in the model space and the moving body 1 (FIG. 16), the following expressions (1) to (3) are derived.

... (1)

... (2)

... (3)

Here, (X _M , Y _M ) and θ _M are the position and direction of the moving body 1 in the model space. F is the focal length of the camera 2.

As described above, Wpa, Wpb is specified based on the camera image Ri _t (step S24 in FIG. 13). F is specified by the specifications of the camera 2. By inputting these into the equations (2) and (3), α and β are specified.

Furthermore, as described above, (Xq, Yq) and (Xp, Yp) are specified based on the three-dimensional model (step S25 in FIG. 13). Accordingly, when the direction θ _M of the moving body 1 is arbitrarily determined based on the expression (1), the position (X _M , Y _M ) of the moving body 1 is uniquely determined.

  The model image creating means 15 creates a model image viewed from the camera height H at an arbitrary position and direction of the moving body 1 based on the three-dimensional model (FIG. 9).

[Direction θ _M of moving body 1 in model space]
Next, a procedure for recognizing the direction θ _M of the moving body 1 by restraining the reference lines rRL _t and lRL _t will be described. First, the reference direction the direction theta _L of the moving body 1 recognized in the predetermined time t-1. Any angle Ψ about the reference direction theta _L and search angle range. This search angle range Ψ is equally divided into a predetermined number (seven in this example), and a set S _φ composed of φi (i = 1, 2,..., 7) is determined. _Sφ is shown in equation (4).

... (4)

With this φi as the direction of the moving body 1 in the model space, the position (X _φi , Y _φi ) of the moving body 1 in the model space is determined from the equation (1). Based on the determined position ( _Xφi , _Yφi ) and direction φi of the moving body 1 in the model space, the model image creating means 15 creates a model image _Viφi (step S26 in FIG. 13).

All pixels (all pixels) are subjected to HSV conversion, and the luminance value V of the pixel composed of the hue H of the floor surface and the wall surface is converted to “0”, and the luminance value V of the other pixels is converted to “1” to be binary. The model image Vi _φi having only the feature amount is extracted by extracting the target object 60 (FIG. 10).

Then, the matching degree calculating unit 16, performs comparison of the camera image Ri _t and the model image Vi _.theta.i (FIG. 11), determining the degree of matching CVi (step S27 in FIG. 13). Incidentally, the degree of coincidence CVi is feature quantity at the camera image Ri _t and the model image Vi _.phi.i based on pixels (luminance value V "0"), employing a ratio of pixels to match for all the pixels.

Repeated previous procedure (steps S26 and S27 in FIG. 13) in each direction phi ₁ to [phi] _7, as compared to the camera image Ri _t and the model image Vi _Fai1～7 obtained thereby, the respective degree of coincidence CV _{1 to} CV ₇ are obtained.

  FIG. 17 is a graph showing the relationship between an arbitrary direction and the degree of coincidence. The coincidence degree calculation means 16 performs approximation by the three-dimensional curve f (φ) by the least square method based on the coincidence degree CVi of the direction φi in i = 1 to 7 (step S28 in FIG. 13). Then, a direction φm that is maximum in the three-dimensional curve f (φ) is obtained (step S29 in FIG. 13).

The direction φm as a new reference direction theta _L, as the search angle range [psi / 2, do the previous steps (Step S26~S29 in FIG. 13), which is repeated _{once n,} self-position recognizer 18 Then, the direction θ _M of the moving body 1 in the model space is recognized (step S30 in FIG. 13).

From the above, the direction θ _M of the moving body 1 is recognized by the constraints of the reference lines rRL _t and lRL _t , and this is input to the equation (1) to obtain the position (X _θM , Y _θM ) of the moving body 1. The position of the moving body 1 _{(X M, Y} M) in order to improve the recognition accuracy, by the following procedure, the reference line _RRL t, without constraints LRL _t, camera image Ri _t and the model image Vi _θ1 to 7 are compared.

[Position (X _M , Y _M ) of the moving object 1 in the model space]
Next, a procedure for recognizing the position (X _M , Y _M ) of the moving body 1 will be described. As an arbitrary change amount δ, a position set (Sx, Sy) centered on the position (X _θM , Y _θM ) obtained above is created. The set Sx is expressed by equation (5), and the set Sy is expressed by equation (6). Therefore, the position set (Sx, Sy) is 9 (= 3 × 3) positions that are all combinations of the set Sx and the set Sy, and is a peripheral position of the position (X _θM , Y _θM ). The amount of change δ is about 5 cm in real space.

... (5)

... (6)

For each of the nine position sets (Sx, Sy), a model image Vi _θM is created by the model image creation means 15 (step S31 in FIG. 14). Then, a comparison between each of the nine model image Vi _.theta.M the camera image Ri _t (FIG. 11), determining the coincidence degree CVi a match degree calculating section 16 (step S32 in FIG. 14). Then, the position (X _θM , Y _θM ) is determined using the matching degree CVi that is the maximum value (step S33 in FIG. 14).

Further, with the position (X _θM , Y _θM ) as the center, an arbitrary amount of change is set to δ / 2, the previous procedure (steps S31 to S33 in FIG. 14) is performed, and this is repeated n ₂ times, The recognition means 18 recognizes the position (X _M , Y _M ) of the moving body 1 in the model space (step S34 in FIG. 14).

In the above description, when the direction θ _M of the moving body 1 is determined, the approximation by the cubic curve f (φ) is performed from the degree of coincidence CVi, but when the position (X _M , Y _M ) of the moving body 1 is determined. In addition, approximation by a cubic curve can be performed from the degree of coincidence CVi.

[Position (X _R , Y _R ) and Direction θ _R of Moving Body 1 in Real Space]
From the position (X _M , Y _M ) and direction θ _M of the moving body 1 in the model space recognized from the above, the position (X _R , Y _R ) and direction θ _R of the moving body 1 in real space are obtained. Each is determined by the equations (7) and (8) (step S35 in FIG. 14).

... (7)

... (8)

Here, K is a scale between the real space and the model space, (X _S , Y _S ) is an offset amount in the XY axis direction between the real space and the model space, and λ is a rotation angle between the real space and the model space. .

[Obstacle recognition]
Obstacle recognition means 19 performs the obstacle recognition self-position _(X R, _{Y R)} after recognizing and direction theta _R, based on the camera image Ri _t and the model image Vi _M (step S36 in FIG. 14). The model image Vi _M is created based on the position (X _M , Y _M ) and the direction θ _M of the moving body 1.

An area composed of the passage 61 and the wall surface (an area other than the target object 60) is an area for detecting an obstacle (detection area). The detection region is extracted in the model image Vi _M. Also extracts a detection region for the camera image Ri _t.

Then, the pixels in the detection region with respect to each of the camera images Ri _t and the model image Vi _M to HSV conversion, each hue (H), lightness (V), and less than the difference between the threshold of the saturation (S) to remove consisting pixel in the detection region of the camera image Ri _t, it creates an image obtained by extracting only the pixels of different colors and model image Vi _M. In this image, if there is a region having an area of a certain value or more, the region is recognized as an obstacle.

[Target trajectory tracking]
Based on the position (X _R , Y _R ) and the direction θ _R of the mobile body 1 identified as described above, the target trajectory tracking means 20 follows the target trajectory ST (traveling path) stored in advance. Thus, the steering motor 40 and the traveling motor 50 are controlled (step S37 in FIG. 14).

  Then, self-position recognition is performed every predetermined time interval Δt, and control is performed so that the moving body 1 follows the target trajectory ST. In the above method, since there are steps of adopting various values at the predetermined time t-1, the value at the start (initial value) is set in advance.

  When the obstacle is recognized by the obstacle recognition method described above, the target trajectory tracking means 20 controls the steering motor 40 and the traveling motor 50 so as to take a predetermined measure such as the moving body 1 stopping. To do.

[Others]
In the above-described embodiment, the self-position is recognized based on the camera image from the camera 2 facing the front of the moving body 1, but the camera image from the camera 2 facing the rear is used or the camera image from the front is used. And may be recognized in combination.

  In the above description, the automatic guided vehicle moving in the distribution warehouse where the rack is arranged has been described. However, a security robot (running or walking in the concert hall (moving space 6) where the seat (target object 60) is arranged ( The present invention can also be applied to the moving body 1) and the like, and can be employed in various moving bodies and moving spaces as long as it does not depart from the spirit of the present invention.

It is a figure which shows the mobile body carrying a self-position recognition system. It is a figure for demonstrating a target track | orbit. It is a block diagram which shows the self-position recognition system of 1st Embodiment. It is a flowchart figure explaining the self-position recognition method of 1st Embodiment. It is a figure for demonstrating the model image which 1st Embodiment compares. It is a conceptual diagram which shows the three-dimensional model in movement space. It is a figure which shows a camera image. It is a figure which shows the binarized camera image. It is a figure which shows a model image. It is a figure which shows the binarized model image. It is a figure which shows the state which compares a camera image and a model image. It is a block diagram which shows the self-position recognition system of 2nd Embodiment. It is a flowchart figure explaining the self-position recognition method of 2nd Embodiment. It is a flowchart figure following FIG. It is a figure which shows the camera image which prescribed | regulated the reference line. It is a top view which shows the positional relationship of a reference line and a mobile body. It is a graph which shows the relationship between arbitrary directions and a coincidence degree.

Explanation of symbols

1 moving space 60 the object 61 passages mobile 2 camera 3 controller 6 Ri _t camera image Vi _t model image _RRL t, LRL _t baseline theta _M direction _X M of the moving object in the model space, the moving body in the _{Y M} model space Position θ _R Direction of moving object in real space X _R , Y _R Position of moving object in real space

Claims (12)

A mobile body self-position recognition method for moving in a moving space along a pre-stored target trajectory,
Creating a three-dimensional model of the moving space;
Installing a target object in the moving space;
Obtaining a camera image from the position and direction of the moving body at a predetermined time;
Calculating a target position and direction on the target trajectory at the predetermined time;
Defining a boundary vertical line of the target object in the camera image as a reference line;
Identifying a distance from an image center vertical line in the camera image to the reference line;
Identifying the position coordinates of the reference line in the three-dimensional model;
Determining a position coordinate of the moving body based on a distance to the reference line in the camera image and a position coordinate of the reference line in the three-dimensional model;
Forming a model image of a plurality of directions in a predetermined angle range from the three-dimensional model with reference to the position coordinates of the moving body;
Calculating a degree of coincidence of color information between the plurality of model images and the camera image;
And a step of recognizing the position and direction of the moving body based on the degree of coincidence. The self-position recognition method according to claim 1, wherein the target object is a passage forming body that forms a passage. The camera image and the self-position recognition method according to claim 1 or 2, characterized in that it comprises a step of binarizing the model image. The method includes approximating a quadratic or cubic curve from each degree of coincidence, and recognizing both or one of the position and direction of the moving body based on the maximum value of the curve. 4. The self-position recognition method according to any one of 3 . The self-position recognition according to any one of claims 1 to 4 , further comprising a step of changing a shape of the target object in the three-dimensional model based on information from a storage management system for the moving space. Technique. Extracting a detection area other than the target object in the camera image and the model image, comparing each detection area, and an obstacle in the direction of the moving object based on the comparison value and a predetermined threshold self location recognition method according to any one of claims 1 to 5, characterized in that it comprises a step of recognizing objects. A mobile body self-position recognition system that moves in a moving space along a pre-stored target trajectory,
Means for creating a three-dimensional model of the moving space;
Means for installing a target object in the moving space;
Means for acquiring a camera image from the position and direction of the moving body at a predetermined time;
Means for calculating a target position and direction on the target trajectory at the predetermined time;
Means for defining a boundary vertical line of the target object in the camera image as a reference line;
Means for specifying a distance from an image center vertical line in the camera image to the reference line;
Means for specifying position coordinates of the reference line in the three-dimensional model;
Means for determining a position coordinate of the moving body based on a distance to the reference line in the camera image and a position coordinate of the reference line in the three-dimensional model;
Means for forming a model image of a plurality of directions in a predetermined angle range from the three-dimensional model with reference to the position coordinates of the moving body;
Means for calculating the degree of coincidence of the color information between the plurality of model images and the camera image;
A self-position recognition system comprising: means for recognizing the position and direction of the moving body based on the degree of coincidence. The self-position recognition system according to claim 7, wherein the target object is a passage forming body that forms a passage. The self-position recognition system according to claim 7 or 8, further comprising means for binarizing the camera image and the model image. Approximating said from the coincidence degree in the second or third order curve, claim 7, characterized in that it comprises means for recognizing the position and orientation one or both of the moving object based on the maximum value of the curve The self-position recognition system in any one of -9 . The self-position according to any one of claims 7 to 10 , further comprising means for changing the shape of the target object in the three-dimensional model based on information from the storage management system for the moving space. Recognition system. A means for extracting a detection area other than the target object in the camera image and the model image, a means for comparing the detection areas, and an obstacle in the direction of the moving body based on the comparison value and a predetermined threshold value The self-position recognition system according to any one of claims 7 to 11 , further comprising means for recognizing an object.