JP5763986B2 - MOBILE BODY AND METHOD FOR CONTROLLING MOBILE BODY - Google Patents

Description

  The present invention relates to a moving body that moves while recognizing its own position and orientation by measuring the surrounding environment, and a method for controlling the moving body.

  In order for a moving body to move efficiently in the environment, estimation of the position / posture in the environment (hereinafter, position / posture estimation) is indispensable. Patent documents 1 and patent documents 2 are mentioned as background art about a mobile object which performs position and orientation estimation of the mobile object itself. Japanese Patent Application Laid-Open No. 2004-133867 discloses a technique for measuring a plurality of reflectors installed in the environment to detect the inclination of a moving body due to road surface inclination or unevenness and accurately determine its own position. In Patent Document 2, sensor data obtained when actually measured by a sensor is predicted using an environment map and vehicle body movement information, and a position estimation error is detected by comparing the sensor data obtained by this prediction with the sensor data obtained by actual measurement. If it is determined that it has occurred, perform a turn on the spot and perform position estimation using sensor data and a map obtained when the sensor is not shut off after the turn. It describes the technology to restore.

JP 2001-74458 A JP 2010-61848 A

  The technology disclosed in Patent Document 1 is a method of providing a regular reflection light in which a laser beam emitted from a sensor of a moving body is reflected by a reflector and returned from a predetermined number of places where a known retroreflector and a regular reflector are provided. The tilt and position of the moving object are detected by detecting recursive reflection. However, it is difficult to install retrospective reflectors and regular reflectors in a normal environment in advance because of the time and effort required for preparation. In addition, the calculation for detection is complicated, and cost is also required from this point.

  The technique disclosed in Patent Document 2 measures the geometry of a general object in a normal environment with a sensor, compares (matches) the measurement result with a map stored in advance, and compares the position of the moving object on the map. The posture is estimated, and the moving body moves based on the estimation result. However, since the moving body is a moving mechanism such as a suspension, the sensor may tilt when the road surface is uneven, accelerated, decelerated, or shaken when stopped. If the sensor is tilted, the original measurement point (location) of the geometrical shape of the object in the environment may be missed, the map data and the measurement point of the object cannot be matched, and a measurement error occurs, making it difficult to estimate the position and orientation There is a fear. Patent Document 2 does not consider this point.

  In view of the problems of the prior art, the present invention provides a moving body and a moving body control method capable of reducing the influence of measurement errors due to sensor inclination and more accurately estimating the position and posture. Objective.

In order to solve the above-mentioned problem, the present invention measures the environment around the moving body with a distance sensor, and compares the measured distance data with map data stored in advance to determine the position of the moving body on the map data. In a moving object that moves to the destination based on the estimation result,
A distance sensor control unit for converting the distance data of the distance sensor into the geometric data of the environment;
A distance sensor error reducing unit that sets the size of one of the matching objects of the geometric shape data and the map data obtained by the distance sensor control unit according to the distance from the sensor of the measurement point;
A position / orientation estimation unit for estimating the position and orientation of a moving body by performing a matching process on geometric shape data and map data with a matching target size set by the distance sensor error reduction unit is provided.

  In the moving body described above, the distance sensor error reduction unit sets the size of the matching object to be larger as the distance from the sensor of the measurement point is longer.

  In the moving body described above, the distance sensor error reduction unit sets a size of a matching target for each measurement point forming the geometric shape data.

  Further, in the moving body described above, the distance sensor error reduction unit sets a size of a matching target of geometric shape data obtained by the distance sensor control unit.

  Further, in the moving body described above, the distance sensor error reduction unit sets the shape of the matching target of the geometric shape data obtained by the distance sensor control unit to an elliptical shape according to the sensor error. And

  Further, in the mobile body described above, the distance sensor error reduction unit sets a size of a matching target of the map data.

  In the moving object described above, the distance sensor error reduction unit sets a range of object existence pixels that are matching targets of the map data.

  In the moving body described above, the distance sensor error reduction unit removes measurement points that are considered to include a measurement error due to the floor surface of the environment around the moving body from the geometric shape data.

