JP5477167B2 - Moving body - Google Patents

Description

  The present invention relates to a technique for measuring the position of a moving body that moves on a floor surface.

  Patent Document 1 discloses a moving body that moves on the floor surface while detecting its own position and angle (orientation) (hereinafter, the position and angle are collectively referred to as position information). This moving body stores a plurality of data indicating the surface shape of an obstacle (for example, a wall or the like) extending linearly in a direction parallel to the floor surface. The surface shape data is represented by a global coordinate system based on the floor surface. The moving body calculates its own position information while moving on the floor surface, and corrects its own position information by an extended Kalman filter (hereinafter referred to as EKF). Specifically, the moving body repeats the following process while moving. First, self position information on the floor surface (position information in the global coordinate system) is calculated by odometry. Next, the light beam is scanned by the laser range finder, and the surface shape in the direction parallel to the floor surface of the obstacle present on the scanning surface is detected. According to the laser range finder, the surface shape of the obstacle is detected as a coordinate point group on a relative coordinate system with respect to the moving body. Next, the moving body calculates the surface shape of the obstacle by linearly approximating the coordinate point group. Next, surface shape data corresponding to the detected surface shape (surface shape approximated by a straight line) is specified from the stored surface shape data. This specification is performed in consideration of a surface shape detection error (that is, a detection error by a laser range finder). Next, a deviation between the specified surface shape data and the detected surface shape is calculated, and the position information of itself is corrected from the deviation. In this way, the surface shape data of the obstacle is stored in the moving body in advance, and the surface shape of the obstacle is detected by a laser range finder while moving, and the detected surface shape and the corresponding surface shape data are detected. By correcting the position information of the self based on the deviation, the position information of the self can be specified more accurately.

A. Garulli, A. Giannitrapani, A. Rossi, A. Vicino, "Mobile robot SLAM for line-based environment representation," Tech. Rep. 2005-3, Dipartimento di Ingegneria dell'Informazone, Universita di Siena, 2005

  When detecting the surface shape of an obstacle, an error occurs. For this reason, in order to accurately specify the surface shape data corresponding to the detected surface shape from the plurality of surface shape data, it is necessary to consider an error. In the technique of Non-Patent Document 1 described above, the corresponding surface shape data is specified in consideration of the error of the detected surface shape (that is, the detection error by the laser range finder). The surface shape data may not be accurately identified. For this reason, there is a case where the position information of the self cannot be corrected accurately.

  In the present specification, in view of the above-described situation, a moving body capable of more accurately specifying surface shape data corresponding to a detected surface shape and correcting its own position information more accurately is provided.

  The surface shape of the obstacle detected by the moving body is represented by a relative coordinate system with respect to the moving body. On the other hand, the surface shape data stored in the moving body is represented by a global coordinate system. Therefore, the surface shape data corresponding to the detected surface shape is specified based on the detected surface shape, the stored surface shape data, and the position information of the moving body. For this reason, not only the detected surface shape error but also the positional information error of the moving body affects this specification. The present inventors pay attention to this point and propose to specify surface shape data corresponding to the detected surface shape in consideration of errors in position information of the moving body.

  The moving body disclosed in the specification of the present application moves on a floor surface on which an obstacle exists. The moving body includes a storage unit, a self-position / angle calculation unit, an obstacle detection unit, a matching unit, and a self-position / angle correction unit. The storage unit stores a plurality of pieces of surface shape data representing, by a global coordinate system, a surface shape of an obstacle extending linearly in a direction parallel to the floor surface in a direction parallel to the floor surface. The self-position / angle calculation unit calculates the position and angle of the moving body using the global coordinate system. The obstacle detection unit scans the light beam and detects the surface shape of the obstacle existing on the scanning plane in a direction parallel to the floor surface by using a relative coordinate system with respect to the moving object. The matching unit is stored in the storage unit based on the position and angle of the moving body calculated by the self-position / angle calculation unit and their errors, the surface shape detected by the obstacle detection unit, and the error. Surface shape data corresponding to the surface shape detected by the obstacle detection unit is specified from the plurality of surface shape data. The self-position / angle correction unit corrects the position and angle calculated by the self-position / angle calculation unit from the deviation between the surface shape detected by the obstacle detection unit and the surface shape data specified by the matching unit.