In order to solve the above-mentioned problem, the present invention measures the environment around the moving body with a distance sensor, and compares the measured distance data with map data stored in advance to determine the position of the moving body on the map data. In the control method of the moving object that moves to the destination based on the estimation result,
The distance sensor control unit converts the distance data of the distance sensor into the geometric data of the environment,
The distance sensor error reduction unit sets the size of one of the geometric shape data and the map data obtained above according to the distance from the sensor of the measurement point,
The position and orientation estimation unit estimates the position and orientation of the moving object by matching the geometric shape data and the map data with the size of the matching target set as described above.

  In the above-described method for controlling a moving body, the distance sensor error reduction unit sets the size of the matching target to be larger as the distance from the sensor of the measurement point is longer.

  Further, in the above-described method for controlling a moving body, the distance sensor error reduction unit sets a size of a matching target for each measurement point forming geometric shape data.

  In the control method for a moving body described above, the distance sensor error reduction unit sets a size of a matching target of geometric shape data obtained by the distance sensor control unit.

  Further, in the above-described moving body control method, the distance sensor error reduction unit sets the shape to be matched of the geometric shape data obtained by the distance sensor control unit to an elliptical shape according to the sensor error. It is characterized by that.

  Moreover, in the control method of the moving body described above, the distance sensor error reduction unit sets the size of the matching target of the map data.

  In the moving body control method described above, the distance sensor error reduction unit sets a range of object existing pixels that are targets of matching of the map data.

  In the method for controlling a moving body described above, the distance sensor error reduction unit removes from the geometric shape data measurement points that are considered to include measurement errors due to the floor surface of the environment around the moving body. And

  According to the present invention, it is possible to reduce the influence of measurement errors caused by the tilt of the sensor and perform more accurate position and orientation estimation.