The “global coordinate system” and the “relative coordinate system” may be a three-dimensional coordinate system or a two-dimensional coordinate system. If the “global coordinate system” and the “relative coordinate system” are two-dimensional coordinate systems, the amount of calculation performed by the moving object can be reduced. When a two-dimensional coordinate system is adopted, it may be a coordinate system defined by two coordinate axes parallel to the floor surface (for example, an orthogonal coordinate system), or specified by a distance and an angle in a plane parallel to the floor surface. It may be a polar coordinate system. The coordinate system of the surface shape data may be different from the coordinate system of the position and angle of the moving body. For example, the surface shape data may be represented by a polar coordinate system, and the position of the moving body may be represented by an orthogonal coordinate system. Further, the surface shape data, the position and angle of the moving body, and the surface shape of the obstacle detected by the obstacle detection unit may be defined by at least a shape (coordinate component) in a direction parallel to the floor surface, A coordinate component in a direction perpendicular to the floor surface may be included. For example, the surface shape data may be data indicating a three-dimensional shape of the surface of the obstacle including a coordinate component in a direction perpendicular to the floor surface.
In addition, the “deviation between the surface shape detected by the obstacle detection unit and the surface shape data specified by the matching unit” is the “surface shape detected by the obstacle detection unit” indicated in the relative coordinate system and the global coordinate system. This means a deviation when comparing the “surface shape data specified by the matching unit” in the same coordinate system. Therefore, the “surface shape detected by the obstacle detection unit” may be coordinate-transformed into the global coordinate system, and these deviations may be calculated in the global coordinate system. Alternatively, the “surface shape data specified by the matching unit” may be coordinate-converted into a relative coordinate system, and these deviations may be calculated in the relative coordinate system.
In addition, the above “error” means a value indicating the magnitude of a deviation that may occur between an actual characteristic value and a detected value when a certain characteristic value is detected (or calculated).
Further, the surface shape of the obstacle detected by the obstacle detection unit may represent the surface shape of the obstacle existing on the scanning plane of the light beam by a plurality of coordinate points, and is defined by two coordinate points. The surface shape may be represented by a straight line (or line segment). When the surface shape of the obstacle is represented by a straight line (or line segment), for example, a laser range finder that detects the surface shape of the obstacle existing on the scanning plane of the light beam by a plurality of coordinate points, and a laser range finder The obstacle detection unit can be configured by a calculation unit that calculates a straight line (or line segment) obtained by linearly approximating a plurality of detected coordinate points.
The self-position / angle calculation unit may calculate the position and angle of the moving body by odometry, may calculate the position and angle of the moving body by GPS, or may be calculated by other techniques. Good.

  In this moving body, the self-position / angle calculation unit calculates the position and angle of the moving body during movement. In addition, during the movement, the obstacle detection unit detects the surface shape of the obstacle. Then, the matching unit specifies surface shape data corresponding to the surface shape detected by the obstacle detection unit from the plurality of surface shape data stored in the storage unit. At this time, the matching unit specifies surface shape data corresponding to the detected surface shape in consideration of not only the surface shape error detected by the obstacle detection unit but also the position and angle error of the moving object. . Therefore, the surface shape data corresponding to the detected surface shape can be accurately specified. The self-position / angle correcting unit corrects the position and angle of the moving body based on the deviation between the specified surface shape data and the detected surface shape. Since the corresponding surface shape data is accurately specified, the self-position / angle correction unit can correct the position and angle of the moving body accurately.

  In the moving body described above, it is preferable that the obstacle detection unit detects a surface shape in a direction parallel to the floor surface of the obstacle as a line segment in the relative coordinate system. In addition, the moving body calculates an error of deviation between the surface shape detected by the obstacle detection unit and the surface shape data specified by the matching unit based on the position error of only the both ends of the detected line segment. It is preferable to further include a deviation error calculation unit. The self-position / angle correcting unit preferably corrects the position and angle of the moving body based on the error calculated by the deviation error calculating unit.

  According to such a configuration, since the error of the deviation is calculated only from the error of the position of both ends of the detected line segment, compared to the case of calculating the error of the deviation from the error of the coordinate point of the entire surface shape, It is possible to reduce the amount of calculation performed by the mobile object. This makes it possible to specify the position and angle of the moving body at a higher speed.

  In the moving body described above, it is preferable that the obstacle detection unit detects a surface shape in a direction parallel to the floor surface of the obstacle as a line segment in the relative coordinate system. In addition, the matching unit calculates the probability that both ends of the line segment exist on the same surface shape data based on the error of the position of both ends of the detected line segment, and the calculated probability It is preferable to specify the surface shape data having the highest value.

  According to such a configuration, since the corresponding surface shape data is specified based on the existence probabilities on the surface shape data of both ends of the detected line segment, the existence probabilities of the coordinate points of the entire surface shape are calculated. Compared to the case, it is possible to reduce the amount of calculation performed by the moving object. This makes it possible to specify the position and angle of the moving body at a higher speed.

  The present specification also provides the following moving objects. This moving body moves on the floor surface where the obstacle exists. The moving body includes a storage unit, a self-position / angle calculation unit, an obstacle detection unit, a matching unit, and a self-position / angle correction unit. A memory | storage part memorize | stores the surface shape data which represents the surface shape in the direction parallel to the floor surface of the obstruction which extends linearly in the direction parallel to a floor surface by a global coordinate system. The self-position / angle calculation unit calculates the position and angle of the moving body using the global coordinate system. The obstacle detection unit scans the light beam and detects the surface shape of the obstacle existing on the scanning plane in a direction parallel to the floor surface by using a relative coordinate system with respect to the moving object. When a plurality of surface shapes are detected by the obstacle detection unit, the matching unit detects the position and angle of the moving body calculated by the self-position / angle calculation unit and their errors, and is detected by the obstacle detection unit. Based on the plurality of surface shapes and their errors, the surface shape corresponding to the surface shape data stored in the storage unit is specified from among the plurality of surface shapes detected by the obstacle detection unit. The self-position / angle correction unit corrects the position and angle calculated by the self-position / angle calculation unit from the deviation between the surface shape specified by the matching unit and the surface shape data stored in the storage unit. -It has an angle correction part.

  According to this moving body, when a plurality of surface shapes are detected by the obstacle detection unit, the surface shape corresponding to the surface shape data stored in the storage unit is accurately specified from these surface shapes. be able to. Therefore, the position and angle of the moving body can be corrected accurately.

1 is a schematic perspective view of an autonomous mobile body 10. FIG. The block diagram of the autonomous mobile body 10. FIG. Explanatory drawing of environmental map. Explanatory drawing of the measuring method of the laser range finder 30. FIG. Explanatory drawing of the angle of the autonomous mobile body 10. FIG. The flowchart which shows the process which the control apparatus 50 performs. Explanatory drawing which shows the calculation method of the target line segment 120. FIG. The flowchart which shows the detailed process of step S10. Explanatory drawing about the point P and area | region Q on a line segment. Explanatory drawing regarding the line segment corresponding to the target line segment 120. FIG. Explanatory drawing of the positional information on the line segment 100f corresponding to the target line segment 120. FIG. Explanatory drawing which shows the case where two line segments are detected.

The preferred features of the embodiments described below are summarized.
(Feature 1) The storage unit stores data indicating a plurality of line segments representing the surface shape of the obstacle.
(Feature 2) The moving body includes a line segment calculation unit that calculates a line segment obtained by linear approximation of the surface shape detected by the obstacle detection unit.
(Feature 3) The matching unit identifies a line segment corresponding to the line segment (target line segment) calculated by the line segment calculation unit from among the plurality of line segments stored in the storage unit.
(Feature 4) The self-position / angle correction unit calculates a deviation between the target line segment and the corresponding line segment and the error of the deviation, and based on the deviation and the error of the deviation, Correct position and angle.

  FIG. 1: has shown the schematic perspective view of the autonomous mobile body 10 which concerns on an Example. The autonomous mobile body 10 has two driving wheels 12a and 12b and a driven wheel 12c, and moves on a horizontal floor by driving the driving wheels 12a and 12b. The driven wheel 12c rotates as the autonomous mobile body 10 moves. When the drive wheel 12a and the drive wheel 12b are driven at the same rotational speed, the autonomous mobile body 10 translates in the direction of the arrow F1 in FIG. Further, when the driving wheel 12a and the driving wheel 12b are driven at different rotational speeds, the autonomous moving body 10 moves while curving on the floor surface. The autonomous mobile body 10 autonomously moves to the destination while calculating its own position on the floor surface.

  FIG. 2 shows a functional block diagram of each part of the autonomous mobile body 10. The autonomous mobile body 10 includes a storage device 20, a laser range finder 30, servomotors 40a and 40b, and a control device 50. As shown in FIG. 1, the laser range finder 30 is installed on the upper part of the autonomous mobile body 10. The storage device 20, the servo motors 40 a and 40 b, and the control device 50 are incorporated in the autonomous mobile body 10.

The storage device 20 stores an environment map indicating the surface shape of an obstacle (such as a wall) existing on the floor surface on which the autonomous mobile body 10 moves. The environment map is data representing the surface shape of an obstacle in a horizontal plane by a line segment. FIG. 3 shows an example of a line group represented by the environment map. In FIG. 3, line segments 100a to 100j are shown. In the environment map, the line segments 100a to 100h are defined by the coordinates of the start point and the end point. The coordinates of the start point and the end point are determined by an orthogonal coordinate system (hereinafter referred to as a global coordinate system) using an x-axis and a y-axis set in the horizontal direction with reference to the floor surface. For example, the line segment 100a in FIG. 3 is represented by a start point coordinate (x _a1 , y _a1 ) and an end point coordinate (x _a2 , y _a2 ). The line segment 100b is represented by a start point coordinate ( _xb1 , _yb1 ) and an end point coordinate ( _xb2 , _yb2 ). In the example of FIG. 3, the end point coordinates (x _a2 , y _a2 ) of the line segment 100a and the start point coordinates (x _b1 , y _b1 ) of the line segment 100b are equal. As for the start point and end point of each line segment, the right side of the vector from the start point to the end point is the obstacle side (the area where the autonomous mobile body 10 cannot enter), and the left side of the vector is the area where the autonomous mobile body 10 can move. Show. In FIG. 3, an area surrounded by line segments 100 a to 100 j is an area where the autonomous mobile body 10 can move, and an outer area is an obstacle (an area where the autonomous mobile body 10 cannot enter).

  The laser range finder 30 scans light rays in a horizontal plane and detects the surface shape of an obstacle existing in the scanning plane. FIG. 4 is a plan view showing the laser range finder 30 from above. First, the laser range finder 30 irradiates laser light to the right side by an angle w1 (angle when viewed in a horizontal plane) from the traveling direction F1 of the autonomous mobile body 10. And the reflected light of the irradiated laser beam is detected, and the distance r1 (distance in the horizontal plane) to the obstacle existing in the direction of the angle w1 is detected. That is, the relative position coordinate (r1, w1) with respect to the autonomous mobile body 10 of the location irradiated with the laser beam is detected. Next, the laser beam is irradiated in the direction of the angle w2 that is slightly shifted to the left side from the start angle w1, and the distance r2 to the portion irradiated with the laser beam is detected. That is, the relative position coordinates (r2, w2) with respect to the autonomous mobile body 10 at the location irradiated with the laser light are detected. The laser range finder 30 repeats the same processing at a constant angular interval from the start angle w1 to the end angle wn (angle on the left side of the traveling direction F1 of the autonomous mobile body 10), and the autonomous mobile body 10 at the location irradiated with the laser. Relative position coordinates (r1, w1), (r2, w2), (r3, w3)... (Rn, wn) are detected. The coordinate point groups (r1, w1) to (rn, wn) detected by the laser range finder 30 represent the surface shape of the obstacle. The surface shape of the obstacle detected by the laser range finder 30 is input to the control device 50.