It is a block diagram of the function of the Example of this invention. It is also a hardware / software configuration diagram. It is explanatory drawing which similarly measures the object in an environment with a distance sensor. It is an operation | movement flowchart of Example 1 of this invention. It is explanatory drawing which shows the example of a laser spot light. It is explanatory drawing which shows the example of the matching of the geometric shape data to map data. It is explanatory drawing which shows the example of the setting of the matching window size of Example 1 of this invention. It is explanatory drawing which shows the example of the matching of Example 2 of this invention. It is explanatory drawing which expanded a part of FIG. It is explanatory drawing which expanded a part of FIG. It is explanatory drawing which shows the modification of Example 1 of this invention.

  Embodiments of the present invention will be described below with reference to the drawings. A robot is used as a specific example of the moving body.

  In the above description, information, data, signals, and the like that are output, input, received, transmitted, referenced, stored, and set between the units are expressed differently depending on the description. It is not limited by the wording. For example, even if it is described as “value (setting value, detection value, correction value, output value, etc.)”, it is not limited only to data, but it may be treated and expressed as information or a signal with the same meaning . The same applies to the following description.

  In the present specification, “measurement” is described. However, the present invention is not limited to this “measurement”, and is generally described as “measurement”, and it is assumed that the same meaning is expressed or expressed as a configuration for performing “measurement”.

  Here, the configuration and main processing contents of the robot function assumed in this embodiment will be described first, followed by a more specific hardware / software configuration of the robot and the operation thereof.

  The configuration of the robot function is shown in FIG. The robot (moving body) 0101 has a controller unit 0102, a distance sensor unit 0103, and a moving mechanism unit 0109. The controller unit 0102 includes a distance sensor control unit 0104, a distance sensor error reduction unit 0105, a position and orientation estimation unit 0106, a route plan unit 0107, a movement mechanism control unit 0108, a distance sensor installation data 0110, a distance sensor spot light size data 0111, a map It consists of data 0112 and route data 0113. Each data is held in a storage device as will be described later. Further, although not shown here, it is assumed that the parts necessary for the respective parts to cooperate and operate, such as a case supporting each part and a power source / wiring, are provided.

  First, processing performed when the robot 0101 automatically travels is described. The robot 0101 controls the distance sensor unit 0103 by the distance sensor control unit 0104, and measures distance data including the distance and direction from the robot 0101 to an object in the surrounding environment. Here, a laser distance sensor is used as the distance sensor unit 0103.

  This laser distance sensor measures the distance from the sensor to the object in the environment by measuring the time from when the laser is irradiated to the object until the irradiated laser is reflected by the object and returns to the sensor. A laser irradiation unit (not shown) is provided. It is assumed that measurement (hereinafter referred to as scanning) of a distance to an object within a certain rotation angle range is possible by measuring the laser irradiation unit while rotating it at every constant rotation angle. By this scanning, a distance and direction to an object on a plane (hereinafter referred to as a scanning plane) formed by the laser by scanning can be obtained.

  These data sets are called distance data. Each of the distance data can be converted from distance and direction data into position data. The data obtained by converting the distance data into the position data in this way is referred to as geometric data. Further, the data of each position constituting the geometric shape data represents the position where the laser spot light reaches the object in the environment, that is, the position of the point, but this point is called a measurement point. Here, assuming that the scanning surface of the laser distance sensor is attached to the robot so as to be parallel to the floor surface, the geometric shape at the height of the scanning surface of the laser distance sensor is obtained as data. .

  As described above, the geometric shape data obtained from the distance data by the distance sensor control unit 0104 is sent to the distance sensor error reduction unit 0105. It is assumed that distance sensor error reduction unit 0105 has previously read distance sensor installation data 0110 and distance sensor spot light size data 0111.

  The distance sensor installation data 0110 is data indicating the position / posture of the distance sensor unit 0103 relative to the robot and the height from the floor. The distance sensor spot light size data 0111 is data indicating how spot light such as a laser emitted from the distance sensor toward an object in the environment spreads according to the distance. More specifically, here, it is assumed that the diameter of the spot light for each measurement distance from the sensor is recorded. Using these data, the distance sensor error reduction unit 0105 first removes the spot light cracking sensor data, and then, for each distance value forming the distance data, matching that becomes a matching determination criterion for position and orientation estimation described later. The window size is set.

  First, the removal of spot light crack sensor data will be described.

  The removal of the spot light crack sensor data is performed using the distance sensor installation data 0110 and the distance sensor spot light size data 0111 described above. More specifically, on the assumption that the sensor is not tilted, the distance at which the laser spot light emitted from the distance sensor unit 0103 spreads according to the distance and intersects the floor surface is obtained, and the geometric shape data is obtained. Of the many measurement points to be formed, the geometric shape data of the measurement point located farther than this distance is regarded as an erroneous measurement of the floor surface and is removed from the geometric shape data.

  Subsequently, a matching window size (matching target size) is set for each measurement point forming the geometric shape data. First, as the matching here, when the geometric shape data expressed as an image and the map data 0112 that can handle the geometric shape of the environment at the height of the scan plane in advance as an image are superimposed, A method is assumed in which the position / orientation of the geometric shape data on the map data when the pixels indicating the existence (hereinafter referred to as object existence pixels) are most overlapped is obtained by searching. Further, the matching window (matching target) refers to a frame provided around an object existing pixel (corresponding to a measurement point forming the geometric shape data) in the geometric shape data image. This matching window is used as a matching criterion when the geometric shape data and the map data are superimposed.

  Specifically, if an object existence pixel is included in the pixel of the map data in the range that overlaps the matching window provided at the measurement point forming the geometric shape data, the measurement point is regarded as matching with the map data. Used in a fussy manner. Therefore, the larger the window size, the larger the deviation is allowed when the measurement points forming the geometric shape data and the map data are superimposed. In the distance sensor error reduction unit 0105, the matching window size is set larger as the distance from the sensor of the measurement point forming the geometric shape data is larger (longer).

  When measurement is performed with the distance sensor unit 0103 tilted (the scan surface is not parallel to the floor surface), the measurement is performed with the sensor not tilted (the scan surface is parallel to the floor surface). A larger distance value can be obtained. Assume that map data is created so as to correspond to the scan plane of the sensor that is parallel to the floor and not tilted on the premise of matching of geometric shape data obtained in a state where the sensor is not tilted. In this situation, if the matching window size is constant regardless of the distance, even if the sensor is not tilted, even if it is a measurement point that overlaps the map data, it is difficult to overlap the map data when the sensor is tilted. No longer matches. This is because the larger the distance from the sensor of the measurement point, the greater the deviation of the measurement point, making it difficult to match.

  On the other hand, in the first embodiment, as described above, when the distance from the sensor of the measurement point forming the geometric shape data is set to be larger as the matching window size is larger, the measurement point that should originally overlap with the map data is set. However, even if it slightly deviates from the point that should originally overlap on the map data, it falls within the window size range, that is, the possibility of matching increases, and the matching error due to the tilt of the sensor is reduced. As described above, by setting the matching window size to be changed, the matching criterion is relaxed, and even if there is an error in measurement due to the inclination of the distance sensor, the matching error is reduced, that is, the position and orientation estimation error Can be reduced.

  The geometric shape data in which the size of the matching window is set according to the distance is sent to the position / orientation estimation unit 0106. In the position / orientation estimation unit 0106, the map data 0112 is read in advance, the geometric shape data and the map data 0112 are matched, and the geometric shape data when the ratio of the geometric shape data and the map data 0112 overlaps is the maximum. The position / posture is determined. The position and orientation of the starting point of the geometric shape data in the coordinate system defined on the map data 0112, that is, the position and orientation of the starting point of the distance sensor unit 0103 are obtained. When representing the position / orientation of the robot, any position of the robot casing may be used as a reference, and the position / orientation of the distance sensor unit 0103 thus obtained is used as the position / orientation of the robot 0101.

  The route planning unit 0107 reads in advance route graph data 0113 in which the length and direction of the path that the robot can pass is recorded, and the purpose is based on the current robot position / posture obtained by the position / posture estimation unit 0106. Find the route to the ground and the difference between the route and the current position.

  Then, the movement mechanism control unit 0108 controls the movement mechanism unit 0109 so as to reduce the deviation between the robot and the path to be followed. That is, an instruction is given to the motor or the like by obtaining the rotational speed of the wheel, the turning angle of the steering, or the like. As a result, it is possible to follow the robot's route and thus automatically travel to the destination. The above is the outline of the processing performed for each function constituting the robot.

  Next, the configuration of the hardware and software of the robot will be described first. Through the example of estimating the position and orientation using the geometric shape data obtained by measuring the robot with the robot's distance sensor tilted, The flow of the entire software process is described.

  FIG. 2 shows the configuration of the robot hardware and the software stored therein. A robot 0201 (corresponding to 0101) is a position / orientation estimation controller 0202, a laser distance sensor 0207 (corresponding to 0103), a moving mechanism 0208 (corresponding to 0109), a display 0209, an input device 0210, and these devices communicate with each other. A communication line 0205 is used. In FIG. 2, only elements directly related to the flow of processing are shown, and it is assumed that a power source and the like necessary for the operation of each element are provided.

  As the laser distance sensor 0207, a sensor having the same method as the laser distance described in the above-described distance sensor unit 0104 is used. Here, as an example, it is assumed that the angle range scanned by the laser distance sensor 0207 is 180 degrees, the laser is irradiated every 0.5 degrees in this angle range, and the distance to the object is measured. The angle range, the step size of the angle of laser irradiation, the maximum distance measurement range, and the like may be different.

  FIG. 3 shows how an object in the environment is measured by a laser distance sensor attached to the robot. This figure is a plan view showing the environment and the robot 0305 (corresponding to the robot 0201 in FIG. 2) as viewed from above. In addition, the wall surface of the object 0301 in the environment is perpendicular to the floor surface and has no unevenness. As an example, if the robot 0305 in the position / orientation in the figure scans an angle range 0309 of 180 degrees with a laser distance sensor 0306 (corresponding to the laser distance sensor 0207 in FIG. 2), geometric shape data 0308 (indicated by dots) Further, a depth camera or the like that can measure the distance to the object for each pixel by irradiating the object with infrared rays in a plane may be used.

  As the moving mechanism unit 0208, a robot 0305 shown in FIG. 3 is assumed. The robot 0305 is provided with a caster at the front and a drive wheel at the rear, and can move straight and turn by controlling the difference in rotational angular velocity of the drive wheel. In this embodiment, the use of such a moving mechanism is assumed, but the moving mechanism may be different as long as the effect of moving in the environment can be obtained. For example, other moving mechanisms such as a vehicle having an endless track, a robot having legs, a ship, an aircraft, an airship, and the like may be used. In this embodiment, the robot automatically travels. However, a person may board and operate the robot, or a person may communicate by remote communication without boarding. May be to steer.

  The position / orientation estimation controller 0202 includes a processor 0203, a memory 0204, a storage device 0206, an OS 0211, a controller initialization program 0212 for reading the BIOS and starting the OS, and a laser distance sensor control program for acquiring distance data from the laser distance sensor 0207. 0213: Removes invalid data from geometric shape data and sets matching window size. Laser distance sensor error reduction program 0214. Matches geometric shape data and map data 0220 to calculate position and orientation. Position / orientation estimation program 0215, route planning program 0216 for calculating a route to reach the destination based on route graph data 0221, and calculating the rotational speed of the wheel so that the vehicle body moves along the route. Composed of the moving mechanism control program 0217 to.

  Further, in the storage device 0206, the laser distance sensor installation data 0218 indicating the installation position / posture of the distance sensor with respect to the robot and the height from the floor, and the laser spot light emitted from the laser distance sensor spread according to the distance. Laser distance sensor spot light size data 0219 representing the diameter for each distance, map data 0220 used for matching by the position and orientation estimation program 0215, and route graph data 0221 representing the length and direction of the route through which the robot passes in the environment. Yes.

  It is assumed that the program and data in FIG. 2 are loaded into the memory and then processed by the processor, but the implementation may be different as long as the same effect can be obtained. . For example, the above processing may be realized by programmable hardware such as FPGA (Field Programmable Grid Array) or CPLD (Complex Programmable Logic Device). Note that the program and data may be transferred from a storage medium such as a CD-ROM or downloaded from another device via a network.

  In addition, although it is assumed here that each device constituting the robot 0201 such as a processor, a storage device, and a moving mechanism communicates with each other via a wired communication line 0205, the device may be wireless, and communication may be performed. If possible, the controller 0202, display 0208, and input device 0210 devices may be physically remote. The above hardware and software may be selected according to the embodiment.

  Next, the flow of processing performed by the robot 0201 will be described. The flow of processing of the activated controller 0202 is shown in FIG.

  When the controller 0202 is activated (0401), the controller initialization program 0212 reads the OS 0211 and activates the programs 0212 to 0217 (0402). Next, a confirmation screen as to whether or not to finish setting the destination is displayed on the display 0209. When making the robot 0201 perform automatic traveling, the operator selects to set the destination by using the input device 0210 (0403). If the robot 0201 does not automatically run, end is selected. In this case, the program ends immediately (0404). Now, assuming that the operator selects a destination setting method in order to cause the robot to automatically run, the process proceeds to process 0405.