The servo motor 40a has a servo control circuit 42a, a motor 44a, and an encoder 46a. The motor 44a rotates the drive wheel 12a. A control command value is input from the control device 50 to the servo control circuit 42a. The servo control circuit 42a drives the motor 44a according to the input control command value. Therefore, the drive wheel 12a rotates according to the control command value. The encoder 46a detects the rotation speed of the motor 44a (that is, the drive wheel 12a). The rotation speed detected by the encoder 46 a is input to the control device 50.
The servo motor 40b has a servo control circuit 42b, a motor 44b, and an encoder 46b. The motor 44b rotates the drive wheel 12b. A control command value is input from the control device 50 to the servo control circuit 42b. The servo control circuit 42b drives the motor 44b according to the input control command value. Therefore, the drive wheel 12b rotates according to the control command value. The encoder 46b detects the rotation speed of the motor 44b (that is, the drive wheel 12b). The rotation speed detected by the encoder 46b is input to the control device 50.

  The control device 50 is electrically connected to the storage device 20, the laser range finder 30, and the servo motors 40a and 40b. The control device 50 can read data from the storage device 20 and can store data in the storage device 20. The control device 50 calculates the position of the autonomous mobile body 10 (position of the autonomous mobile body 10 in the horizontal plane) and the angle (direction of the traveling direction F1 of the autonomous mobile body 10 in the horizontal plane) by calculating the position and angle described later. calculate. Note that the position of the autonomous mobile body 10 is represented by the xy coordinates of the global coordinate system described above, and the angle of the autonomous mobile body 10 is the progression of the autonomous mobile body 10 with respect to the x axis of the global coordinate system, as shown in FIG. It is represented by an angle φ in the direction F1. Hereinafter, the position and angle of the autonomous mobile body 10 may be referred to as position information. The control device 50 inputs a control command value to the servo motors 40a and 40b based on the calculated position information of the autonomous mobile body 10. The control device 50 calculates the position information of the autonomous mobile body 10 and repeats the process of inputting the control command value to the servo motors 40a and 40b based on the position information, so that the autonomous mobile body 10 moves toward the target point. Move autonomously.

Next, processing executed by the control device 50 will be described in detail. The flowchart in FIG. 6 illustrates processing executed by the control device 50. At the start of the process of FIG. 6, the initial position information (x ₀ , y ₀ , φ ₀ ) of the autonomous mobile body 10 (position and angle of the autonomous mobile body 10 at the time of disclosure of the process) The location of the point is entered. The control device 50 moves while calculating the position information of the autonomous mobile body 10 by repeating steps S2 to S16 shown in FIG.

In step S <b> 2, the control device 50 calculates the future route of the autonomous mobile body 10 based on the current position information of the autonomous mobile body 10, the position of the target point, and the environment map (the position of the obstacle). In the first step S2, the control device 50 calculates a route using the initial position information (x ₀ , y ₀ , φ ₀ ) input at the start as current position information. In step S2 after the second time, the control device 50 determines the position information X _{k−1 | k−1} (= x _{k−1 | k−1} , y) of the autonomous mobile body 10 corrected in step S16 of the immediately preceding cycle. _{k−1 | k−1} , φ _{k−1 | k−1} ) is used as the current position information to calculate a route. After calculating the route, the control device 50 inputs a control command value to the servo motors 40a and 40b so that the autonomous mobile body 10 moves along the calculated route. The servo motors 40a and 40b drive the drive wheels 12a and 12b according to the control command value input in the immediately preceding step S2 until the next step S2 is executed. Thereby, the autonomous mobile body 10 moves. When the drive wheels 12a and 12b are driven, the rotation speeds of the drive wheels 12a and 12b are detected by the encoders 46a and 46b.

  Steps S4 to S16 are executed after a predetermined time has elapsed since step S2. Thereby, the current position information of the autonomous mobile body 10 is calculated. The processes in steps S4 to S16 are performed based on the extended Kalman filter.

In step S4, the control device 50 uses the odometry to determine the current position information X _{k | k−1} (= (x _{k | k−1} , y _{k | k−1} , φ _{k | k−1} )) of the autonomous mobile body 10. ) Is calculated. Specifically, the control device 50 detects the rotational speeds of the drive wheels 12a and 12b detected by the encoders 46a and 46b, and the position information X _{k−1 | k of} the autonomous mobile body 10 corrected in step S16 of the immediately preceding cycle. ₋₁ (past position information), current position information X _{k | k−1} of the autonomous mobile body 10 is calculated.
In step S4, the position information _{X k | k-1} calculated at the same time, the position information _{X k |} calculating an error sigma _R of _k-1. Incidentally, the error sigma _R is located _{(x k | k-1,} y k | k-1) and the angle phi _{k |} dispersing errors that may occur _{k-1 (vx k | k} -1, vy k | k- ₁ , vφ _{k | k−1} ). Error sigma _R can be calculated according to the techniques described in the literature "S. Thrun, W.burgard, D. Fox, " Probabilistic robotics ", The MIT Press, 2005 ". That is, the error sigma _R is calculated based on the route of the autonomous moving body 10 has moved in the past. The larger the moving distance in the x direction (the value obtained by integrating the absolute values of the moving distances in the x direction in each period) is, the larger the error vx _{k | k−1} is calculated. The larger the moving distance in the y direction is, the larger the error vy _{k is. | K−1} is calculated to be larger, and the error vφ _{k | k−1} is calculated to be larger as the change amount of the angle φ (a value obtained by integrating the absolute value of the change amount of the angle φ in each period) is larger. Incidentally, the error sigma _R may be calculated by other methods described above. Further, when the error sigma _R does not change over time, the error sigma _R may be stored in the storage device 20.

  In step S <b> 6, the control device 50 scans the light beam in the space with the laser range finder 30 and detects the surface shape of the obstacle existing in the scanning range. As described above, the laser range finder 30 uses the coordinate point group (r1, w1), (r2, w2), (r3, w3),. It is detected as (rn, wn).