Subsequently, the operator confirms the destination on the list of transport destination candidates displayed on the display 0209, and sets the destination using the input device 0210 (0405).
Next, the laser distance sensor installation data 0218 is read by the laser distance sensor error reduction program 0214 (0406). As described above, the installation position / posture of the laser distance sensor 0207 with respect to the robot 0201 and the height from the floor surface are recorded in this data.

  Next, the laser distance sensor spot light size data 0219 is read by the laser distance sensor error reduction program 0214 (0407). As described above, this data records how the spot light such as a laser irradiated from the distance sensor toward the object in the environment spreads according to the distance. Specifically, the diameter of the spot light for each measurement distance from the sensor is recorded here.

  Next, map data 0220 is read by the position / orientation estimation program 0215 (0408). Here, the map data 0220 is image data, and it is assumed that the presence or absence of an object in the environment is recorded for each pixel.

  Next, the route graph data 0221 is read by the route planning program 0216 (0409). Here, it is assumed that the path graph data 0221 records the length and direction of the path through which the robot passes in the environment.

  Subsequently, distance data is acquired from the laser distance sensor 0207 by the laser distance sensor control program 0213 (0410). The position is obtained from the distance and the direction forming the distance data, and the presence / absence of the object is recorded in the pixel and converted into the geometric shape data as an image.

  The geometric shape data converted by the laser distance sensor control program 0213 is sent to the distance sensor error reduction program 0214. As described above, the laser distance sensor error reduction program 0214 reads the laser distance sensor installation data 0218 and the laser distance sensor spot light size data 0219. By using these data, the laser distance sensor error reduction program 0214 first removes the spot light crack sensor data (0411).

  Here, on the assumption that the laser distance sensor 0207 is not inclined, the distance when the laser spot light emitted from the laser distance sensor 0207 spreads according to the distance and intersects the floor surface (hereinafter referred to as the floor surface intersection distance). ) Of the measurement points forming the geometric shape data, the measurement points where the distance from the sensor to the measurement point is greater than the floor crossing distance are considered to be erroneously measured. Remove from shape data.

  Specifically, as the laser distance sensor installation data 0218, the installation position / posture of the laser distance sensor 0207 with respect to the robot 0201 and the installation height of the laser distance sensor 0207 from the floor surface are as shown in FIG. It is assumed that there is a record and that the laser spot light is recorded to spread as shown in FIG. 5 according to the distance. In this case, it can be seen that the laser spot light 0504 intersects the floor surface 0505 at a point 0506 in the drawing. The distance 0507 from the sensor to the point 0506 where the laser spot light 0504 and the floor surface 0505 intersect is the floor surface intersection distance.

  When the distance to the object 0508 in the environment is measured by the laser spot light 0504 in FIG. 5, the laser spot light 0504 intersects the floor surface 0505 as shown in FIG. It will be. For this reason, the data of this measurement point shall be removed.

  Subsequently, the matching window size is set to the geometric shape data for each measurement point (0412). In processing 0412, the larger the distance from the sensor of the measurement point forming the geometric shape data, the larger the size of the matching window (matching target) of the geometric shape data, so that the matching error in the subsequent processing 0414 Thus, accurate position / posture calculation (estimation) is performed.

  As a premise of the matching window here, the matching performed in the position and orientation estimation of the next process 0414 will be described first. Here, the matching is to obtain the position / posture of the geometric shape data on the map data 0220 when the geometrical shape data and the map data 0220 are overlapped with each other and the object existing pixels are most overlapped. FIG. 6 shows how the geometric shape data is matched with the map data 0220.

  The map data 0601 in FIG. 6 (corresponding to the map data 0220 in FIG. 2) corresponds to the environment in FIG. Here, processing for obtaining the position / posture of the robot on the coordinate system 0608 of the map data will be described. All the position / postures that the geometric shape data 0602 obtained when the robot measures at the position / posture of FIG. 6 can be taken within a certain area by the dotted line in the figure centering on the previous position / posture estimation result 0606. Thus, the map data 0601 is overlaid and a process of calculating the ratio of overlapping object-existing pixels is performed. Accordingly, for example, in the position / posture 0607 of the start point of the geometric shape data 0603 (shifted upward), since the ratio of overlapping with the map data 0601 is low, the geometric shape is excluded from the solution candidates and has a higher overlapping ratio. The position / posture 0604 solution of the start point of the data 0602 is obtained. The position / posture of the geometric shape data on the obtained coordinate system 0608 is set as the position / posture of the robot.

  Here, a criterion for determining whether or not the above-described object-existing pixels are overlapped will be described with reference to FIG. 7 and FIG. 9 showing an enlarged part of FIG. Consider a case where the geometric data 0702 obtained by the measurement with the laser distance sensor 0720 (corresponding to the laser distance sensor 0207 in FIG. 2) being not tilted matches the map data 0701. Assume that the geometric shape data 0702 is superimposed on the map data 0701 as measured at the position / orientation 0703 as a candidate for the position / orientation solution at the time of search in matching. At this time, 0705 shows an enlarged view of a range 0704 in which the geometric shape data 0702 and the map data 0701 are overlapped.

  In the enlarged view 0705, a hatched pixel 0707 is a measurement point forming the geometric data 0702. That is, the hatched pixel 0707 is an object existing pixel in the geometric shape data 0702. A white pixel 0708 and a black pixel 0706 are portions forming the pixels of the map data 0701. The white pixel 0708 indicates that no object exists, and the black pixel 0706 indicates that an object exists. Yes. That is, the black pixel 0706 is the object existing pixel of the map data 0701.

  When the geometric shape data 0702 is in the position / posture of FIG. 7, for example, the object existence pixel 0707 at the measurement point overlaps with the object existence pixel 0706 of the map data 0701, so that it is determined that they match. The matching window size is a frame having a certain size around the object existence pixel (hatched portion) of the geometric shape data 0702. In the object existence pixel 0707 of the geometric shape data 0702, the matching window size is the same as that of the pixel of the measurement point 0707.

  If the measurement is performed with the laser distance sensor 0720 tilted, the measurement point with a longer distance is larger in difference from the distance when the sensor is not tilted than the measurement point with a short distance from the sensor. If geometric data is to be matched with map data without taking into account the inclination of the sensor, measurement points that have a long distance from the sensor, that is, objects that have a long distance from the sensor among the object-existing pixels in the geometric data. The existing pixels will not match the map data. Taking FIG. 7 as an example, an enlarged view 0705 around the object existence pixel 0707 in the geometric shape data 0702 is a measurement point portion 0704 having a short distance from the sensor 0720 even if the measurement is performed in a state where the sensor is tilted. Therefore, since the difference in distance from the measurement point when not inclined is small, the object existence pixel 0707 in the geometric shape data 0702 matches the object existence pixel 0706 in the map data 0701 even if the matching window size is small.

  On the other hand, when measurement is performed with the sensor tilted, the difference between the distance from the measurement point when the sensor is not tilted increases as the measurement point is farther from the sensor. The object existence pixel 0707 in the geometric shape data 0702 does not match the object existence pixel 0706 in the map data 0701.

  For example, a case where the measurement point is far from the sensor 0720 will be described. When the sensor is not tilted, in the range 0711 where the geometric shape data 0702 and the map data 0701 overlap, when the sensor is not tilted, the geometric shape data 0702 matches the map data 0701 as in 0710. However, when the sensor is tilted, the geometric shape data 0702 is larger than the horizontal distance from the actual sensor to the object (when the sensor is not tilted), as in 0709. Therefore, it matches the map data 0701. Disappear. An enlarged view of this non-matching portion is shown in 0715. In 0715, the matching window size is set to be small by 1 × 1 in pixel units. It can be seen that the object existence pixel 0713 (hatched portion) of the geometric shape data 0702 does not overlap with the object existence pixel 0714 (black pixel) of the map data 0701 and does not match.

  This matching error is dealt with by expanding the matching window size. Specifically, the size of the matching window provided around the object existence pixel 0719 (hatched portion) of the geometric shape data 0702 in the enlarged portion 0716 is set to 3 × 3 as indicated by a broken line 0717. If the object existence pixel 0719 of the map data 0701 is within the range of the matching window 0717 having a large size, it is determined that the match has occurred.

  Thereby, the deviation of the geometric shape data from the map data due to the inclination of the sensor is allowed, and the matching error is reduced. As described above, in the process 0412, the laser distance sensor error reduction program 0214 performs a process of setting the size of the matching window of the geometric shape data to be larger as the distance from the sensor of the measurement point forming the geometric shape data is larger. For example, as the distance from the sensor at the measurement point increases, the size is set as (1 × 1) → (2 × 2) → (3 × 3). The size of the matching window may be simply proportional to the distance from the sensor of the measurement point forming the geometric shape data. If the threshold of the distance is set in advance and the threshold of the distance is exceeded, the matching window The size may be constant. Thereby, a matching error due to an excessively large matching window size may be reduced.

  In addition, the shape of the matching window is simply a square having the same aspect ratio here, but the shape is not limited. For example, it may be a circle or a rectangle. In addition, characteristics of measurement errors by the laser distance sensor may be taken into consideration. For example, as shown in FIG. 11, an ellipse 1103 is provided as a matching window along a straight line 1101 that connects a measurement point that forms geometric shape data from the starting point of the laser spot light of the laser distance sensor and that represents the error range of the measurement point 1104. The size 1106 of the ellipse axis 1102 is set based on the measurement error in the depth direction of the laser distance sensor, and the ellipse is larger than the measurement error due to the control of the spread of the laser spot light and the direction in which the laser spot light is projected. A size 1105 in the direction of the axis 1107 may be set to form a matching window. Of course, it may be a quadrangle along a straight line 1101 that connects the starting point of the laser spot light of the laser distance sensor to the measurement point forming the geometric shape data, instead of an ellipse.

  As described above, the matching error reduction process is performed in the processes 0411 and 0412 by the laser distance sensor error reduction program 0214.

  Subsequently, the position / orientation estimation program 0215 estimates the position / orientation based on the matching between the geometric shape data and the map data 0220 (0414). Here, as described above, the geometric shape data represented in the image shape and the map data 0220 similarly represented in the image shape are superimposed, and the object existence pixel in the geometric shape data is the object existence pixel on the map data 0220. The position / posture of the geometric shape data on the map data 0220, that is, the position / posture of the laser distance sensor 0207 when the ratio of overlapping with the maximum is obtained. Here, the position / posture of the laser distance sensor 0207 is assumed to be the position / posture of the robot 0201. As a matching method, the matching method may be different as long as the effect of obtaining the position / posture of the geometric shape data with respect to the map data can be obtained.

  Next, it is determined whether or not the robot has arrived at the destination (0415). This is performed by determining whether or not the position obtained by the above-described position and orientation estimation is within a certain distance from the coordinates of the destination. When the distance between the position obtained by the position and orientation estimation and the destination is equal to or smaller than the threshold value, it is determined that the destination has arrived, and the process returns to process 0405 to wait for an instruction until automatic travel to the next destination. If the distance between the position obtained by the position and orientation estimation and the destination is larger than the threshold, it is determined that the destination has not arrived, and the process proceeds to process 0417.

  If the robot has not yet arrived at the destination, the route planning program 0216 obtains a movement route to the destination based on the current position / posture. In the environment of FIG. 3, if 0307 is set as the destination of the robot 0201 at the movement start position 0302, the route planning program 0216 performs route search using the route graph data 0221 read in advance at the start, The target travel route 0304 is obtained (0417).

  Subsequently, the moving mechanism control program 0217 calculates the rotational speed of the wheel for moving the vehicle body along the route obtained by the route plan, and the moving mechanism 0208 is configured so that the rotational speed of the wheel becomes a predetermined value. An instruction of a current value for the motor is issued (0416).

  If it is determined that the robot has moved and has arrived at the destination (0415), the process returns to the destination setting confirmation process 0403 as described above. The above is the flow of processing when the robot is performing normal operation.

  In the first embodiment, in the process 0413, the size of the matching window (matching target) to the geometric shape data is set larger as the distance from the sensor of the measurement point forming the geometric shape data is larger in the process 0413. This paper describes a method that suppresses matching errors and reduces the effect of sensor tilt on position and orientation estimation. Here, another method for obtaining the same effect will be described.

  In the second embodiment, the matching window size (the size of the matching target) to the geometric shape data is not increased as in the first embodiment. Instead, the range (size) of the object existence pixel that is the matching target of the map data. ). In other words, by reducing the resolution and enlarging the object existence pixels (expansion processing or thickening processing), even if the geometric shape data has shifted measurement points, it is easy to match the object existence pixels in the map data, thereby matching errors. Is reduced. More specifically, a plurality of map data having different resolutions are prepared, and the larger the distance from the sensor of the measurement point forming the geometric shape data, the lower resolution (large) map data is selected and selected. Matching between map data and geometric data is performed.

  FIG. 8 and FIG. 10 showing an enlarged part thereof show how a matching error is prevented using map data having different resolutions. If the sensor is not tilted, the geometric shape data matches the map data as 0801. However, when the sensor is tilted, it is shifted from the map data as 0802. This is the description of the first embodiment, as described with reference to FIG. Here, 0804 shows how the shifted 0802 geometric data matches low-resolution map data.

  In 0804, a white pixel 0806 is a pixel where no object of map data exists, a black pixel 0807 is an object existence pixel of map data, and a hatched pixel 0805 is an object existence pixel of geometric shape data. The object-existing pixel in the original map data was a line having a width of 1 pixel as indicated by 0719 in FIG. 7, but here the map data is changed to a line having a width of 3 pixels by reducing the resolution. By this pixel change, even if the window size of the geometric shape data is one pixel, the object existence pixel 0805 overlaps with the object existence pixel 0807 of the map data, and it is determined that there is a match.

  If the above processing is used, the second embodiment can reduce the influence on the matching of the tilt of the sensor as in the first embodiment. As the operation flow of the second embodiment, the processes 0408, 0412, and 0414 in FIG. 4 may be changed.

  First, in process 0408, a plurality of map data having different resolutions are read. For map data with different resolutions (different sizes of matching targets), the actual number of pixels may be reduced, or the actual resolution may be reduced by expanding the area where the object-existing pixels remain with the number of pixels unchanged. Also good. The process 0412 is omitted from the flow of FIG. 4, and the process of the process 0414 is changed. That is, in the modified process 0414, for matching a measurement point having a large distance from the sensor, matching is performed after low-resolution map data is selected from a plurality of map data. As described above, in the second embodiment, the matching error reduction process is performed in the modified process 0414 by the laser distance sensor error reduction program 0214.

  In the first embodiment, a matching error that may occur due to the tilt of the sensor is dealt with by changing the matching window size (the size of the matching target) of the geometric shape data. In the second embodiment, the map data is matched by reducing the resolution. Correspond by changing the size.

  Both embodiments have the same effect, but in the first embodiment, the window size changes each time matching is performed, and it is necessary to determine whether or not the window size matches. The more measurement points that are far from the sensor are included in the geometric shape data, the larger the matching determination is performed with a large window size, which increases the amount of calculation. However, a plurality of map data is prepared as in the second embodiment. Since there is no need to do this, the amount of memory used is small.

  On the other hand, in the second embodiment, if map data with different resolutions is generated each time matching is performed, the amount of calculation increases. However, if a plurality of map data with different resolutions are prepared in advance, the amount of calculation is reduced. , Processing speed can be increased.

  Note that the size of the matching target of both the geometric shape data and the map data may be changed. However, since the size setting control is complicated, it is preferable to perform only one.

  0101, 0201 ... Robot, moving object, 0102 ... Controller unit, 0103 ... Distance sensor unit, 0104 ... Distance sensor control unit, 0105 ... Distance sensor error reduction unit, 0106 ... Position and orientation estimation unit, 0107 ... Path planning unit, 0108 ... Moving mechanism control unit, 0109 ... Moving mechanism control unit, 0110 ... Distance sensor installation data, 0111 ... Distance sensor spot light size data, 0112 ... Map data, 0113 ... Route graph data, 0717, 0807 ... Matching target.