In step S8, the control device 50 calculates a line segment by linearly approximating the coordinate point group detected by the laser range finder 30 in step S6. The line segment calculation process in step S8 can be executed in accordance with, for example, the document “J. Xavier,“ Rast Line, Arc / Circle and Leg Detection from Laser Scan Data in Player Driver ”, ICRA, 2005”. Specifically, the line segment 120 is calculated by linearly approximating the coordinate point groups (r1, w1) to (rn, wn) as shown in FIG. This line segment is defined by the coordinates (ss, ts) of the start point and the coordinates of the end point (ss, ts) by an orthogonal coordinate system (hereinafter referred to as a local coordinate system) based on the s axis and the t axis set in the horizontal direction with respect to the autonomous mobile body 10. se, te). The start point (ss, ts) indicates the end of the line segment on the side (coordinate point (r1, w1) side) where the laser range finder 30 starts scanning the light beam, and the end point (se, te) is the end point (se, te). The end of the line segment on the opposite side (coordinate point (rn, wn) side) is shown. Next, the control device 50 converts the coordinates of the start point (ss, ts) and end point coordinates (se, te) of the line segment expressed in the relative coordinate system into coordinates (xs, ys) on the global coordinate system. ), (Xe, ye). This coordinate conversion is performed based on the position information X _{k | k−1} of the autonomous mobile body 10 calculated in step S4. Hereinafter, the line segment calculated in step S8 is referred to as a target line segment.

  In step S <b> 10, the control device 50 identifies a line segment corresponding to the target line segment from the line segments defined by the environment map stored in the storage device 20. The line segment identification process corresponding to the target line segment is executed according to the flowchart shown in FIG.