Claims (6)

The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the moving body that moves to the destination
A distance sensor control unit for converting the distance data of the distance sensor into the geometric data of the environment;
In accordance with the distance from the range sensor measurement points, to set the size of one of the matching target of the map data and the geometry data obtained by the distance sensor control unit, from the distance sensor of the measuring points A distance sensor error reduction unit that sets a larger size of the matching target as the distance is longer ;
Characterized by comprising a position and orientation estimation unit for estimating the position and orientation of the moving body the geometry data and the map data by matching the size of said distance sensor matching target set by the error reducing unit Moving body. The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the moving body that moves to the destination
A distance sensor control unit for converting the distance data of the distance sensor into the geometric data of the environment;
According to the distance of the measurement point from the distance sensor, the size of one of the matching objects of the geometric shape data and the map data obtained by the distance sensor control unit is set, and obtained by the distance sensor control unit A distance sensor error reducing unit that sets a shape to be matched of the geometric shape data to an elliptical shape in accordance with an error of the distance sensor;
A position / orientation estimation unit that estimates the position and orientation of the moving body by performing a matching process on the geometric shape data and the map data with a matching target size set by the distance sensor error reduction unit. Moving body. The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the moving body that moves to the destination
A distance sensor control unit for converting the distance data of the distance sensor into the geometric data of the environment;
According to the distance of the measurement point from the distance sensor, the size of one of the geometric shape data and the map data obtained by the distance sensor control unit is set, and the environment floor around the moving body is set. A distance sensor error reducing unit that removes measurement points considered to include measurement errors due to a surface from the geometric shape data;
A position / orientation estimation unit that estimates the position and orientation of the moving body by performing a matching process on the geometric shape data and the map data with a matching target size set by the distance sensor error reduction unit. Moving body. The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the control method of the moving body that moves to the destination,
The distance sensor control unit converts the distance data of the distance sensor into the geometric data of the environment,
The size of one of the geometric shape data and the map data obtained by the distance sensor error reduction unit is set according to the distance from the distance sensor of the measurement point, and the distance of the measurement point from the distance sensor The longer the is, the larger the size of the matching target is set,
A method for controlling a moving body, wherein the position and orientation of the moving body is estimated by performing a matching process on the geometric shape data and the map data with the size of the matching target set as described above by a position and orientation estimation unit. The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the control method of the moving body that moves to the destination,
The distance sensor control unit converts the distance data of the distance sensor into the geometric data of the environment,
The size of one of the geometric shape data and the map data obtained by the distance sensor error reduction unit is set according to the distance from the distance sensor of the measurement point, and the distance sensor control unit obtains the size Set the shape to be matched in the geometric data to an elliptical shape according to the error of the distance sensor,
A method for controlling a moving body, wherein the position and orientation of the moving body is estimated by performing a matching process on the geometric shape data and the map data with the size of the matching target set as described above by a position and orientation estimation unit. The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the control method of the moving body that moves to the destination,
The distance sensor control unit converts the distance data of the distance sensor into the geometric data of the environment,
The size of one of the geometric shape data and the map data obtained by the distance sensor error reduction unit is set according to the distance from the distance sensor of the measurement point, and the floor surface of the environment around the moving body The measurement points that are considered to contain measurement errors due to are removed from the geometric data,
A method for controlling a moving body, wherein the position and orientation of the moving body is estimated by performing a matching process on the geometric shape data and the map data with the size of the matching target set as described above by a position and orientation estimation unit.

Images