At step S40, the control unit 50 calculates the error sigma ₂ position error sigma ₁ and end positions of the start point of the target segment. The error Σ ₁ represents an error that may occur at the position (xs, ys) of the start point of the target line segment by the variance (vxs, vys), and the error Σ ₂ represents the position (xe, The error that can occur in (ye) is represented by the variance (vxe, vye). As described above, the target line segment is calculated based on the coordinate point group detected by the laser range finder 30 and the position information X _{k | k−1} of the autonomous mobile body 10. Therefore, a measurement error of the laser range finder 30, the position information X _k of the autonomous moving body 10 _| by the influence of the _k-1 error sigma _R, an error occurs in the position of the start point and the end point of the target segment. Therefore, the control unit 50, a measurement error of the laser range finder 30, the position information X _{k |} calculated based on the _k-1 error sigma _R, the error sigma ₁ and the end point of the start point of the target segment error sigma ₂ To do. Specifically, the control device 50 calculates these errors by the following mathematical formula.
Σ ₁ = J ₁ × Σ _R × J ₁ ^T + Σ _O
Σ ₂ = J ₂ × Σ _R × J ₂ ^T + Σ _O
Here, sigma _O is for the starting point and the error of the position of the end point that occurs based on the measurement error of the laser range finder 30, expressed in the variance of x and y coordinates. In this embodiment, the error sigma _O is can be considered a fixed value, error sigma _O is stored in advance in the storage device 20. J ₁ is the Jacobian matrix of the start point of the line segment related to the position information (x, y, φ) of the autonomous mobile body 10, and J ₁ ^T is the transposed matrix of the Jacobian matrix J ₁ . J ₂ is the Jacobian matrix at the end of the line segment related to the position information (x, y, φ) of the autonomous mobile body 10, and J ₂ ^T is the transposed matrix of the Jacobian matrix J ₂ . The Jacobian matrices J ₁ and J ₂ are represented by the following formula 1.

In step S42, the position of the start point (xs, ys) and on the basis of the error sigma _1, calculates the probability density function for the position of the starting point. The position of the end point (xe, ye) and on the basis of the error sigma _2, calculates the probability density function for the position of the end point. The probability density function at the position of the starting point represents the probability that the starting point exists as a function of the x coordinate and the y coordinate. By integrating the probability density function in the xy coordinate range, the probability that the start point exists in the coordinate range can be calculated. The probability density function at the end point also represents the probability that the end point exists as a function of the x coordinate and the y coordinate, like the probability density function at the start point.

  In steps S44 to S50, a line segment corresponding to the target line segment is specified from a plurality of line segments in the environment map based on the probability density function calculated in step S42.

  In step S44, the control device 50 extracts a line segment having the same direction as the direction of the target line segment (direction from the start point toward the end point) from the environment map. Whether the directions are equal is whether the inner product of the vector indicating the target line segment (vector from the start point to the end point) and the vector indicating each line segment in the environment map (vector from the start point to the end point) is greater than zero. It is done by judging by. That is, a line segment having a vector whose angle with respect to the vector of the target line segment is less than 90 degrees is specified as a line segment having the same direction. As described above, the area on the right side of the line segment in the environment map indicates an obstacle. The direction of the target line segment indicates the scanning direction by the laser range finder 30. Therefore, it is impossible that the direction of the target line segment and the direction of the corresponding line segment in the environment map are reversed because the laser range finder 30 exists inside the obstacle. Therefore, a line segment having the same direction as the target line segment is extracted in step S44, and the process of step S46 is performed only for the extracted line segment. In this way, by extracting only line segments having the same direction and executing step S46, the load on the control device 50 is reduced.

In step S46, the control device 50 calculates, for each line segment extracted in step S44, a probability that both the start point and the end point of the target line segment exist on the line segment. Specifically, the following processing is executed for each of the extracted line segments.
First, the points on the extracted line segment are extracted at regular intervals. Note that the extracted start point and end point of the line segment are extracted regardless of the interval between adjacent points. Next, as shown in FIG. 9, a square area Q having a diagonal length equal to the interval is set around each extracted point (shown as a point P in FIG. 9). Next, the probability that the starting point exists on the point P is calculated for each point P by substituting the coordinates of each extracted point P into the probability density function for the starting point calculated in step S42. Next, the probability that the start point exists on the point P is multiplied by the area of the square region Q corresponding to the point P to calculate the probability that the start point exists in the region Q for each region Q. Next, the sum of the existence probabilities of the start points for all the regions Q set for the line segment is calculated. The calculated sum can be approximated as the probability that the starting point exists on the line segment.
Next, the probability that the end point exists on the point P is calculated for each extracted point P by substituting the coordinates of each extracted point P into the probability density function for the end point calculated in step S42. Next, the probability that an end point exists in the region Q is calculated by multiplying the probability that the end point exists on the point P by the area of the square region Q corresponding to the point P. Next, the total sum of the existence probabilities of the end points for all the regions Q set for the line segment is calculated. The calculated sum can be approximated as a probability that an end point exists on the line segment.
Finally, the start point existence probability and the end point existence probability are multiplied. Accordingly, the probability that both the start point and the end point of the target line segment exist on the line segment (more specifically, within a predetermined range from the line segment) is calculated.

In step S48, the control device 50 specifies the maximum value among the probabilities calculated in step S46. Then, it is determined whether or not the specified maximum value exceeds a threshold value. If the specified maximum value does not exceed the threshold value, the control device 50 reports NULL data and stops the process of FIG. When NULL data is reported, steps S12 and S14 in FIG. 6 are not executed, and the next step S2 is the position information X _{k | k−1} that has not been corrected (position information calculated in step S4). Based on. If the specified maximum value exceeds the threshold value, the line segment having the maximum value is specified as a line segment corresponding to the target line segment in step S50. Thus, the process of FIG. 8 (that is, step S10 of FIG. 6) ends.

As described above, in step S42, not only the measurement error sigma _O of the laser range finder 30, the position information X _k of the autonomous moving body 10 _| also contemplates error sigma _R of _k-1, a probability distribution function calculate. Then, based on the probability distribution function, a line segment corresponding to the target line segment is specified from the environment map. In this way, by taking into account the error sigma _R which changes from moment to moment, it is possible to specify the line segments corresponding to more accurately target segment. For example, considering the case where the line segments 100d to 100f in the environment map and the target line segment 120 have the positional relationship shown in FIG. 10, if the error in the position information of the autonomous mobile body 10 is large in the x direction, the corresponding line A line segment 100f is specified as the minute. If the error of the position information of the autonomous mobile body 10 is large in the y direction, the line segment 100d is specified as the corresponding line segment. In this way, it is possible to perform accurate association by considering the error of the position information of the autonomous mobile body 10.

  In step S <b> 12 of FIG. 6, the control device 50 calculates relative position information with respect to the autonomous mobile body 10 for each of the target line segment and the corresponding line segment. The relative position information of these line segments is represented by a distance to the autonomous mobile body 10 and an angle with respect to the traveling direction F1 of the autonomous mobile body 10. For example, as shown in FIG. 11, when the target line segment 120 and the corresponding line segment 100f are specified, the relative position information of the target line segment 120 is the distance ra (target The distance in the perpendicular direction of the line segment) and the angle wa (the angle between the vertical line of the target line segment and the travel direction F1) with respect to the travel direction F1 of the autonomous mobile body 10. The relative position information of the corresponding line segment 100f includes the distance rb to the autonomous mobile body 10 (distance in the perpendicular direction of the corresponding line segment) and the angle wb (corresponding line segment) with respect to the traveling direction F1 of the autonomous mobile body 10. The angle between the vertical line and the traveling direction F1). Next, the control device 50 calculates a shift (dr, dw) in relative position information between the target line segment and the corresponding line segment. The length deviation dr is a value obtained by subtracting the distance rb from the distance ra, and the angle deviation dw is a value obtained by subtracting the angle wb from the angle wa.

また、角度の誤差ｖｄｗは、対象線分の始点及び終点の誤差Σ_Ｏを以下の計算式により座標変換することで得られる。
ｖｄｗ＝Ｊ_４×Σ_Ｏ×Ｊ_４ ^Ｔ
ここで、Ｊ_４は、対象線分の始点と終点の位置に関する角度ｗａのヤコビ行列であり、Ｊ_４ ^Ｔはヤコビ行列Ｊ_４の転置行列である。ヤコビ行列Ｊ_４は以下の数３により表される。

In step S <b> 14, the control device 50 calculates the error Σ ₃ (= (vdr, vdw)) of the deviation (dr, dw) described above. The error vdr is a variance representing an error that can occur when calculating the distance deviation dr, and the error vdw is a variance representing an error that can occur when calculating the angle deviation dw. Since the position information of the corresponding segments are those defined in the environment map, the error sigma ₃ of the above-mentioned deviation is equal to the detection error of the target segment relative position information (ra, wa).
Therefore, the error vdr distances are calculated by coordinate transformation by the following equation error sigma _O of start and end points of the target segment.
vdr = J ₃ × Σ _O × J ₃ ^T
Here, J ₃ is a Jacobian matrix of the distance ra regarding the position of the start point and the end point of the target line segment, and J ₃ ^T is a transposed matrix of the Jacobian matrix J ₃ . The Jacobian matrix J ₃ is represented by the following formula 2.

The error vdw angles are obtained by coordinate conversion by the following equation error sigma _O of start and end points of the target segment.
vdw = J ₄ × Σ _O × J ₄ ^T
Here, J ₄ is a Jacobian matrix of the angle wa regarding the position of the start point and the end point of the target line segment, and J ₄ ^T is a transposed matrix of the Jacobian matrix J ₄ . The Jacobian matrix J ₄ is expressed by the following Equation 3.

In step S16, the control device 50 determines in step S4 based on the deviation (dr, dw) of the relative position calculated in step S12 and the deviation error Σ ₃ (= (vdr, vdw)) calculated in step S14. The calculated position information X _{k | k−1} (= (x _{k | k−1} , y _{k | k−1} , φ _{k | k−1} )) of the autonomous mobile body 10 is corrected, and the corrected position information X _{k | K} (= (x _{k | k} , y _{k | k} , φ _{k | k} )) The position information of the autonomous mobile body 10 is corrected by a general EKF technique. Thrun, W. burgard, D. Fox, “Probabilistic robotics”, The MIT Press, 2005 ”can correct the position information of the autonomous mobile body 10. By correcting the position in this way, The position information of the autonomous mobile body 10 can be specified accurately.

  Then, the control apparatus 50 performs the process from step S2 again. In step S2, a route is calculated based on the corrected accurate position information. In this way, calculation of position information (step S4), correction of position information based on the detection value of the laser range finder 30 (steps S6 to S16), and control based on the corrected position information (step S2) are repeatedly performed. Thus, the autonomous mobile body 10 can accurately move to the target value.

  As described above, in the autonomous mobile body 10 of the embodiment, not only the detection error of the laser range finder 30 but also the error of the position information of the autonomous mobile body 10 is taken into consideration, and the line segment corresponding to the target line segment. Is identified from the environment map. For this reason, an incorrect line segment is prevented from being associated with the target line segment. Thereby, position information can be corrected more accurately.

  Further, in the autonomous mobile body 10 described above, when calculating the error of the relative position shift between the target line segment and the corresponding line segment in step S14, only the error of the start point and end point position of the target line segment is calculated. Considering. In this way, the relative position deviation error is not considered by considering only the detection errors of the positions of both end points of the target line segment, instead of considering all the detection errors of each coordinate point detected by the laser range finder 30. It is possible to reduce the amount of calculation required to calculate Thereby, the flowchart of FIG. 6 can be executed in a shorter cycle, and the autonomous mobile body 10 can be moved more accurately.

  In the autonomous mobile body 10 described above, only the existence probabilities of both end points of the target line segment are considered when the line segment corresponding to the target line segment is specified in step S10. As described above, the line segment corresponding to the target line segment is not considered by considering only the existence probabilities of both end points of the target line segment, instead of considering all the existence probabilities of each coordinate point detected by the laser range finder 30. The amount of calculation required for the specific processing can be reduced. Thereby, the flowchart of FIG. 6 can be executed in a shorter cycle, and the autonomous mobile body 10 can be moved more accurately.

  In addition, although the autonomous mobile body 10 was demonstrated in the Example mentioned above, you may apply this invention to the mobile body which does not move autonomously. For example, when the mobile body is remotely controlled, the position of the mobile body may be specified by the present invention.

  In the above-described embodiment, there is no other moving body in the moving area, but another moving body may exist in the moving area. In this case, the surface shape of the wall and the surface shape of another moving body are detected by the laser range finder. For this reason, a coordinate point group corresponding to the surface shape of the wall is specified from the coordinate point group detected by the laser range finder, and the above-described processing may be executed on the coordinate point group.

  In the above-described embodiment, the position and angle error of the autonomous mobile body was calculated by the technique described in the document “S. Thrun, W. burgard, D. Fox,“ Probabilistic robotics ”, The MIT Press, 2005”. However, various methods can be used to calculate the position and angle errors. For example, the error of the position and angle of the autonomous mobile body can be calculated in consideration of the motion state of the autonomous mobile body. For example, when the position of the autonomous mobile body is detected by a GPS sensor, if the speed of the autonomous mobile body is large in the x direction and small in the y direction, the detection error of the GPS sensor is large and the error in the x direction is large. Is calculated so as to reduce the error. By considering the motion state of the autonomous mobile body, the position and angle errors of the autonomous mobile body can be appropriately calculated.

  Further, in the above-described embodiment, there is one line segment obtained from the detection result of the laser range finder 30, and the line segment corresponding to this line segment is specified from the plurality of line segments of the environment map. However, there may be a plurality of line segments obtained from the detection result of the laser range finder 30. In addition, only a part of the line segments in the environment map may be used as a reference (that is, a landmark) for correcting the self position / angle. For example, consider a case where two line segments 130 and 132 are obtained from the detection result of the laser range finder 30 as shown in FIG. 12, and only the line segment 100x is used as a reference for correcting the self-position and angle. For example, when another obstacle is placed in front of the wall surface indicated by the line segment 100x, or when a door provided on the wall surface indicated by the line segment 100x is opened, one line as shown in FIG. There are cases where two line segments are detected at a position where a minute is to be detected. Also in this case, not only the detection error of the laser range finder 30 but also the error of the position information of the autonomous mobile body 10 is taken into consideration, and the line segment corresponding to the line segment 100x from the detected line segments 130 and 132 is considered. Can be specified. The corresponding line segment can be specified by the same calculation as in the above-described embodiment. In this way, not only when one line segment is detected and there are a plurality of line segments serving as landmarks, but also when a plurality of line segments are detected and one line segment serving as a landmark is present. In addition, the correspondence relationship of the line segment can be specified in consideration of the error. Also, when there are a plurality of detected line segments and a plurality of landmark line segments, the correspondence relationship between the line segments can be specified in consideration of similar errors.

Specific examples of the present invention have been described in detail above, but these are merely examples and do not limit the scope of the claims. The technology described in the claims includes various modifications and changes of the specific examples illustrated above.
The technical elements described in this specification or the drawings exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. In addition, the technology illustrated in the present specification or the drawings achieves a plurality of objects at the same time, and has technical utility by achieving one of the objects.

10: Autonomous moving body 20: Storage device 30: Laser range finder 40a: Servo motor 40b: Servo motor 42a: Servo control circuit 42b: Servo control circuit 44a: Motor 44b: Motor 46a: Encoder 46b: Encoder 50: Control device

Claims (4)

A moving object that moves on the floor where an obstacle exists,
A storage unit for storing a plurality of surface shape data representing a surface shape in a direction parallel to the floor surface of the obstacle extending linearly in a direction parallel to the floor surface by a global coordinate system;
A self-position / angle calculation unit that calculates the position and angle of the moving object using a global coordinate system;
An obstacle detection unit that scans the light beam and detects a surface shape in a direction parallel to the floor surface of the obstacle present on the scanning surface by a relative coordinate system with respect to the moving body;
A plurality of surface shapes stored in the storage unit based on the position and angle of the moving body calculated by the self-position / angle calculation unit and their errors, the surface shape detected by the obstacle detection unit, and the errors. A matching unit for identifying surface shape data corresponding to the surface shape detected by the obstacle detection unit from the data;
A self-position / angle correction unit that corrects the position and angle calculated by the self-position / angle calculation unit from the deviation between the surface shape data detected by the obstacle detection unit and the surface shape data specified by the matching unit,
A moving object comprising: The obstacle detection unit detects the surface shape in the direction parallel to the floor surface of the obstacle as a line segment in the relative coordinate system,
A deviation error calculation unit that calculates an error in deviation between the surface shape detected by the obstacle detection unit and the surface shape data specified by the matching unit based only on the position error of both ends of the detected line segment; Have
The self-position / angle correction unit corrects the position and angle of the moving body based on the error calculated by the deviation error calculation unit.
The moving body according to claim 1. The obstacle detection unit detects the surface shape in the direction parallel to the floor surface of the obstacle as a line segment in the relative coordinate system,
The matching unit calculates the probability that both ends of the line segment exist on the same surface shape data based on the error in the position of both ends of the line segment detected by the obstacle detection unit, and calculates the plurality of surface shape data. The moving body according to claim 1 or 2, wherein surface shape data having the highest probability of being detected is specified. A moving object that moves on the floor where an obstacle exists,
A storage unit for storing surface shape data representing, by a global coordinate system, a surface shape in a direction parallel to the floor surface of an obstacle extending linearly in a direction parallel to the floor surface;
A self-position / angle calculation unit that calculates the position and angle of the moving object using a global coordinate system;
An obstacle detection unit that scans the light beam and detects a surface shape in a direction parallel to the floor surface of the obstacle present on the scanning surface by a relative coordinate system with respect to the moving body;
When a plurality of surface shapes are detected by the obstacle detection unit, the position and angle of the moving object calculated by the self-position / angle calculation unit and their errors, and the plurality of surface shapes detected by the obstacle detection unit And a matching unit for identifying a surface shape corresponding to the surface shape data stored in the storage unit from the plurality of surface shapes detected by the obstacle detection unit based on the error, and
A self-position / angle correction unit that corrects the position and angle calculated by the self-position / angle calculation unit from the deviation between the surface shape specified by the matching unit and the surface shape data stored in the storage unit,
A moving object comprising:

Images