JP2013072813A - Level difference part recognition device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a device capable of accurately recognizing an arrangement position for a level difference part such as a stair including a spatial position and a direction of a front side edge of a tread of the level difference part by utilizing stereo images acquired by a stereo camera.SOLUTION: A plurality of lines of arithmetic processing areas R3 min(k2) are set to a reference image of cameras 3R and 3L, a plurality of candidate positions v(k1) of an actual edge projection line L52r in each of the plurality of lines of arithmetic processing areas R3 min(k2) are set, and in each arithmetic processing area, a value of an evaluation function indicating the adaptation to a position of the actual edge projection line (a projection line of the front side edge of the level difference part) of each candidate position is calculated utilizing projective transformation. An estimated edge projection line obtained by estimating the actual edge projection line is determined such that composite adaptation obtained by compositing the adaptations of the positions of the estimated edge projection lines in each of the plurality of lines of arithmetic processing areas R3 min(k2) becomes the highest. On the basis of the estimated edge projection line and a plane parameter of the tread of the level difference part, the spatial arrangement of the level difference part is recognized.

Description

  The present invention relates to an apparatus for recognizing a spatial arrangement position of a stepped portion such as a stair existing in a moving environment of a moving body using a stereo image acquired by a stereo camera mounted on the moving body.

  Conventionally, for example, when a robot that can move autonomously moves up and down stairs, a technique found in, for example, Patent Document 1 has been proposed as a technique for causing the robot to recognize the stairs.

  In this technology, distance data of the outside world of the robot is acquired by a stereo vision system mounted on the robot, and a plane portion corresponding to the tread surface of the stairs is detected based on the distance data. Then, a polygon including the detected plane portion is obtained by Sklansky's algorithm or Melkman's algorithm, and the front edge, back edge, width, and length of the stairs are specified from the obtained polygon.

International Publication No. WO2005 / 087452

  By the way, when a legged mobile robot such as a two-legged mobile robot performs a raising / lowering operation of a stepped portion such as a staircase, the foot portion of the leg of the robot is set to the tip side edge of the stepped surface of the stepped portion (referred to in Patent Document 1). It may be necessary or preferable to place a part of the foot part over the step surface of the step part.

  For example, if the depth dimension of the tread surface of a staircase is narrower than the size of the foot portion of the robot, it may not be possible to place the entire foot portion on the tread surface. Alternatively, even if it is possible to place the entire foot portion of the robot on the tread surface, in order to avoid the foot portion from interfering with the kicking of the stairs and the like when swinging out the foot portion, In some cases, it is preferable that a part of the foot of the robot protrudes from the tread.

  In such a case, in order to appropriately determine the target landing position of the foot portion of the robot, it is necessary to accurately recognize the position and direction of the leading edge of the stepped portion such as the staircase. .

  However, in the conventional technique as seen in Patent Document 1, the leading edge of the tread surface of a stepped portion such as a staircase cannot often be detected.

  That is, the distance data recognized from the stereo image obtained by the stereo vision system is easily affected by the texture of the captured image, etc., so that reliable distance data near the front edge of the tread of the stepped portion is obtained. There are many cases where this is not possible. In such a case, the vicinity of the tip side edge of the stepped surface of the stepped portion cannot be recognized as a plane portion or is excluded from the plane portion. As a result, the polygon has a different boundary shape from the leading edge of the stepped tread surface, and the front edge recognized from the polygon is an error from the actual leading edge of the stepped tread. Tends to be big.

  Therefore, the conventional technique as seen in Patent Document 1 often cannot detect the leading edge of the tread surface of a stepped portion such as a staircase.

  The present invention has been made in view of such a background, and uses a stereo image acquired by a stereo camera, the step including the spatial position and direction of the tip side edge of the step surface of a step such as a staircase. It is an object of the present invention to provide an apparatus that can accurately recognize the position of a part.

  Here, the terms in the present invention will be supplemented. In the present invention, the “step portion” has one or more sets (one set or a plurality of sets) of a lower step surface and an upper step surface one step above. In this case, each “tread surface” of the “step portion” is a surface that can be regarded as a flat surface included in a certain virtual plane (infinite plane). The “step portion” in the present invention includes a staircase as one form thereof, but does not mean only a staircase. For example, the step part comprised by the step installed on a floor surface or arbitrary installation things is also contained.

  In addition, the `` tip edge '' of the upper step surface of the step portion means that the outer shape of the upper step surface is viewed when the step portion is viewed from the line of sight where the lower step surface is the front side and the upper step surface is the back side. Among the boundary lines, it means a boundary line extending linearly in the lateral direction (horizontal direction or a direction close thereto) on the front side.

  In other words, the “tip edge” of the upper step surface of the step portion means that when the step portion is viewed from the line of sight where the upper step surface is the front side and the lower step surface is the back side, It means a boundary line extending linearly in the lateral direction on the back side among the outer boundary lines.

  Based on the above, the present invention will be described below.

In order to achieve the above object, the step recognition apparatus of the present invention exists in the moving environment of a moving body equipped with a stereo camera, and one set of a lower step side tread and an upper step side tread that is one step above. A stepped portion recognition device for recognizing a spatial arrangement position of stepped portions having more than one set by using a standard image and a reference image constituting a stereo image acquired by the stereo camera,
In a state where the stepped portion is imaged by the stereo camera, different plane portions including at least two plane portions respectively corresponding to the upper step surface and the lower step surface of the step portion are projected from the reference image. A plane projection area extracting means for extracting a plurality of plane projection areas, which are areas, and determining plane parameters representing a spatial position and orientation of a plane including a plane portion corresponding to each plane projection area;
Each of which includes an actual edge projection line formed by projecting the leading edge of the upper tread surface of the stepped portion onto the reference image, and has a shape that is elongated in the vertical direction of the reference image in the horizontal direction of the reference image. A plurality of parallel arithmetic processing areas are set in the reference image, and a plurality of candidate positions of the actual edge projection line in the vertical direction of the reference image in each of the multiple arithmetic processing areas are set, Evaluation function calculation that calculates the value of the evaluation function representing the degree of suitability of each candidate position with respect to the position of the actual edge projection line for each arithmetic processing region using the reference image and the reference image and the determined plane parameter Means,
The combined adaptability obtained by combining the estimated edge projection line, which is a line obtained by estimating the actual edge projected line, with the adaptability of the position of the estimated edge projected line in each of the plurality of arithmetic processing regions is the highest. An estimated edge projection line determination means for determining based on the calculated evaluation function,
As data for recognizing the spatial arrangement position of the stepped portion, at least data representing the spatial position and direction of the tip side edge of the upper tread of the stepped portion is used as the determined estimated edge projection line and the A stepped portion space arrangement determining means for determining based on the determined plane parameter;
The evaluation function calculating means calculates each candidate position in each calculation processing area in order to calculate a value of an evaluation function representing the degree of suitability of each candidate position with respect to the position of the actual edge projection line for each calculation processing area. A process for setting a plurality of projection conversion target areas for edge estimation including at least two projection conversion target areas adjacent as boundaries according to each candidate position, and a plane parameter for projection conversion corresponding to each projection conversion target area for edge estimation Processing according to the plane parameters determined by the plane projection area extraction means, and for each set of calculation processing areas and each candidate position, pixel value distribution of the image of each edge estimation projective transformation target area, The image of the region in the reference image corresponding to the projection conversion target region for edge estimation is changed in accordance with the plane parameter for projection conversion corresponding to the projection conversion target region for edge estimation. A process of calculating an error function value representing an error from the pixel value distribution of the projective transformation image obtained in the case, and a plurality of sets set in each calculation processing area for each set of each calculation processing area and each candidate position A process of calculating, as a value of the evaluation function, a value obtained by linearly combining the error function values calculated corresponding to each of the projection conversion target regions for edge estimation (first) invention).

  According to the first aspect of the invention, the process of the planar projection area extraction unit is executed in a state where the step portion is imaged by the stereo camera. In this process, a plurality of planar projection areas including at least two planar portions respectively corresponding to the upper step surface and the lower step surface of the step portion are extracted from the reference image. Furthermore, plane parameters representing the spatial position and orientation of the plane including the plane portion corresponding to each plane projection area are determined.

  Supplementally, the processing of the planar projection area extracting means in the first invention may use any method as long as it can perform the extraction of the planar projection area and the determination of the plane parameters corresponding thereto. . The method may be a known method. For example, using a known distance measuring device such as a laser distance measuring device mounted on a moving body, the distance (distance from the moving body) of each stepped portion is measured, and based on the measurement data, the plane projection area You may make it perform extraction and determination of the plane parameter corresponding to it.

  Alternatively, it is also possible to extract a plane projection region and determine a plane parameter corresponding to it by a method using projection transformation (planar projection transformation) described later.

  Further, each plane projection area extracted by the plane projection area extraction unit does not have to be the entire projection area of the corresponding plane portion (such as each tread of the step portion), and is a projection area of a part of the plane portion. There may be. For example, the plane projection area extracted corresponding to the upper step surface of the stepped portion does not need to include a line obtained by projecting the leading edge of the upper step surface on the reference image.

  Further, the planar projection area extracted by the planar projection area extracting means is not limited to the planar projection area corresponding to each tread of the step portion, but for example, there is a planar projection area corresponding to the stair kicking surface or the stair nosing tip surface. It may be included. However, you may make it exclude the planar projection area | region corresponding to planar parts other than each tread of a level | step difference part from extraction object. In that case, for example, based on the direction of the normal direction of the planar portion, it is possible to exclude the planar projection region corresponding to the planar portion other than each tread of the stepped portion from the extraction target.

  In the first invention, next, the process of the evaluation function calculation means is executed. In this process, the evaluation function calculation means includes an actual edge projection line obtained by projecting the leading edge of the upper tread surface of the stepped portion onto the reference image, and has an elongated shape in the vertical direction of the reference image. A plurality of arithmetic processing areas that are arranged in parallel in the horizontal direction of the reference image are set in the reference image.

  In this case, since the actual edge projection line exists between the planar projection area corresponding to the upper step surface of the stepped portion and the planar projection area corresponding to the lower step surface, the actual edge projection line is respectively set inside. It is possible to set a plurality of calculation processing areas including the number based on the plane projection area and plane parameters obtained by the process of the plane projection area extracting means. Each calculation processing area has an elongated shape in the vertical direction of the reference image, and thus is an area in which the width in the horizontal direction of the reference image is very small (for example, one pixel width).

  Further, the evaluation function calculating means sets a plurality of candidate positions of the actual edge projection line in the vertical direction of the reference image in each of a plurality of calculation processing areas, and sets each candidate position for each calculation processing area. A value of the evaluation function representing the degree of conformity with respect to the position of the actual edge projection line is calculated using the standard image and the reference image and the determined plane parameter.

  This evaluation function is calculated as follows. In other words, the evaluation function calculation means sets a plurality of edge estimation projection transformation target regions including at least two projection transformation target regions adjacent to each other with each candidate position as a boundary in each computation processing region according to each candidate position.

  In this case, assuming that any one candidate position matches the position of the actual edge projection line in each calculation processing region, two edge estimation projections set adjacent to each other with the candidate position as a boundary Of the conversion target area, the area above the candidate position corresponds to the projection area of the upper tread surface of the stepped portion. The lower region of the candidate position corresponds to a projection region of a surface portion (a surface portion corresponding to a stair nosing or kicking surface) that rises downward from the tip side edge of the upper step surface of the step portion.

  Further, the evaluation function calculation means sets the projection conversion plane parameters corresponding to each of the edge estimation projection conversion target areas in accordance with the plane parameters determined by the plane projection area extraction means. For example, the projection conversion plane parameter corresponding to the area above the candidate position, out of the two edge estimation projection conversion target areas set adjacently with each candidate position as a boundary, is displayed on the upper tread of the step portion. Correspondingly, the plane parameter determined by the plane projection area extracting means coincides with the plane parameter.

  The projection transformation plane parameter corresponding to the lower region of the candidate position among the two edge estimation projection transformation target regions set adjacently with each candidate position as a boundary is, for example, the upper part of the stepped portion. It corresponds to the plane parameter determined by the plane projection area extracting means corresponding to the surface standing between the side tread surface and the lower side tread surface.

  Alternatively, the projective transformation plane parameter rises at a predetermined angle (for example, 90 degrees or an angle close thereto) with respect to the upper step surface of the stepped portion, for example, and the virtual tip side edge corresponding to the candidate position The position and orientation of a plane including a straight line (virtual front end edge such that the position of the line on which the front end edge is projected onto the reference image (the vertical position of the reference image) matches the candidate position) It corresponds to the plane parameter to be expressed (this can be calculated from the plane parameter determined by the plane projection area extraction unit corresponding to the upper tread surface and the candidate position).

  Further, the evaluation function calculation means, for each set of the calculation processing area and each candidate position, the pixel value distribution of the image of each edge estimation projection conversion target area and the reference corresponding to the edge estimation projection conversion target area A value of an error function representing an error from the pixel value distribution of the projective transformation image obtained when the image of the region in the image is subject to projective transformation according to the plane parameter for projective transformation corresponding to the projective transformation target region for edge estimation. calculate.

  Note that the region in the reference image corresponding to the edge estimation projective transformation target region means that the portion of the real space projected on the edge estimation projective transformation target region of the standard image is the edge estimation projective transformation target region. It means an area obtained when a portion on a plane defined by the corresponding projective transformation plane parameter is projected on the reference image.

  In this case, if a certain candidate position matches the position of the actual edge projection line in each calculation processing area, the pixel value distribution of the image of the projection conversion target area for each edge estimation and the pixel value distribution of the projection conversion image And match (or nearly match) each other. On the other hand, when the candidate position does not coincide with the position of the actual edge projection line, the pixel value distribution of the image for each edge estimation projective transformation target area and the pixel value distribution of the projective transformation image are different from each other. .

  Then, the evaluation function calculating means calculates the error function calculated for each of the plurality of edge estimation projective transformation target regions set in each arithmetic processing region for each arithmetic processing region and each candidate position set. A value obtained by linearly combining the values is calculated as the value of the evaluation function.

  As a result, for each calculation processing region that is elongated in the vertical direction of the reference image (the width in the horizontal direction of the reference image is small), the candidate position of the actual edge projection line in the vertical direction of the reference image and the candidate position Data representing the correlation with the degree of conformity to the position of the actual edge projection line is obtained. In other words, this data indicates which position in the vertical direction of the reference image is appropriate as the position of the actual edge projection line (having a high degree of matching) in each arithmetic processing region.

  Next, the process of the estimated edge projection line determination unit is executed. In this processing, the estimated edge projection line determination means adapts the estimated edge projection line, which is a line obtained by estimating the actual edge projection line, to the position of the estimated edge projection line in each of the plurality of arithmetic processing regions. It is determined based on the calculated evaluation function (the value of the evaluation function calculated for each set of calculation processing regions and candidate positions) so that the combined fitness obtained by combining the degrees becomes the highest.

  In this case, the actual edge projection in the vertical direction of the reference image is performed for each calculation processing region by the value of the evaluation function calculated for each set of each processing processing region and each candidate position by the processing of the evaluation function calculating unit. Data representing the correlation between the candidate line position and the value of the evaluation function is obtained. Therefore, when a certain estimated edge projection line is assumed, the degree of suitability (validity) of the position of the estimated edge projection line in each calculation processing region (position in the vertical direction of the reference image) to the actual position of the actual edge projection line The degree of sex) will be understood.

  If the estimated edge projection line coincides with the actual edge projection line, the matching degree of the position of the estimated edge projection line in each calculation processing region is, in principle, the highest in the calculation processing region.

  Therefore, the estimated edge projection line determining means determines the estimated edge projection line so that the combined fitness obtained by combining the fitness of the positions of the estimated edge projection lines in each of the plurality of arithmetic processing regions is the highest. To do. As a result, an estimated edge projection line that has the highest degree of fitness for the actual edge projection line in each of the plurality of arithmetic processing regions, that is, an estimated edge that is highly valid as an estimation of the actual edge projection line. A projection line is determined.

  Note that the estimated edge projection line determined by the processing of the estimated edge projection line determination means does not need to have the highest combined fitness, and one of the position and direction in the estimated edge projection line and the reference image. Alternatively, it may be such that the composite fitness is relatively higher than a plurality of lines that are slightly different from each other. Accordingly, among the plurality of estimated edge projection line candidates whose positions and / or directions in the reference image are different from each other, the one having the highest combined fitness may be determined as the estimated edge projection line.

  Next, the processing of the stepped portion space arrangement determining means is executed. In this process, the step portion space arrangement determining means uses at least the space of the front edge of the stepped portion on the upper step as the data for recognizing the spatial arrangement position of the step portion existing in the imaging area of the stereo camera. Data representing a specific position and direction is determined based on the determined estimated edge projection line and the determined plane parameter.

  In this case, the leading edge of the upper step surface of the stepped portion is a line on a plane whose spatial position and orientation are defined by the plane parameters determined by the plane projection area extraction means corresponding to the upper step surface. A line obtained by optically projecting this line onto the reference image is the estimated edge projection line. Therefore, based on this estimated edge projection line and the plane parameter determined by the plane projection area extraction means (the plane parameter of the plane portion corresponding to the upper tread surface), the spatial position and direction of the tip side edge are represented. Data can be determined.

  As the data, for example, a spatial position of two points on the leading edge, or a set of a spatial position of one point on the leading edge and the direction vector of the leading edge is used. be able to.

  According to the first invention described above, the value of the evaluation function calculated for each set of each calculation processing region and each candidate position by the processing of the evaluation function calculation means is long and narrow in the vertical direction of the reference image. For each region, the correlation between the candidate position of the actual edge projection line in the vertical direction of the reference image and the degree of suitability of the candidate position with respect to the position of the actual edge projection line is represented.

  Therefore, by determining the estimated edge projection line based on the value of the evaluation function, the estimated edge projection line that has the highest possible fitness in each of the plurality of arithmetic processing regions arranged in the horizontal direction of the reference image is obtained. Can be determined. For this reason, it is possible to determine an estimated edge projection line having a high degree of coincidence with the actual edge projection line.

  Therefore, according to the first aspect, it is possible to accurately recognize the spatial position and direction of the leading edge of the tread surface of a stepped portion such as a staircase.

  In the first aspect of the invention, the plane projection area extracting means extracts the plane projection area and determines the plane parameters using the stereo image (standard image and reference image) without using a laser distance measuring device or the like. It is also possible to perform.

  Specifically, the planar projection area extracting means sets a plurality of planar projection area extraction local areas whose positions in the reference image are different from each other in the reference image, and each of the planar projection area extraction local areas A plane parameter for extracting a plane projection area, which is a plane parameter that defines projective transformation between an image and an image of an area in the reference image corresponding to the area for extracting a plane projection area, Pixel value distribution of image of region and pixel of image obtained when projective transformation is performed on region image in reference image corresponding to local region for plane projection region extraction according to plane parameter for plane projection region extraction Processing for determining to minimize an error from the value distribution, and for extracting a planar projection area corresponding to each of the planar projection area extracting local areas included in each planar projection area Plane defined by a surface parameter extracts each planar projection area from the reference image so as to match with each other (second invention).

  According to this second aspect, the planar projection area extracting means sets a plurality of planar projection area extracting local areas whose positions in the reference image are different from each other in the reference image. The local region for planar projection region extraction is a region having a sufficiently small area as compared with the entire reference image.

  Here, when an image of a certain plane projection region extraction local region is an image of a plane portion, the image and a portion of the real space projected on the plane projection region extraction local region of the reference image An image of a portion formed by projecting a local portion (here, a local portion) into a reference image is a projective transformation (a planar projective transformation) corresponding to a plane parameter representing a position and orientation of a plane including the plane portion. ).

  That is, in principle, projective transformation is performed on the image in the reference image corresponding to the local area for extracting the planar projection area according to the plane parameter of the plane portion projected on the local area for extracting the planar projection area. Thus, an image having the same pixel value distribution as the image of the local region for extracting the planar projection region of the reference image is obtained.

  On the other hand, even if the image of the local area for extracting the planar projection area is an image of the planar part, the reference corresponding to the local area for extracting the planar projection area is used by using different plane parameters from those corresponding to the actual planar part. An image obtained when projective transformation is performed on an image in the image is an image having a different pixel value distribution from the image of the local area for planar projection area extraction of the reference image.

  Therefore, in the second invention, the plane projection area extracting means next performs a step between each plane projection area extraction local area image and an area image in the reference image corresponding to the plane projection area extraction local area. A plane parameter for extracting a plane projection area, which is a plane parameter that defines the projective transformation of the image, a pixel value distribution of the image of the local area for extracting the plane projection area, and the reference image corresponding to the local area for extracting the plane projection area Is determined so as to minimize an error with respect to the pixel value distribution of the image obtained when projective transformation is performed on the image of the area in accordance with the plane parameter for plane projection area extraction.

  As a result, in the planar projection region extraction local region where the planar portion such as the tread of the step portion is projected, the planar parameter for extracting the planar projection region indicating the position and orientation of the plane including the planar portion is appropriate. Will be determined.

  In addition, when a single plane part and other parts are included in the local area for plane projection area extraction, the plane defined by the plane parameter for plane projection area extraction to be determined is generally The plane is different from the plane including the plane portion included in the local area for plane projection area extraction.

  When the plane projection area extraction plane parameter corresponding to each plane projection area extraction local area is determined as described above, the plane projection area extraction plane parameter corresponding to the plane projection area extraction local area existing on the same plane is determined. Are identical (or nearly identical) to each other.

  Therefore, the plane projection area extraction means next causes the planes defined by the plane projection area extraction plane parameters corresponding to the respective plane projection area extraction local areas included in each plane projection area to coincide with each other. Each planar projection area is extracted from the reference image. Here, the fact that the plane parameters for extracting the plane projection area are the same does not mean that they are strictly the same, but the position and orientation of the plane defined by the plane parameters for extracting the plane projection area It is also included when they are almost identical within a certain predetermined range.

  Thus, according to the second invention, a method using a stereo image and projective transformation (planar projective transformation) without mounting a distance measuring device such as a laser distance measuring device on a moving body separately from the stereo camera. Thus, a planar projection area can be extracted from the reference image.

  In the first invention or the second invention, the plurality of candidate positions set in each calculation processing area by the evaluation function calculation means are N3a (N3a: integer greater than or equal to 2) candidates common to all calculation processing areas. And the evaluation function calculation means calculates the value of the error function corresponding to the k1th candidate position (arbitrary one of integers from k1: 1 to N3a) and each calculation processing area. In order to calculate, an image in the combined projective transformation target area formed by combining the projection conversion target areas for edge estimation of the respective computation processing areas in which the plane parameters for the projective conversion are the same with each other, and the composite projective conversion target area A process of obtaining an error image from an image obtained by performing projective transformation on an image of an area in the reference image according to a projective transformation plane parameter corresponding to the synthetic projective transformation target area; Using the difference image, it is preferable to calculate the value of the error function corresponding to the first k1-th candidate location and each processing region (third invention).

  According to the third invention, the evaluation function calculating means calculates the error function corresponding to the k1th candidate position, which is any one of the plurality of candidate positions, and each calculation processing area. The error image relating to the combined projective transformation target area obtained by synthesizing the edge estimation projective transformation target areas of the respective computation processing areas having the same projective transformation plane parameters is obtained in advance. Then, the evaluation function calculation means calculates the value of the error function corresponding to the k1th candidate position and each calculation processing region using the error image. As a result, the error function value of each candidate position and each set of calculation processing regions is compared with the case where the value of the error function is calculated individually for each set of each candidate position and each calculation processing region. Calculation can be performed efficiently.

  In the first to third aspects of the invention, in the case of further comprising a stepped portion presence detecting means for detecting whether or not a stepped portion is present in the imaging region of the stereo camera, the planar projection region extracting means and the evaluation The processing of the function calculating means and the step portion arrangement determining means is processing executed on the condition that the step portion presence detecting means detects that a step portion is present in the imaging area of the stereo camera. The

  In this case, the stepped portion presence detecting means can detect whether or not a stepped portion is present based on information given from the outside, but using the reference image and the reference image The detection can also be performed.

  Specifically, the stepped portion presence detection means sets a stepped portion presence detection region for detecting whether or not a stepped portion is present in the imaging region of the stereo camera in the reference image; Step parameter presence detection plane parameter, which is a plane parameter that defines projective transformation between the image of the level difference presence detection region and the image of the region in the reference image corresponding to the level difference presence detection region, Pixel value distribution of an image of the stepped portion presence detection region and a pixel value of an image obtained by performing projective transformation on the image of the region corresponding to the stepped portion presence detecting region according to the stepped portion presence detection plane parameter Processing to determine an error with the distribution is minimized, and a step is set in the imaging area of the stereo camera based on the direction of the normal direction of the plane indicated by the determined step parameter for detecting the presence of the stepped portion. Whether to detect the part is present (fourth invention).

  According to the fourth aspect of the invention, the direction of the normal direction of the plane defined by the step parameter presence detection plane parameter determined by the processing of the level difference presence detection means is such that the level difference is in the imaging area of the stereo camera. If present, the image pickup area is inclined with respect to the normal direction when the imaging area is a flat floor surface. Therefore, it is possible to detect whether or not there is a stepped portion in the imaging region of the stereo camera based on the direction of the normal direction of the plane defined by the stepped portion presence detection plane parameter.

In the first to fourth aspects of the invention, a step portion type for determining whether the step portion is an ascending step portion or a descending step portion when the step portion is imaged by the stereo camera. A judgment means,
The evaluation function calculating means includes
When the stepped portion type determining means determines that the stepped portion is an ascending stepped portion, in the process of setting the plurality of calculation processing regions in the reference image, the upper end of each calculation processing region is the stepped portion. The plurality of lines of arithmetic processing are positioned in the planar projection area corresponding to the upper stage tread of the section and the lower end of each computation processing area is positioned in the plane projection area corresponding to the lower stage tread of the step In the process of setting a region and setting the plurality of edge estimation projection transformation target regions in each of the calculation processing regions, the first and second edge estimation projection transformation target regions adjacent to each other with the candidate positions as boundaries Set on the upper side of the arithmetic processing region, and between the first and second edge estimation projective transformation target regions, the second edge estimation projective transformation target region is the lower region. Depending on the candidate position The third projective transformation target area for edge estimation with interval set on the lower side of the processing area,
When the stepped portion type determining means determines that the stepped portion is a descending stepped portion, in the processing for setting the plurality of calculation processing regions in the reference image, the upper end of each calculation processing region is on the lower side. The plurality of calculation processing areas are set so that the lower end of each calculation processing area is located in the planar projection area corresponding to the tread, and the lower end of each calculation processing area is positioned in the flat projection area corresponding to the upper tread. In the process of setting the plurality of edge estimation projective transformation target regions in the processing region, the fourth and fifth edge estimation projective transformation target regions adjacent to each other at the candidate positions are set in the calculation processing region. It is preferable (5th invention).

  According to this fifth aspect, when the stepped portion type determining means determines that the stepped portion is an ascending stepped portion, the evaluation function calculating means calculates the plurality of items on the reference image. In the process of setting the processing area, the upper end of each calculation processing area is positioned in the planar projection area corresponding to the upper step surface of the stepped portion, and the lower end of each calculation processing area is positioned on the lower step surface of the stepped portion. The plurality of calculation processing areas are set so as to be positioned in the corresponding planar projection areas. Accordingly, the plurality of calculation processing areas can be set as the reference image so that each of the plurality of calculation processing areas includes the actual edge projection line.

  Further, the evaluation function calculating means is configured to set the plurality of edge estimation projection transformation target areas in the calculation processing areas, and the first and second edge estimation projections adjacent to each other with the candidate positions as boundaries. A conversion target area is set on the upper side of the arithmetic processing area, and a second edge estimation projective transformation target area which is a lower area of the first and second edge estimation projective transformation target areas; A third edge estimation projective transformation target area having an interval according to the candidate position is set to the lower side of the arithmetic processing area.

  In this case, in any one set of each calculation processing region and each candidate position, the first to third edge estimation projective transformation target regions set in the calculation processing region are respectively the actual candidate positions. A region where the upper step surface of the step portion is projected when it coincides with the position of the edge projection line, a region where a surface standing up with respect to the upper step surface (a tip surface of the nose or a kick surface) is projected, a step This is an area that is set to correspond to the area on which the lower tread surface is projected. And these 1st-3rd edge estimation projection conversion object area | regions will change according to a candidate position.

  Therefore, when there is an ascending step in the imaging area of the stereo camera, the evaluation function value calculated by the evaluation function calculation unit corresponding to each set of each calculation processing area and each candidate position includes: In addition to the images of the first and second edge estimation projection transformation target areas adjacent to each other at the candidate position, the pixel values of the third edge estimation projection transformation target area images having different plane parameter vectors are reflected. The Rukoto. Therefore, it is possible to increase the reliability of the value of the evaluation function as representing the degree of fitness of each candidate position in each calculation processing region with respect to the position of the actual edge projection line.

  On the other hand, when it is determined that the stepped portion is a descending stepped portion by the stepped portion type determining means, the evaluation function calculating means is a process for setting the plurality of calculation processing regions in the reference image. The plurality of lines are arranged such that the upper end of each calculation processing area is located in the planar projection area corresponding to the lower tread, and the lower end of each calculation processing area is located in the planar projection area corresponding to the upper tread. Set the calculation processing area.

  Accordingly, the plurality of calculation processing areas can be set as the reference image so that each of the plurality of calculation processing areas includes the actual edge projection line.

  Further, the evaluation function calculating means is configured to set the plurality of edge estimation projection transformation target areas in the calculation processing areas, and the fourth and fifth edge estimation projections adjacent to each other with the candidate positions as boundaries. A conversion target area is set in the calculation processing area.

  Here, when the stepped portion present in the imaging basin of the stereo camera is a stepped portion that descends, the reference image has an attitude that stands up with respect to the upper transparent side, such as the tip surface of the nose or the kicked surface. The face of is not reflected. Therefore, the evaluation function calculation means sets the fourth and fifth edge estimation projection transformation target areas in the calculation processing area. These fourth and fifth edge estimation projective transformation target regions are regions where the upper step side tread is projected when the candidate position matches the position of the actual edge projection line, and the step portion. This is an area set to correspond to the area where the lower tread is projected. Then, these fourth and fifth edge estimation projective transformation target regions change in accordance with the candidate positions.

  Therefore, when the stepped portion present in the imaging region of the stereo camera is a stepped stepped portion, the evaluation function calculated by the evaluation function calculating unit corresponding to each set of each calculation processing region and each candidate position The value appropriately reflects the pixel values of the images of the fourth and fifth edge estimation projection transformation target areas adjacent to each other at the candidate position. Therefore, it is possible to appropriately ensure the reliability of the value of the evaluation function as representing the degree of fitness of each candidate position in each arithmetic processing region with respect to the position of the actual edge projection line.

  Therefore, according to the fifth invention, the estimated edge projection is assumed to correspond to the actual edge projection line regardless of whether the stepped portion present in the imaging area of the stereo camera is an ascending stepped portion or a descending stepped portion. The line can be determined with appropriate reliability.

  In the fifth aspect of the present invention, when the stepped portion present in the imaging region of the stereo camera is an ascending stepped portion, an upper stepped tread like a nose tip or kick surface is obtained by the planar projection region extracting means. When the plane projection area of the plane portion in the standing posture is extracted, the plane in the standing posture is used as the projection transformation plane parameter corresponding to the second edge estimation projection transformation target region. Planar parameters corresponding to the planar projection area of the part can be used.

  However, a flat portion in a posture standing with respect to the upper step surface, such as a step nose tip surface or a kick surface of the step portion, generally stands at an angle that is generally perpendicular to or close to the upper step surface. Therefore, in the fifth aspect of the invention, the evaluation function calculation means corresponds to each edge estimation projection transformation target area when the step difference type determination means determines that the step difference is an ascending step difference. In the process of setting the plane parameter for edge estimation to be performed, as the projection conversion plane parameter corresponding to each of the first edge estimation projection conversion target region and the third edge estimation projection conversion target region, Setting the plane parameter determined for the planar projection area corresponding to the upper step surface of the step portion and the plane parameter determined for the plane projection area corresponding to the lower step surface of the step portion, As the projection transformation plane parameter corresponding to the second edge estimation projection transformation target region, the first edge estimation projection transformation subject region. A plane parameter that represents the position and orientation of a plane that intersects the plane defined by the corresponding plane parameter for projective transformation at a predetermined angle (for example, 90 degrees or an angle close thereto) may be set (No. 6 invention).

  According to the sixth invention, when the stepped portion present in the imaging region of the stereo camera is an ascending stepped portion, only the planar projection region corresponding to each tread is extracted by the planar region extracting means. Even if it exists, the plane parameter for projection transformation corresponding to the 1st-3rd edge estimation projection transformation object area | region can be set appropriately.

  If the stepped portion type determining means determines that the stepped portion is a descending stepped portion, the evaluation function calculating means is for edge estimation corresponding to each edge estimation projective transformation target region. In the process of setting the plane parameter, the projection conversion plane parameter corresponding to each of the fourth edge estimation projective transformation target area and the fifth edge estimation projective transformation target area, The plane parameter determined for the planar projection area corresponding to the lower tread surface and the plane parameter determined for the plane projection area corresponding to the upper tread surface of the stepped portion may be set.

  Supplementally, in the fifth invention or the sixth invention, when the configuration of the fourth invention is incorporated, the stepped portion type determining means is the stepped portion presence detecting plane determined by the stepped portion presence detecting means. Based on the direction of the normal direction of the plane indicated by the parameter, it can be determined whether the stepped portion is an ascending stepped portion or a descending stepped portion.

  This is because, depending on whether the stepped portion is an ascending stepped portion or a descending stepped portion, the normal line of the plane indicated by the stepped portion presence detection plane parameter is flat without a stepped portion. This is because the direction of inclination with respect to the normal direction of the smooth surface is different.

  By configuring the stepped portion type determining means in this way, the stepped portion is an ascending stepped portion easily using the stepped portion presence detecting plane parameter determined by the stepped portion presence detecting means, It can be judged whether it is a step part of descending.

  In the present invention, the stepped portion space arrangement determining means predetermines a tip side edge of the upper stepped tread of the stepped portion with respect to a plane defined by the plane parameter corresponding to the upper stepped tread. It is preferable to include means for determining a line projected on a plane defined by the plane parameter corresponding to the lower step surface of the stepped portion in a direction intersecting with an angle as a boundary line on the base end side of the lower step surface. (Seventh invention).

  According to the eighth aspect of the invention, in addition to the data representing the spatial position and direction of the leading edge of the upper tread surface of the stepped portion, the boundary of the base end side of the lower tread surface is determined by the stepped portion space arrangement determining means. A line is determined. For this reason, it becomes possible to recognize the depth of the lower step surface of the upper step side tread.

  Further, in the present invention, it is preferable that the moving body is equipped with a projector that gives a texture to an imaging region of the stereo camera during imaging by the stereo camera (eighth invention).

  According to the eighth aspect of the invention, the pixel value distribution of an image in an arbitrary area of a standard image on which a certain plane portion is projected and the corresponding image of the area of the reference image are projected and converted according to appropriate plane parameters. The difference from the pixel value distribution of the projective transformation image obtained in this case tends to become conspicuous when the plane parameter is different from the plane parameter of the actual plane portion.

  For this reason, the value of the evaluation function calculated by the evaluation function calculation means, or the plane parameter for plane projection area extraction (and thus the plane parameter of each plane projection area) determined by the plane projection area extraction means in the second invention, Alternatively, the reliability of the stepped portion presence detection plane parameter determined by the stepped portion presence detecting means in the fourth invention can be enhanced.

FIGS. 1A and 1B are perspective views showing a situation in which a robot (moving body) according to an embodiment of the present invention is going up and down. The figure which illustrates the texture provided to the imaging region of a stereo camera by the light projector mounted in the robot of embodiment. The figure for demonstrating the structure of a typical staircase. The block diagram which shows the functional structure of the arithmetic processing unit with which the robot shown in FIG. 1 was equipped. The flowchart which shows the process of the level | step-difference part presence detection part shown in FIG. The figure for demonstrating the process of STEP1 of FIG. 7A and 7B are diagrams for explaining the processing of STEP2 in FIG. The flowchart which shows the process of the plane projection area | region extraction part shown in FIG. The figure for demonstrating the process of STEP11,15 of FIG. The graph which shows the data obtained by the process of STEP13 of FIG. 5 is a flowchart showing processing of an edge projection line estimation processing unit shown in FIG. 5 is a flowchart showing processing of an edge projection line estimation processing unit shown in FIG. The figure for demonstrating the process of STEP22 of FIG. The figure for demonstrating the process of STEP23 of FIG. The figure for demonstrating the process of STEP30 of FIG. 16A and 16B are graphs illustrating data obtained by the processing of STEP 32 in FIG. The figure for demonstrating the process of STEP35 of FIG. The figure for demonstrating the process of STEP36 of FIG. The figure for demonstrating the process of STEP37 of FIG. The figure for demonstrating the process of STEP38 of FIG. The figure for demonstrating the process of STEP45 of FIG.

  An embodiment of the present invention will be described below with reference to FIGS.

  With reference to FIGS. 1A and 1B, in the present embodiment, a case where a spatial arrangement position of a stepped portion such as a staircase is recognized by a legged mobile robot 1 as a moving body will be described as an example. An illustrated legged mobile robot 1 (hereinafter simply referred to as a robot 1) is a biped walking robot including a pair of left and right legs.

  The robot 1 is equipped with a pair of left and right cameras 3R and 3L that constitute a stereo camera, and a projector 4 that projects light onto the imaging areas of the cameras 3R and 3L.

  The camera 3R is a camera on the right side facing the imaging area, and the camera 3L is a camera on the left side facing the imaging area, and each is constituted by a CCD camera or the like. Note that images captured by the cameras 3R and 3L may be either monotone images or color images. 1A and 1B, the cameras 3R and 3L are mounted on the head of the robot 1, but may be mounted on other parts such as the chest of the robot 1.

  The robot 1 captures a moving environment around the robot 1 (mainly, an environment ahead of the direction of travel of the robot 1) with these cameras 3R and 3L, so that the captured image of the camera 3R and the captured image of the camera 3L It is possible to acquire a stereo image composed of

  Further, in the present embodiment, the robot 1 projects the auxiliary light SL from the projector 4 for applying a texture to the imaging area during imaging by the cameras 3R and 3L. In the present embodiment, as shown in FIG. 2, the projector 4 intersects a plurality of oblique bright lines SLa arranged in parallel with each other on the surface of the imaging region (a plane in FIG. 2) and the oblique bright lines SLa. Laser light of a predetermined color (for example, red) that forms a texture composed of a plurality of oblique bright lines SLb arranged in parallel with each other in the direction is output as the auxiliary light SL.

  Note that the texture formed by the projector 4 is not limited to the above pattern, but may be a pattern with another pattern such as random dots.

  Then, the robot 1 moves while recognizing the floor shape, the installed object, and the like of the surrounding moving environment based on the stereo images acquired by the cameras 3R and 3L.

  For example, as shown in FIGS. 1 (a) and 1 (b), there is a staircase 50 (ascending staircase in FIG. 1 (a), descending staircase in FIG. 1 (b)) in front of the robot 1 in the direction of travel. In the situation, the robot 1 has the staircase 50 in the imaging area of the cameras 3R and 3L based on the stereo image of the environment ahead in the traveling direction acquired by the cameras 3R and 3L, and the space of the staircase 50. Recognizing specific placement positions.

  In this case, in the robot 1 according to the present embodiment, in order to recognize the spatial arrangement position of the staircase 50, the spatial positions of the leading edge 52 and the proximal edge 53 of the tread surface 51 of each step of the staircase 50 are determined. An arrangement position and a spatial posture (normal direction) of a plane including each tread surface 51 are estimated.

  Here, supplementing the leading edge 52 and the proximal edge 53 of each tread 51 of the staircase 50 recognized by the robot 1, the lower tread 51 of the staircase 50 is moved to the staircase 50 as in the robot 1 shown in FIG. When viewed from a line of sight where the upper tread surface 51 on the near side and the upper step side tread 51 is on the back side, the leading edge 52 of each tread surface 51 is the boundary line of the outer shape of the tread surface 51. This is a boundary line extending linearly in the lateral direction (the width direction of the stairs 50) on the front side. Further, in this case, the base end side edge 53 of each tread surface 51 is a boundary line extending linearly in the lateral direction on the back side of the tread surface 51 among the boundary lines of the outer shape of the tread surface 51.

  Further, as in the robot 1 shown in FIG. 1B, when the staircase 50 is viewed with a line of sight where the upper tread surface 51 is on the near side and the lower tread surface 51 on the upper step side tread 51 is on the back side. In this case, the leading edge 52 of each tread surface 51 is a boundary line extending linearly in the lateral direction (the width direction of the stairs 50) on the back side among the boundary lines of the outer shape of the upper tread surface 51. Further, in this case, the base end side edge 53 of each tread surface 51 is a boundary line extending linearly in the lateral direction on the front side of the tread surface 51 among the outer boundary lines of the tread surface 51.

  Further, supplementing the structure of the staircase as the step portion with reference to FIG. 3, the tip portion of the tread surface of each step of the staircase is a portion generally referred to as a nose. Accordingly, the leading edge of the tread surface of the staircase means, in other words, the front boundary line of the upper surface of the nose of each step of the staircase. In addition, a portion between the distal end side of the upper step surface of the stairs and the proximal end side of the lower step surface of the lower step is generally referred to as kicking. In many cases, the tip (step nose) of the tread surface of each step of the staircase is a surface portion (hereinafter referred to as a kick-in) formed with the tip surface standing upright at the above-mentioned kick-in location as shown in FIG. It is formed so as to protrude slightly from the surface).

  However, in the present embodiment, in the staircase that is mainly described as an example of the stepped portion, it is not necessary that the tip end surface of the stair nose protrudes slightly from the kicked surface. For example, like the staircase 50 described briefly in FIGS. 1A and 1B, the front end surface of the step nose and the kicking surface may be formed flush or substantially flush.

  In the present embodiment, the staircase that is mainly described as an example of the stepped portion has a structure in which the plate member that forms the kick surface is not provided at the place of the kick (the kick surface does not exist). There may be.

  Referring to FIG. 4, the robot 1 is equipped with an arithmetic processing unit 10 configured by an electronic circuit unit including a CPU, a RAM, and a ROM. The cameras 3R and 3L and the projector 4 are connected to the arithmetic processing unit 10.

  This arithmetic processing unit 10 has a function of controlling the operations of the cameras 3R and 3L and the projector 4, and the imaging operation of the cameras 3R and 3L and the output operation of the auxiliary light SL of the projector 4 are synchronized with each other. Is controlled so as to be performed at the timing of (a timing of a predetermined control processing cycle). Then, the arithmetic processing device 10 takes in pixel value data (data representing luminance, saturation, hue, and the like for each pixel constituting the captured image) of the captured images respectively acquired by the cameras 3R and 3L, and this pixel value data Are stored and held while being sequentially updated in an image memory (not shown).

  It should be noted that a band-pass filter that limits the wavelength of light incident on the cameras 3R and 3L is provided, and only the light having a wavelength that matches or substantially matches the wavelength of the auxiliary light SL may be captured by the cameras 3R and 3L. Good.

  Furthermore, the arithmetic processing device 10 includes a stepped portion presence detection unit 11 that detects whether or not a stepped portion is present in the imaging regions of the cameras 3R and 3L, and the imaging region as functions realized by the installed program. A plane projection that extracts a plane projection area formed by projecting a plane portion of an actual stepped portion present in a reference image among stereo images and determines (estimates) plane parameters of the plane portion corresponding to each plane projection area An edge projection line estimation process that executes processing for estimating (specifying) an actual edge projection line that is obtained by projecting the leading edge of the tread of the actual stepped portion existing in the imaging region onto the reference image Part 13 and a step part space arrangement determining part 14 for specifying a spatial arrangement position of the step part. And the arithmetic processing apparatus 10 implement | achieves the function as one Embodiment of the level | step difference recognition apparatus of this invention by the process of these function parts 11-14.

  In this case, in the present embodiment, one of the captured image of the camera 3R and the captured image of the camera 3L, for example, the captured image of the camera 3R is used as the reference image, and the captured image of the other camera 3L is used as the reference image. Note that the arithmetic processing device 10 may have a function of controlling the operation of the robot 1.

  Hereinafter, the processes of the stepped portion presence detecting unit 11, the planar projection region extracting unit 12, the edge projected line estimation processing unit 13, and the stepped portion space arrangement determining unit 14 will be described in detail.

  Pixel value data of a captured image (stereo image) ahead of the robot 1 in the traveling direction acquired by the cameras 3R and 3L is provided to the stepped portion presence detecting unit 11. And the level | step-difference part presence detection part 11 performs the process shown to the flowchart of FIG. 5 using the given stereo image.

  The step difference presence detection unit 11 first executes the processing of STEP1. In STEP 1, the stepped portion presence detection unit 11 detects a stepped portion presence detection that is a region for detecting the presence or absence of a stepped portion in a reference image of a given stereo image as illustrated in FIG. 6. A use area R1 is set.

  The reference image shown in FIG. 6 schematically illustrates the reference image in the case where the staircase 50 (ascending staircase in the illustrated example) exists in the imaging regions of the cameras 3R and 3L. In this case, bright lines SLa and SLb as textures are given to the surface of the staircase 50 by the auxiliary light SL output from the projector 4. The bright lines SLa and SLb are also projected on the reference image. FIG. 6 representatively shows a part of the bright lines SLa and SLb projected on the reference image.

  In this embodiment, the step portion presence detection region R1 set by the step portion presence detection unit 11 in STEP 1 is a rectangular region whose size (width in the horizontal direction and vertical direction of the reference image) is predetermined. It is. The width in the horizontal direction of the stepped portion presence detection region R1 is large enough to fit within the width in the horizontal direction of the stepped portion in the reference image when a stepped portion such as a staircase is projected onto the reference image. Is set.

  Further, the vertical width of the stepped portion presence detection region R1 includes images of a plurality of steps on the stepped portion presence detection region R1 when a stepped portion such as a staircase is projected onto the reference image. It is set to such a size.

  Then, the stepped portion presence detection region R1 exists in the planned travel region of the robot 1 (the region through which the robot 1 passes when the robot 1 is moved in the target travel direction) in the reference image. Placed in position.

  Next, the stepped portion presence detection unit 11 performs the processing of STEP2. In STEP 2, the stepped portion presence detection unit 11 assumes that the image of the stepped portion presence detection region R 1 set in the reference image is a projection image of a portion on a certain plane, and fits this assumption as much as possible. A plane parameter representing the spatial position and orientation of the plane is determined by a method using projective transformation (specifically, plane projective transformation).

  In the present embodiment, the unit vector ↑ n in the normal direction of the plane is used as the plane parameter, and the distance from the optical center C of the camera 3R that captures the reference image (hereinafter also referred to as the reference camera 3) to the plane. A vector ↑ m (≡ ↑ n / d) obtained by dividing by d is used.

  For example, a plane parameter representing the position and orientation of a plane (a virtual plane in the figure) indicated by a two-dot chain line in FIG. 7 (a) or FIG. 7 (b) is indicated by an arrow with a reference symbol ↑ m1 in the figure. The vector is expressed as a vector perpendicular to the plane and having a size of 1 / d.

  Hereinafter, the vector ↑ m defined in this way is referred to as a plane parameter vector. In this specification, “↑” is used as a symbol representing a vector (vertical vector).

  Here, generally speaking, the projective transformation, when projecting a planar portion such as a tread of a staircase on two captured images constituting a stereo image, converts an image of the planar portion in one captured image, The image is converted into an image of the plane portion in the other captured image. The projective transformation matrix P for performing the conversion is known when the image coordinates (two-dimensional coordinates representing the position of each pixel in each captured image) of the two captured images of the cameras 3R and 3L are expressed in homogeneous coordinates. This is a matrix represented by the following equation (1-1).

P = R + ↑ t · ↑ m ^T (1-1)

In Expression (1-1), R is a rotation matrix between the camera coordinate systems of the cameras 3R and 3L, ↑ t is a translation vector between the camera coordinate systems, and ↑ m ^T is the plane parameter vector ↑ This is a transposed vector of m.

  In this case, the camera coordinate system of the camera 3R is a coordinate system fixed with respect to the camera 3R, as shown in FIGS. In this camera coordinate system, for example, with the optical center C of the camera 3R as the origin, the direction of the optical axis Lc is the z-axis direction, the vertical direction (vertical direction) of the imaging surface of the camera 3R is the y-axis direction, and the horizontal direction of the imaging surface It is a triaxial coordinate system in which the direction is the x-axis direction. The same applies to the camera coordinate system of the left camera 3L.

  The rotation matrix R and the translation vector ↑ t are determined depending on the relative positional relationship and posture relationship between the cameras 3R and 3L, and are specified in advance in this embodiment. Accordingly, the projective transformation matrix P is defined by the plane parameter vector ↑ m.

  Supplementally, the projection transformation matrix from the standard image to the reference image and the projection transformation matrix of the inverse transformation thereof (projection transformation matrix from the reference image to the standard image) are both on the right side of Expression (1-1). Expressed in shape. However, the values of R, ↑ t, and ↑ m are different for each projection transformation matrix.

Specifically, when the projective transformation matrix P expressed by the expression (1-1) is a projective transformation matrix from one of the standard image and the reference image to the other, the inverse transformation of the projective transformation matrix ( Hereinafter, this is expressed as P ⁻¹ ) is defined by R ′, ↑ t ′, ↑ m ′, ↑ m ′, defined by the proviso expressions (1-2a) to (1-2d) of the following expression (1-2): Using ↑ d ′, it is expressed by Expression (1-2) having the same form as Expression (1-1) above. Note that R ^T _and (↑ m ′) ^T are an R transpose matrix and an (↑ m ′) transpose vector, respectively.

P ⁻¹ = R ′ + ↑ t ′ · (↑ m ′) ^T (1-2)
However,
R′≡R ^T (1-2a)
↑ t'≡-R ^T・ ↑ t ...... (1-2b)
↑ m'≡R ・ ↑ m / d '(1-2c)
↑ d'≡d ・ (1 + ↑ m ^T・ R ^T・ ↑ t) ...... (1-2d)

In the following description of the present embodiment, for the sake of convenience, the projection transformation matrix from the base image to the reference image is referred to as a projection transformation matrix P represented by Equation (1-1), and the projection transformation matrix from the reference image to the base image is represented by Equation (1-1). Let it be a projective transformation matrix P ⁻¹ represented by (1-2). Further, the plane parameter vector ↑ m is expressed as a vector viewed in the camera coordinate system of the reference camera 3 (camera 3R in the present embodiment) for convenience.

  When the image of the stepped portion presence detection region R1 in the reference image is a projection image of a portion on a certain plane (planar portion), the image of the stepped portion presence detection region R1 and the stepped portion presence detection A region in the reference image corresponding to the region R1 (specifically, a region obtained by projecting a planar portion of the real space (actual space) projected onto the stepped portion presence detection region R1 of the standard image onto the reference image) The relationship between these images is correlated by the projective transformation.

More specifically, an image of a region in the reference image corresponding to the stepped portion presence detection region R1 of the standard image is projected according to the above formula (1-2) according to the plane parameter vector ↑ m of the plane portion. The pixel value distribution of the projective transformation image obtained when the projective transformation is performed using the transformation matrix P ⁻¹ matches or substantially matches the pixel value distribution of the image of the stepped portion presence detection region R1 of the reference image.

  It should be noted that the pixel value at each pixel position of the projective transformation image (position seen in the image coordinate system of the reference image) is expressed by the above equation (1-) according to the plane parameter vector ↑ m of the plane portion. It coincides with the pixel value of the pixel position in the reference image (position viewed in the image coordinate system of the reference image) converted by the projective transformation matrix P defined in 1). Therefore, in other words, the projective transformation image is obtained by converting the pixel values at all pixel positions of the stepped portion presence detection region R1 of the reference image into the pixel positions of the stepped portion presence detecting region R1 using the projective transformation matrix P. Matches the image that is replaced with the pixel value at the pixel position in the reference image associated with.

  In the processing of STEP 2, the stepped portion presence detecting unit 11 is a plane of the virtual plane (virtual plane) when it is assumed that a planar portion is projected on the stepped portion presence detecting region R <b> 1 set in the reference image. The parameter vector ↑ m is determined by the process described below.

  That is, the stepped portion presence detecting unit 11 sets the provisional value (candidate value) of the planar parameter vector ↑ m, the pixel value distribution of the image of the stepped portion presence detection region R1 of the reference image, and the set ↑ m An error function D1 representing an error from the pixel value distribution of the projective transformation image obtained when the image of the region in the reference image corresponding to the stepped portion presence detection region R1 is subject to the projective transformation defined by the provisional value. And the process of updating the provisional value of the plane parameter vector ↑ m in accordance with the value of the error function D1 are repeated until a predetermined predetermined condition is satisfied.

  Then, the stepped portion presence detection unit 11 uses the provisional value of ↑ m when the predetermined condition is satisfied as a plane parameter vector ↑ m of the virtual plane projected on the stepped portion presence detection region R1 of the reference image. decide. Hereinafter, the plane parameter vector ↑ m determined by the step portion presence detection unit 11 in this way is referred to as a step portion presence detection plane parameter vector ↑ m1.

  Here, the pixel value at an arbitrary pixel position Q in the stepped portion presence detection region R1 of the reference image is defined by I1 (Q), and the pixel position Q on the reference image is defined by the provisional value of the plane parameter vector ↑ m. The pixel position on the reference image obtained by projective transformation by the projective transformation matrix P of the equation (1-1) is f (Q), and the pixel value of the reference image at this pixel position f (Q) is I2 (f (Q )), And the total number of pixels in the step portion presence detection region R1 is N1. At this time, the error function D1 is a function given by the following equation (2-1), for example.

  In the present embodiment, the pixel value of each pixel of the captured image (base image or reference image) is the luminance value of the captured image at the pixel. However, when the captured image is a color image, the hue or saturation of the captured image may be used as the pixel value.

D1 = (1 / N1) · Σ (I1 (Q) −I2 (f (Q))) ² (2-1)

  In the equation (2-1), Σ means the sum of all the pixels in the stepped portion presence detection region R1. Therefore, the error function D1 defined by the equation (2-1) is the pixel value I1 (Q) in the stepped portion presence detection region R1 of the standard image and the corresponding pixel of the projective transformation image of the reference image. The average value of the square values of the difference from the value I2 (f (Q)) is obtained by calculating the pixel value distribution of the reference image in the stepped portion presence detection region R1 and the pixel value distribution of the corresponding projected image of the reference image. It is a function expressed as an error.

  Instead of the error function D given by the equation (2-1), for example, an error function D1 given by the following equation (2-2) may be used.

D1 = (1 / N1) · Σ | (I1 (Q) −I2 (f (Q)) | (2-2)

  The error function D1 represented by the equation (2-2) is an average value of absolute values of differences between I1 (Q) and I2 (f (Q)) in the stepped portion presence detection region R1 of the reference image. Is expressed as an error between the pixel value distribution of the base image in the stepped portion presence detection region R1 and the pixel value distribution of the projection conversion image of the reference image corresponding thereto.

  The value of the error function D1 calculated as described above changes depending on the provisional value of ↑ m. When the portion projected on the stepped portion presence detection region R1 is an actual plane portion, when the provisional value of ↑ m matches the plane parameter vector of the plane including the plane portion, The value of the error function D1 is minimum (almost zero).

  Accordingly, the provisional value of ↑ m that minimizes (or almost minimizes) the value of the error function D1 is the position of the virtual plane including the virtual plane portion projected on the stepped portion presence detection region R1 and The goodness of fit as a plane parameter vector representing the posture is high.

  Therefore, in the process of updating the provisional value of ↑ m according to the error function D1, the stepped portion presence detection unit 11 can bring the value of the error function D1 closer to the minimum value by a known convergence calculation processing method. Update the provisional value of ↑ m. Then, the stepped portion presence detection unit 11 executes the update of the provisional value of ↑ m until a predetermined condition is satisfied, so that the value of the error function D1 is finally minimized (or substantially minimized). The step parameter detecting plane parameter vector ↑ m1 is determined.

  In this case, the predetermined condition is a predetermined condition that assumes that the updated ↑ m matches or substantially matches the value that can minimize the error function D when the condition is satisfied. The predetermined condition is, for example, that the absolute value of the difference between the value of the error function D1 calculated according to ↑ m before update and the value of the error function D1 calculated according to ↑ m after update is There are conditions such as a condition that the value is equal to or less than a predetermined threshold value (threshold value close to “0”), or a condition that the number of updates of the provisional value of ↑ m exceeds a predetermined value.

  As an initial value of the temporary value of ↑ m, for example, a plane parameter vector of a surface (floor surface or the like) on which the robot 1 is currently grounded can be used. However, the initial value may be a predetermined fixed value. The initial value may be a plane parameter vector of a plane inclined with respect to the current grounding surface of the robot 1.

  Next, the stepped portion presence detection unit 11 performs the process of STEP3. In STEP 3, the step portion presence detection unit 11 determines whether or not a step portion exists in the imaging region of the cameras 3 R and 3 L based on the direction of the step portion presence detection plane parameter vector ↑ m 1 determined in STEP 2. Judging.

  Here, when there is no step portion in the imaging regions of the cameras 3R and 3L and the imaging region is a substantially flat floor surface, the step parameter presence detection plane parameter vector determined in STEP 2 as described above ↑ The direction of m1 is substantially the same as the normal direction of the current grounding surface of the robot 1.

  On the other hand, when a stepped portion such as a staircase is present in the imaging areas of the cameras 3R and 3L, the direction of the stepped portion presence detection plane parameter vector ↑ m1 determined in STEP2 is the current ground plane of the robot 1 The inclination is relatively large with respect to the normal direction.

  For example, as shown in FIG. 7A, when the ascending staircase 50 exists in the imaging regions of the cameras 3R and 3L, or as shown in FIG. 7B, the staircase 50 descends into the imaging regions of the cameras 3R and 3L. Is present, the direction of the stepped portion presence detection plane parameter vector ↑ m1 determined by the stepped portion presence detecting unit 11 is the axis in the pitch direction (the direction crossing the traveling direction of the robot 1 (the left-right direction of the robot 1)). In the direction of the surroundings), the robot 1 is relatively inclined with respect to the normal direction of the current grounding surface of the robot 1.

  Therefore, in STEP 3, the step portion presence detection unit 11 tilts the determined step portion presence detection plane parameter vector ↑ m 1 by a predetermined angle or more in the pitch direction with respect to the normal direction of the current ground plane of the robot 1. It is determined whether or not there is a stepped portion such as a staircase in the imaging area of the cameras 3R and 3L depending on whether or not it exists.

  Specifically, in the stepped portion presence detection unit 11, the magnitude of the inclination angle of ↑ m1 (inclination angle in the pitch direction) with respect to the normal direction of the current ground plane of the robot 1 is equal to or greater than a predetermined predetermined angle. In this case, it is determined that there is a stepped portion such as a staircase in the imaging area of the cameras 3R and 3L. Further, when the inclination angle of ↑ m1 (inclination angle in the pitch direction) with respect to the normal direction of the current grounding surface of the robot 1 is less than a predetermined angle, the stepped portion presence detection unit 11 detects that the cameras 3R and 3L It is determined that there is no stepped portion such as a staircase in the imaging area.

  Note that the relative position and posture relationship between the current grounding surface of the robot 1 and the cameras 3R and 3L are based on the displacement amount of each joint of the robot 1 (specifically, the grounded leg of the robot 1 and the camera The amount of displacement of each joint existing between 3R and 3L). Therefore, the inclination angle of ↑ m1 with respect to the normal direction of the current ground plane of the robot 1 is the value of ↑ m1 seen in the camera coordinate system of the reference camera 3 (camera 3R) and the displacement amount of each joint of the robot 1. It can be calculated from the measured value (or a target value for tracking the displacement).

  Then, when the determination result of STEP 3 is affirmative (when it is determined that a step portion exists), the step difference presence detection unit 11 further executes the process of STEP 4. In STEP 4, the type of stepped portion (type of rising stepped portion or descending stepped portion) existing in the imaging regions of the cameras 3R and 3L is determined.

  Here, when an ascending step portion (ascending staircase 50 in the figure) is present in the imaging regions of the cameras 3R and 3L as shown in FIG. 7A, the step portion determined by the step portion presence detecting unit 11 is determined. The presence detection plane parameter vector ↑ m1 is a plane parameter vector of an ascending slope when viewed from the robot 1. Also, as shown in FIG. 7B, when there is a stepped portion (a descending staircase 50 in the figure) in the imaging area of the cameras 3R, 3L, the stepped portion presence detection unit 11 determines the presence of the stepped portion. The detection plane parameter vector ↑ m1 is a plane parameter vector of the descending slope when viewed from the robot 1.

  Therefore, in STEP4, the stepped portion presence detecting unit 11 determines whether the stepped portion ↑ m1 is positive or negative in the pitch direction with respect to the normal direction of the current ground plane of the robot 1. It is determined whether it is an ascending step or a descending step.

  The above is the details of the processing of the stepped portion presence detection unit 11.

  When the stepped portion presence detection unit 11 detects that a stepped portion is present in the imaging regions of the cameras 3R and 3L, the arithmetic processing device 10 next executes the process of the planar projection region extraction unit 12. .

  The planar projection area extraction unit 12 executes processing shown in the flowchart of FIG. 8 using captured images (stereo images) acquired by the cameras 3R and 3L.

  The planar projection area extraction unit 12 first performs the processing of STEP11. In STEP 11, the plane projection area extracting unit 12 includes a plurality of (N2) positions (u1, v (i)) (i = 1, 2,..., N2) in the reference image of the given stereo image. ) Is set to a local region R2 (i) which is a region having a very small area.

  Each local region R2 (i) is, for example, a rectangular region as shown in FIG. The size (width in the vertical and horizontal directions) of each local region R2 (i) is such that it can fit within the tread of each step of the step when the step image is projected on the reference image. Is set in advance.

  In FIG. 9, illustration of the oblique bright lines SLa and SLb by the auxiliary light SL output from the projector 4 is omitted. This also applies to FIGS. 13 and 19 described later.

  Supplementally, the shape of each local region R2 (i) may not be a square shape, and may be a shape such as a parallelogram or a trapezoid.

  The arrangement position (u1, v (i)) of each local region R2 (i) is represented by an image coordinate system in which the horizontal direction of the reference image is the u axis and the vertical direction (vertical direction) is the v axis. This is the position of the representative point of R2 (i). The representative point is the upper left corner point of each local region R2 (i) in the example shown in FIG. However, the representative point may be another point of each local region R2 (i), for example, the center point, the upper right corner point, the lower left corner point, the lower right corner point, or the like of each local region R2 (i). Good.

  In the present embodiment, among the arrangement positions (u1, v (i)) of the local regions R2 (i), the position u1 in the horizontal direction (u-axis direction) of the reference image is the same position. The position u1 is set so that each local region R2 (i) is disposed on the planned passage region of the robot 1 in the reference image. For example, the position u1 is set to a position near the center in the width direction of the scheduled passage area of the robot 1.

  Also, among the arrangement positions (u1, v (i)) of the local regions R2 (i), the position v (i) (i = 1, 2,..., N2) in the vertical direction (v-axis direction) of the reference image. Is a position set in advance so as to line up with a predetermined minute interval (for example, an interval of one pixel) from the lower part to the upper part of the reference image.

  In STEP 11, the planar projection area extraction unit 12 projects each local area R2 (i) whose arrangement position is determined as described above onto each local area R2 (i) of the reference image. When it is assumed that the portion is a plane portion, a plane parameter vector ↑ m of the plane portion is calculated. In the following description, the plane parameter vector ↑ m calculated corresponding to each local region R2 (i) (i = 1, 2,..., N2) is described using the reference symbol ↑ m_r2 (i). .

  In this case, the calculation of the plane parameter vector ↑ m_r2 (i) corresponding to each of the local regions R2 (i) (i = 1, 2,..., N2) The same process is performed. In this case, the calculation process of ↑ m_r2 (i) uses the local region R2 (i) instead of the stepped portion presence detection region R1 (R2 (as a region for calculating the value of the error function D1). The process of STEP2 may be the same except that i) is used.

  Next, the planar projection region extraction unit 12 executes the processing of STEP12. In this STEP 12, the plane projection area extraction unit 12 uses the plane parameter vector ↑ m_r2 (i) corresponding to each of the local areas R2 (i) (i = 1, 2,..., N2) calculated as described above. Calculate unit normal vector ↑ n_r2 (i), which is the unit vector in the normal direction of the plane whose position and orientation are defined by m_r2 (i) (virtual plane assumed to be projected on R2 (i)) To do.

  Since this unit normal vector ↑ n_r2 (i) is a unit length vector having the same direction as the plane parameter vector ↑ m_r2 (i), ↑ m_r2 (i) is set to its magnitude (= | ↑ m_r2 (i Calculated by dividing by) |).

  Next, the planar projection area extraction unit 12 executes the processing of STEP13. In STEP 13, the planar projection region extraction unit 12 calculates the inner product (scalar) of the unit normal vector ↑ n_r2 (i) in each local region R2 (i) and the unit vector ↑ nz in the Z-axis direction of the current robot coordinate system. Product).

  The robot coordinate system is a coordinate system fixed with respect to the current ground plane of the robot 1 as shown in FIG. 7A or 7B, and the normal direction of the ground plane is set. This is a coordinate system in which the Z-axis direction, the traveling direction of the robot 1 is the X-axis direction, and the direction perpendicular to the Z-axis and the X-axis (the left-right direction of the robot 1) is the Y-axis direction. Therefore, the unit vector ↑ nz in the Z-axis direction of the current robot coordinate system means a unit vector in the normal direction of the current ground plane of the robot 1.

  When calculating the inner product of the unit vector ↑ nz and the unit normal vector ↑ n_r2 (i), the unit normal vector ↑ n_r2 (i) as a vector viewed in the camera coordinate system of the reference camera 3 is Depending on the measured value (or target value) of the displacement amount of each joint of the robot 1, the coordinate is converted into a vector viewed in the current robot coordinate system. Then, the inner product of the unit normal vector ↑ n_r2 (i) after the coordinate conversion and the unit vector ↑ nz in the normal direction of the current ground plane of the robot 1 (Z-axis direction of the current robot coordinate system) is calculated. Is done.

  The unit normal vector ↑ n_r2 (i) viewed in the camera coordinate system and the vector obtained by coordinate conversion of the unit vector ↑ nz in the normal direction of the current ground plane of the robot 1 from the robot coordinate system to the camera coordinate system. The inner product may be calculated.

  As a result, as illustrated in FIG. 10, data representing the relationship between the position v (i) of each local region R2 (i) in the vertical direction of the reference image and the value of the inner product is obtained. .

  Here, when there are step portions in the imaging regions of the cameras 3 </ b> R and 3 </ b> L, each tread surface of the step portions is generally a plane portion substantially parallel to the current grounding surface of the robot 1. Therefore, the local area R2 (i) (for example, R2 (1), R2 (ib), R2 (N2) shown in FIG. 9) included in the projected area of the tread of the stepped portion in the reference image corresponds to the above. The inner product value calculated as described above is “1” or a value close thereto.

  On the other hand, each local region R2 (i) that is not included in the projection area of the stepped surface or each local region R2 (i) that includes only a part of the projected area of the stepped surface (for example, FIG. The value of the inner product corresponding to R2 (ia)) shown in 9 is a value deviating from “1”.

  Therefore, the value of the inner product is an index indicating whether or not the portion projected on each local region R2 (i) is a stepped portion of the stepped portion.

  Next, the planar projection region extraction unit 12 executes the processing of STEP14. In STEP 14, the planar projection area extracting unit 12 has a predetermined inner product value calculated in STEP 13 out of N2 local areas R2 (i) (i = 1, 2,..., N2). For each local region R2 (i) that is greater than or equal to the threshold Th2, the Z-axis coordinate position of the portion projected on the local region R2 (i) viewed in the robot coordinate system (ie, with respect to the current ground plane of the robot 1) The height of the projection part) is calculated based on the plane parameter vector ↑ m_r2 (i) corresponding to the local region R2 (i).

  Here, the predetermined threshold Th2 is a value slightly smaller than “1”, for example, 0.999. Therefore, each local region R2 (i) where the value of the inner product is equal to or greater than the threshold value Th2 is such that the portion projected onto the local region R2 (i) is substantially parallel to the current contact surface of the robot 1.

  The distance between the plane defined by the plane parameter vector ↑ m_r2 (i) corresponding to each local region R2 (i) that is equal to or greater than the threshold Th2 and the plane including the current grounding surface of the robot 1 (in detail) , The distance in the Z-axis direction of the robot coordinate system) is calculated to determine the Z-axis coordinate position (the height of the robot 1 relative to the current ground plane) of the portion projected onto the local region R2 (i). Is done.

  Next, the planar projection region extraction unit 12 executes the processing of STEP15. In STEP 15, the plane projection area extraction unit 12 extracts (specifies) a plane projection area corresponding to the tread of the step portion in the reference image based on the data obtained in STEP 14.

  Specifically, the planar projection area extraction unit 12 projects a v-axis direction section where the inner product value is equal to or greater than the threshold value Th2, and is projected onto the local area R2 (i) at each position in the section. A section in which the height of the portion (height from the ground contact surface of the robot 1) is substantially constant (the difference in height between the projection portions is equal to or less than a predetermined threshold value) is determined. Note that the threshold value for the magnitude (absolute value) of the difference between the heights of the projection portions is a minute value close to “0”, for example, 10 [mm].

  Δv1, Δv2, and Δv3 shown in FIG. 10 show examples of sections determined in this way. Each section determined in this way is a single area substantially parallel to the current ground contact surface of the robot 1 among the N2 local regions R2 (i) (i = 1, 2,..., N2) set in STEP11. This corresponds to the range of the position (position in the v-axis direction) of the representative point of the local area R2 (i), which is the projection area of one plane portion (a plane portion corresponding to each tread of the stepped portion). Hereinafter, each section determined in this way is referred to as a same plane projection section.

  Then, the plane projection area extraction unit 12 determines a plane projection area corresponding to each of the same plane projection sections determined as described above. In this case, in the present embodiment, each planar projection area has the same width as the local area R2 (i) (i = 1, 2,..., N2) in the lateral direction (u-axis direction) of the reference image, and A rectangular area arranged in the reference image so that its horizontal range (the range between the left and right ends of each planar projection area) matches the horizontal range of the local area R2 (i). It is.

  The range of each plane projection area (the range between the upper end and the lower end of each plane projection area) in the vertical direction (v-axis direction) of the reference image is within the same plane projection section corresponding to the plane projection area. It is determined so as to coincide with the range in the vertical direction of the combined region of all the local regions R2 (i) having the positions of the representative points.

  For example, when a section from the position of the reference image in the v-axis direction from v (ic) to v (ic + n) is one same plane projection section, the plane projection corresponding to the same plane projection section The range in the vertical direction (v-axis direction) of the region is the lowest local region among n + 1 local regions R2 (ic) to R2 (ic + n) having the position of the representative point in the same plane projection section. It is determined as a range between the lower end position of the region R2 (ic) and the upper end position of the uppermost local region R2 (ic + n).

  Through the processing of STEP 15 described above, a planar projection area corresponding to each tread of the stepped portion is extracted from the reference image. For example, a rectangular area AR2 indicated by a broken line frame in FIG. 9 is extracted from the reference image as a planar projection area.

  Next, the planar projection region extraction unit 12 executes the processing of STEP16. In STEP 16, the plane projection area extraction unit 12 determines whether or not two or more plane projection areas AR2 have been extracted by the processing in STEP 15. If the determination result is negative, there is a possibility that the stepped portion is not shown in the reference image, and thus the processing in FIG. 8 (processing in the current calculation processing cycle) is terminated.

  On the other hand, if the determination result in STEP 16 is affirmative, the planar projection area extraction unit 12 executes the process in STEP 17 and then ends the process in FIG. In STEP 17, the plane projection area extraction unit 12 determines a plane parameter vector ↑ m2 of a plane including the plane portion projected on each plane projection area AR2.

  In this case, the planar projection area extraction unit 12 may, for example, each of the local areas R2 (i) included in each planar projection area AR2 among the local areas R2 (i) (i = 1, 2,..., N2). A unit vector having an average azimuth of each azimuth angle (azimuth angle as seen in the camera coordinate system of the reference camera 3) of the plane parameter vector ↑ m_r2 (i) corresponding to the plane vector corresponding to the plane projection area AR2 Unit normal vector ↑ n2.

  Further, the plane projection area extraction unit 12 corresponds to each of the local areas R2 (i) included in each plane projection area AR2, and the magnitude (absolute value) of the plane parameter vector ↑ m_r2 (i) estimated in STEP 11 above. ) Is determined as the distance d2 of the plane corresponding to the planar projection area AR2 from the optical center C of the reference camera 3. The inverse value (= 1 / | ↑ m (i) |)

  A vector (= ↑ n2 / d2) obtained by dividing the unit normal vector ↑ n2 of the plane corresponding to each plane projection area AR2 by the distance d2 is determined as the plane parameter vector ↑ m2 corresponding to the plane projection area AR2. To do.

  The plane parameter corresponding to each planar projection area AR2 is processed by the same process (convergence calculation process using projective transformation) as in the case of estimating the plane parameter vector ↑ m_r2 (i) corresponding to each local area R2 (i). The vector ↑ m2 may be estimated again.

  The details of the processing of the planar projection area extraction unit 12 have been described above.

  Supplementally, in the present embodiment, when the same plane projection section is determined by the processing of STEP 15, in addition to the value of the inner product for each local region R2 (i) (i = 1, 2,..., N2). Although the height of the portion projected on each local region R2 (i) is referred to, the same plane projection section may be determined only from the inner product value.

  For example, a section in which the inner product value is continuously greater than or equal to a predetermined threshold Th2 in the vertical direction (v-axis direction) of the reference image (all local areas arranged in the vertical direction with representative point positions in the section) A section in which the inner product value corresponding to each of the regions R2 (i) is equal to or greater than the threshold value Th2 may be determined as the same plane projection section. In such a case, the processing of STEP14 is unnecessary.

  Further, not only a plane projection area corresponding to the stepped surface of the stepped portion existing in the imaging areas of the cameras 3R and 3L, but also a plane corresponding to a plane portion standing up with respect to the treaded surface, such as a nose tip surface or a kicked surface. You may make it extract a projection area | region.

  In this case, for example, paying attention to the direction of the plane parameter vector ↑ m_r2 (i) (or unit normal vector ↑ n_r2 (i)) for each local region R2 (i) (i = 1, 2,..., N2), A section whose direction is continuously kept substantially constant (it does not have to be the same direction as the normal direction of the current ground plane of the robot 1) is determined as the same plane projection section. Then, a plane projection area is determined in the same manner as described above according to each determined same plane projection section. As a result, not only a planar projection area corresponding to the stepped surface of the stepped portion but also a planar projected region corresponding to a planar portion standing up with respect to the treaded surface, such as the tip surface of the nose or the kicked surface of the stepped portion, can be extracted. it can.

  When two or more plane projection areas AR2 corresponding to the tread of the stepped portion are extracted by the plane projection area extraction unit 12 described above, the arithmetic processing device 10 next moves to the edge projection line estimation processing unit 13. Execute the process.

  The edge projection line estimation processing unit 13 includes captured images (stereo images) acquired by the cameras 3R and 3L, and a plane parameter vector ↑ m2 corresponding to each plane projection area AR2 determined by the plane projection area extraction unit 12. 11 and 12 are executed, the estimated edge projection line is obtained by estimating the actual edge projection line formed by projecting the leading edge of the upper tread surface onto the reference image. Identify.

  Note that estimating the actual edge projection line in the reference image means determining the position and direction (or parameters defining these) of the actual edge projection line in the reference image.

  The edge projection line estimation processing unit 13 first executes the processing of STEP21. In this STEP 21, the edge projection line estimation processing unit 13 has the type of the stepped portion (stepped portion existing in the imaging region of the cameras 3R, 3L) identified by the stepped portion presence detecting unit 11 by the processing in STEP4 of FIG. As seen from the robot 1, it is confirmed whether it is an ascending step or a descending step.

  The edge projection line estimation processing unit 13 executes the processing of STEPs 22 to 36 shown in FIG. 11 when the stepped portion existing in the imaging regions of the cameras 3R and 3L is the rising stepped portion, and the stepped portion of the descending step. If so, the processing of STEPs 37 to 51 shown in FIG. 12 is executed.

  In the case where the stepped portion is an ascending stepped portion, the edge projection line estimation processing unit 13 first, in STEP 22, in the reference image, an edge estimation region R3u that is an image processing region for estimating the actual edge projection line. Set.

  Here, in the following description, for convenience of explanation, it is assumed that the ascending step portion present in the imaging regions of the cameras 3R and 3L is, for example, the ascending staircase 50 as shown in FIG. It is assumed that the ascending staircase 50 is reflected in the reference image as shown in FIG.

  Further, the tip side edge 52 of the upper step surface 51 on the upper stage of one arbitrary lower step surface 51 of the ascending staircase 50 (for example, the lower step surface closest to the robot 1) is projected onto the reference image. Assume that the edge projection line estimation processing unit 13 executes processing for estimating the actual edge projection line L52r.

  Then, an upper tread surface 51 having a front end edge 52 corresponding to the actual edge projection line L52r to be estimated and a lower tread surface 51 below the first step are respectively denoted by reference numerals 51p and 51q. write.

  The planar projection area AR2 extracted by the planar projection area extraction unit 12 corresponding to the upper stage tread surface 51p is hereinafter referred to as an upper stage tread projection area AR2p using the reference symbol AR2p. Then, the plane parameter vector ↑ m2 determined corresponding to the upper side tread projection area AR2p is described using the reference symbol ↑ m2p.

  The planar projection area AR2 extracted by the planar projection area extraction unit 12 corresponding to the lower stage tread surface 51q is hereinafter referred to as a lower stage tread projection area AR2q using the reference symbol AR2q. Then, the plane parameter vector ↑ m2 determined corresponding to the lower stage tread projection area AR2q is described using the reference symbol ↑ m2q.

  In STEP 22, the edge estimation region R3u set in the reference image in STEP 22 is a rectangular region set as shown in FIG. Specifically, the edge estimation region R3u has a lower end position within a vertical width of the lower tread projection area AR2q (for example, a predetermined predetermined amount below the upper end of the lower tread projection area AR2q). And the position of the upper end of R3u is a position within the vertical width of the upper stage tread projection area AR2p (for example, a position above the lower end of the upper stage tread projection area AR2p by a predetermined amount). The In this case, the vertical width of the edge estimation region R3u is determined in accordance with the vertical interval between the lower stage tread projection area AR2q and the upper stage tread projection area AR2p.

  Further, the edge estimation region R3u is arranged such that its horizontal range (the range between the left end and the right end) matches the horizontal range of the lower tread projection area AR2q and the upper tread projection area AR2p. Is done. In this case, the lateral width of the edge estimation region R3u is determined to be the same width (default width) as the lateral width of the lower tread projection area AR2q and the upper tread projection area AR2p.

  The lateral width of the edge estimation region R3u may be slightly smaller than the lateral width of the lower stage tread projection area AR2q and the upper stage tread projection area AR2p. In this case, the edge estimation region R3u may be arranged so that the lateral range thereof falls within the lateral range of the lower stage tread projection area AR2q and the upper stage tread projection area AR2p.

  Through the processing of STEP 22 described above, the edge estimation region R3u has a single actual edge projection line L52r to be estimated (between the upper end side and the lower end side) in the inside thereof, and the edge The upper edge and the lower edge of the estimation area R3u are set in the reference image so as to be included in the upper tread projection area AR2p and the lower tread projection area AR2q, respectively.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP23. In STEP 23, the edge projection line estimation processing unit 13 assumes that the actual edge projection line L52r extends in the horizontal direction (u-axis direction) of the reference image, as shown in FIG. A plurality (N3a) of candidate positions v (k1) (k1 = 1, 2,..., N3a) are determined as candidates for the position of the actual edge projection line L52r in the (v-axis direction).

  In this case, N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) are determined in advance within the vertical range (range between the upper end and the lower end) of the edge estimation region R3u. The positions are set to different positions in the vertical direction of the reference image by predetermined intervals (for example, by one pixel). Of these candidate positions v (k1), the first candidate position v (1) as the uppermost candidate position is a predetermined interval (for example, an interval of one pixel) from the upper end of the edge estimation region R3u. Only the lower position is determined.

  Further, in this embodiment, the N3a-th candidate position v (N3a) as the lowermost candidate position is set when an upper end position of a projection transformation region R3u3 described later is set according to the candidate position v (N3a). Further, the upper end position of the projective transformation area R3u3 is positioned above the lower end of the edge estimation area R3u by a predetermined minimum width (for example, one pixel width) in the vertical direction of the edge estimation area R3u3. The edge estimation region R3u3 is determined in accordance with the upper end position in the minimum vertical width.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP24. In this process, “1” is set as the initial value of k1 in order to sequentially execute the processes corresponding to the candidate positions v (k1).

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP25. In STEP 25, the edge projection line estimation processing unit 13 matches the position of the actual edge projection line L52r in the vertical direction of the reference image with the k1th candidate position v (k1) (k1 is a currently set value). In the edge estimation region R3u, three projection transformation target regions R3u1, R3u2, and R3u3 are set in the edge estimation region R3u as illustrated in FIG.

  Among the projection transformation target regions R3u1 to R3u3, the projection transformation target regions R3u1 and R3u2 use the virtual edge projection line L52 (k1) extending in the horizontal direction at the k1th candidate position v (k1) as a boundary line. It is an area set to be adjacent vertically. In this case, the upper projection conversion target area R3u1 has an upper end side and a lower end side corresponding to the upper end side of the edge estimation area R3u, the virtual edge projection line L52 (k1), In addition, the horizontal range is a region that matches the horizontal range of the edge estimation region R3u.

  Accordingly, in the projection transformation target region R3u1, the vertical width and the position of the lower side edge (the vertical position) are the position of the virtual edge projection line L52 (k1) (= v (k1)). It is an area set so as to change according to. This projective transformation target area R3u1 is an area included in the upper tread projection area AR2p when the virtual edge projection line L52 (k1) matches the actual edge projection line L52r.

  Further, the lower projection transformation target area R3u2 has an upper end side that coincides with the virtual edge projection line L52 (k1) and a lower end side that coincides with the virtual edge projection line L52 (k1) in advance. It has a predetermined interval Δvu2 (for example, an interval of 15 pixels) and coincides with a line parallel to the edge projection line L52 (k1), and the horizontal range is the horizontal direction of the edge estimation region R3u It is an area that matches the range of.

  Accordingly, in the projective transformation target region R3u2, the position of the upper edge side and the position of the lower edge side hold the distance between them (that is, the vertical width of R3u2) at a constant distance Δvu2, This is an area set so as to change in accordance with the position (= v (k1)) of the virtual edge projection line L52 (k1). This projective transformation target region R3u2 is located on the upper side of the ascending staircase 50, such as the tip of the nose or the kicking surface, when the virtual edge projection line L52 (k1) matches the actual edge projection line L52r. This is an area on which a flat surface formed in a posture standing on the upper side tread surface 51p on the front end side of the tread surface 51p is projected.

  The projective transformation target region R3u3 is a rectangular region set below the edge transformation region R3u and below the projective transformation region R3u2. This projective transformation area R3u3 is an area in which the lower edge side coincides with the lower edge side of the edge estimation area R3u and the horizontal range coincides with the horizontal range of the edge estimation area R3u. is there.

  In the present embodiment, the position of the upper edge side of the projection transformation target region R3u3 is set as follows according to the position of the virtual edge projection line L52 (k1) (= v (k1)). .

  That is, the edge projection line estimation processing unit 13 first uses the plane parameter vector ↑ m2p corresponding to the upper stage tread projection area AR2p to use the tip of the upper stage tread 51p corresponding to the virtual edge projection line L52 (k1). A side edge line (a spatial straight line including the front end side edge 52) is obtained. The leading edge line is a plane parameter that includes the optical center C of the reference camera 3 and a virtual edge projection line L52 (k1) on the imaging surface of the reference image, and a plane parameter corresponding to the upper tread projection area AR2p. Calculated as a line of intersection with a plane defined by the vector ↑ m2p (a plane including the upper tread surface 51p).

  Then, the edge projection line estimation processing unit 13 includes a front-side edge line obtained as described above, and is a plane orthogonal to the plane defined by the plane parameter vector ↑ m2p corresponding to the upper tread surface projection area AR2p. (Or a plane that intersects the plane defined by ↑ m2p at a predetermined angle close to 90 degrees). This plane is a virtual plane (hereinafter sometimes referred to as an upright plane) corresponding to the front end surface or kick surface of the stair nosing.

  Further, the edge projection line estimation processing unit 13 projects an intersection line between the standing plane determined as described above and the plane defined by the plane parameter vector ↑ m2q corresponding to the lower tread projection area AR2q onto the reference image. The position of the upper end side is set so that the upper end side of the projection transformation target area R3u3 matches the line.

  Therefore, the upper end side of the projective transformation target region R3u3 is the base end when it is assumed that the base end side edge 53 of the lower step side tread surface 51q exists almost immediately below the front end side edge 52 of the upper step side tread surface 51p. This corresponds to a line formed by projecting the side edge 53 on the reference image. This projective transformation target area R3u3 is an area included in the lower tread projection area AR2q when the virtual edge projection line L52 (k1) matches the actual edge projection line L52r.

  When the plane projection area extraction unit 12 extracts a plane projection area corresponding to the planar portion of the step portion corresponding to the tip surface or the kick surface of the stair nosing, the plane projection is performed. The position of the upper end side of the projection transformation target region R3u3 may be determined using the plane defined by the plane parameter vector corresponding to the region as the standing plane.

  Supplementally, the flat surface portion that is in a posture of standing on the upper tread surface at the front end side of the upper tread surface of the step portion is in a step portion where no plate member is provided at the location of kicking, It corresponds to the tip of the nose. Accordingly, the vertical width of the flat portion is smaller than the distance between the upper step surface and the lower step surface. Further, in this case, there is no plane portion at the stepped portion of the stepped portion.

  Therefore, in the present embodiment, the vertical width Δvu2 of the projection transformation target region R3u2 is set to a relatively small width, and the lower end side of the projection transformation target region R3u2 and the upper end side of the lower projection transformation target region R3u3. An interval is provided between the two.

  When a portion corresponding to the front end surface of the nose is projected on the reference image, the vertical width of the nose image in the reference image is the distance from the optical center C of the reference camera 3 to the nose, or the reference camera. 3 changes in accordance with the azimuth angle of the nose with respect to the optical axis Lc (vertical azimuth angle of the reference image). Accordingly, the vertical width Δvu2 of the projection conversion target region R3u2 is a portion existing in the vicinity of the nose from the optical center C of the reference camera 3 (for example, on the upper step surface 51 corresponding to the lower end of the upper step projection region AR2p). It may be variably set according to the distance to the point), the azimuth angle of the portion with respect to the optical axis Lc of the reference camera 3, and the like. Further, the vertical width Δvu2 of the projective transformation target region R3u2 may be varied according to the resolution of the cameras 3R and 3L, the magnification of the captured image, and the like.

  After executing the processing of STEP25 as described above, the edge projection line estimation processing unit 13 then executes the processing of STEP26. In this STEP 26, assuming that each of the three projective transformation target regions R3u1, R3u2, and R3u3 set in the edge estimation region R3u is a projection region of a plane part, each of the projective transformation target regions R3u1, R3u2, and R3u3 Determine plane parameter vectors ↑ mu1, ↑ mu2, and ↑ mu3 for the plane including the corresponding plane portion.

  In this case, the plane parameter vectors ↑ mu1 and ↑ mu3 corresponding to the projective transformation target areas R3u1 and R3u3 respectively correspond to the plane parameter vector ↑ m2p and the lower stage tread projection area AR2q corresponding to the upper tread projection area AR2p. It is determined to be a vector that matches the plane parameter vector ↑ m2q.

  Further, the plane parameter vector ↑ mu2 corresponding to the projective transformation target region R3u2 includes the standing plane described with respect to the processing of STEP 25 (including the tip side edge line corresponding to the virtual edge projection line L52 (k1), and the upper tread surface To a plane parameter vector ↑ m that is orthogonal or nearly orthogonal to the plane including 51p.

  When the plane projection area extraction unit 12 extracts a plane projection area corresponding to the planar portion of the step portion corresponding to the tip surface or the kick surface of the stair nosing, the plane projection is performed. ↑ mu2 may be matched with the plane parameter vector ↑ m corresponding to the region.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP27. In this STEP 27, the edge projection line estimation processing unit 13 for each projection transformation target region R3u1, R3u2, R3u3, the image (reference image) of each projection transformation target region R3u1, R3u2, R3u3 and the reference image corresponding thereto. An error image representing an error from the projective transformation image obtained when projecting the image of the area is created.

  Error images corresponding to the respective projection transformation target areas R3u1, R3u2, and R3u3 are created as follows. For example, the error image creation process corresponding to the projection transformation target region R3u1 will be described as a representative example. The pixel value at an arbitrary position Q in the projection transformation target region R3u1 of the reference image is I1 (Q), and the position in the reference image. The position in the reference image obtained by performing the projective transformation by Q using the projective transformation matrix P of the equation (1-1) defined by the plane parameter vector ↑ mu1 corresponding to the projection transformation target region R3u1 is f (Q), and the reference image. The pixel value at the position f (Q) is I2 (f (Q)).

At this time, the error image corresponding to the projective transformation target region R3u1 has the pixel value at each position Q in the projective transformation target region R3u1 as the square value of the difference between I1 (Q) and I2 (f (Q)) (= ( I1 (Q) -I2 (f (Q))) ² ) or an absolute value (= | I1 (Q) -I2 (f (Q)) |).

  The same applies to the error images corresponding to the projective transformation target regions R3u2 and R3u3. However, in this case, the plane parameter vector used for the projective transformation for obtaining the position f (Q) in the reference image is a plane parameter vector ↑ mu2 determined for each region instead of ↑ mu1. ↑ mu3 is used.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEPs 28 and 29. In STEP 28, the edge projection line estimation processing unit 13 increases the number k1 of candidate positions v (k1) used for processing by “1”. In STEP 29, the edge projection line estimation processing unit 13 determines whether or not the increased value of k1 exceeds N3a, which is the total number of candidate positions v (k1).

  If the determination result in STEP 29 is negative, the edge projection line estimation processing unit 13 executes the processes in STEPs 25 to 27 using the increased k1th candidate position v (k1).

  As described above, the above error image is created corresponding to each of N3a candidate positions v (k1) (k1 = 1, 2,..., N3a).

  If the determination result in STEP 29 is affirmative, the edge projection line estimation processing unit 13 next executes the process in STEP 30. In this STEP 30, as shown in FIG. 15, the edge projection line estimation processing unit 13 has a plurality (N3b) of arithmetic processing regions R3min (k2) (k2 = 1, 2) elongated in the vertical direction in the edge estimation region R3u. ,..., N3b) are set. Each arithmetic processing region R3min (k2) has a predetermined minute width (for example, one pixel width) in which the horizontal width of the reference image is determined in advance, and the vertical range is an edge estimation region. This is a narrow rectangular (strip-shaped) region that matches the vertical range of R3u.

  In this embodiment, these arithmetic processing regions R3min (k2) are arranged so as to be adjacent to each other in order in the horizontal direction of the reference image, and the entire region thereof coincides with the edge estimation region R3u. Is done.

  Note that the arithmetic processing region R3min (k2) may be set to have a slight interval between adjacent arithmetic processing regions. Further, the width in the horizontal direction of each arithmetic processing region R3min (k2) may be slightly larger than the width of one pixel (for example, the width of two pixels, the width of three pixels, etc.).

  Supplementally, a combination of all of the calculation processing region R3min (k2) (k2 = 1, 2,..., N3b) matches the edge estimation region R3u. Therefore, in STEP 23, N3a candidate positions v ( In other words, the process of setting k1) (k1 = 1, 2,..., N3a) is N3a candidate positions v (k1) (k1 = 1,2, N2) for each arithmetic processing region R3min (k2). .., N3a).

  In STEP 25, the process of setting the projective transformation target areas R3u1, R3u2, and R3u3 according to v (k1), in other words, the projective transformation target areas R3u1, R3u2 for each arithmetic processing area R3min (k2). , R3u3.

  The processing of STEP 30 for setting the arithmetic processing region R3min (k2) does not need to be performed after STEP23 or STEP25. For example, the processing for setting the arithmetic processing region R3min (k2) may be performed in STPE22.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP31. In this processing, “1” is set as the initial value of k2 in order to sequentially execute the processing corresponding to each arithmetic processing region R3min (k2).

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP32. In this STEP 32, the edge projection line estimation processing unit 13 performs N3a candidate positions v (k1) (k1 = 1) in the k2th (k2 is the currently set number) arithmetic processing region R3min (k2). , 2,..., N3a), the value of the evaluation function Eu (k2) representing the degree of fitness of each candidate position v (k1) with respect to the position of the actual edge projection line L52r (position in the vertical direction) is calculated.

  Hereinafter, the evaluation function Eu (k2) representing the fitness of the k1th candidate position v (k1) in the k2th arithmetic processing region R3min (k2) is expressed as Eu (k2) _k1.

  The value of the evaluation function Eu (k2) _k1 corresponding to each candidate position v (k1) in the arithmetic processing region R3min (k2) is the projection transformation target region R3u1 (hereinafter referred to as R3u1) set according to v (k1). in accordance with the value of the error function Du1 (k2) _k1 calculated from the image of the portion included in the calculation processing region R3min (k2) of the error image corresponding to (k1)) and v (k1) The error function Du2 calculated from the image of the portion included in the calculation processing region R3min (k2) in the error image corresponding to the projection transformation target region R3u2 set in the above (hereinafter referred to as R3u2 (k1)). The calculation processing region R3min (k2) in the error image corresponding to the value of (k2) _k1 and the projection transformation target region R3u3 set in accordance with v (k1) (hereinafter referred to as R3u3 (k1)) ) Is linearly combined (added in this embodiment) with the value of the error function Du3 (k2) _k1 calculated from the image of the portion included in Calculated as a value.

  More specifically, the pixel values at each pixel position Q of the error image corresponding to each of the projection transformation target areas R3u1, R3u2, and R3u3 set according to v (k1) are respectively set as I1uerr (Q) and I2uerr ( Q) and I3uerr (Q). Further, the total number of pixels in the projection transformation target areas R3u1 (k1), R3u2 (k1), and R3u3 (k1) in the arithmetic processing area R3min (k2) is expressed as Nu1 (k2) _k1, Nu2 (k2) _k1, Nu3 ( k3) _k1. At this time, the error functions Du1 (k2) _k1, Du2 (k2) _k1, and Du3 (k2) _k1 are calculated by the following equations (3-1), (3-2), and (3-3), respectively. Then, the evaluation function Eu (k2) _k1 is calculated from the values of Du1 (k2) _k1, Du2 (k2) _k1, and Du3 (k2) _k1 by the following equation (4).

Du1 (k2) _k1 = (1 / Nu1 (k2) _k1) · ΣI1uerr (Q) (3-1)
Du2 (k2) _k1 = (1 / Nu2 (k2) _k1) · ΣI2uerr (Q) (3-2)
Du3 (k2) _k1 = (1 / Nu3 (k2) _k1) · ΣI3uerr (Q) (3-3)
Eu (k2) _k1 = Du1 (k2) _k1 + Du2 (k2) _k1 + Du3 (k2) _k1 (4)

  In this case, ΣI1uerr (Q) in equation (3-1) is the projection transformation target region R3u1 (k1) (calculation processing region R3min (k2) and projection transformation target region R3u1 (k1) in the computation processing region R3min (k2). The sum of the pixel values I1uerr (Q) of the error image in the overlapping region).

  In addition, ΣI2uerr (Q) in Expression (3-2) is the projection transformation target region R3u2 (k1) (calculation processing region R3min (k2) and projection transformation target region R3u2 (k1) in the computation processing region R3min (k2). This is the sum of the pixel values I2uerr (Q) of the error image in the overlap region.

  In addition, ΣI3uerr (Q) in Expression (3-3) is the projection transformation target region R3u3 (k1) (calculation processing region R3min (k2) and projection transformation target region R3u3 (k1) in the computation processing region R3min (k2). This is the sum of the pixel values I3uerr (Q) of the error image in the overlapping region.

  In STEP 32, the fitness (actual edge) of each of the N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) in the k2th arithmetic processing region R3min (k2) as described above. The values of N3a evaluation functions Eu (k2) _k1 (k1 = 1, 2,..., N3a) representing the degree of validity as the position of the projection line L52r are calculated. These evaluation functions Eu (k2) _k1 (k1 = 1, 2,..., N3a) are the position in the vertical direction of the reference image in the k2th arithmetic processing region R3min (k2) and the degree of fitness of the position. It has a meaning as data representing the relationship with (the degree of fitness for the position of the actual edge projection line L52r in the k2th arithmetic processing region R3min (k2)).

  Next, the edge projection line estimation processing unit 13 executes the processing of STEPs 33 and 34. In STEP 33, the edge projection line estimation processing unit 13 increases the number k2 of the calculation processing region R3min (k2) to be processed by “1”. In STEP 34, the edge projection line estimation processing unit 13 determines whether or not the increased k2 value has exceeded N3b, which is the total number of calculation processing regions R3min (k2).

  If the determination result in STEP 34 is negative, the edge projection line estimation processing unit 13 executes the process in STEP 32 for the k2th arithmetic processing region R3min (k2) after the increase.

  As described above, each of N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) and N3b operation processing regions R3min (k2) (k2 = 1, 2,..., N3b). ), A total of N3a × N3b evaluation function Eu (k2) _k1 values are calculated.

  Thereby, as illustrated in FIGS. 16A and 16B, a plurality of candidate positions for each predetermined interval in the vertical direction (v-axis direction) of the reference image for each arithmetic processing region R3min (k2). v (k1) (k1 = 1, 2,..., N3a) and the fitness of each candidate position v (k1) (relative to the position of the actual edge projection line L52r in the k2th arithmetic processing region R3min (k2)) Data representing the relationship between the value of the evaluation function Eu (k2) _k1 representing the degree of fitness) is obtained.

  16A and 16B are graphs of the evaluation function Eu (1) relating to the first arithmetic processing region R3min (1), and the evaluation function relating to the N3bth arithmetic processing region R3min (N3b), respectively. A graph of Eu (N3b) is representatively shown.

  If the determination result in STEP 34 is affirmative, the edge projection line estimation processing unit 13 next executes the process in STEP 35. In STEP 35, the edge projection line estimation processing unit 13 virtually sets a plurality of estimated edge projection line candidates (hereinafter referred to as edge projection line candidates) in the edge estimation region R3u. Then, the edge projection line estimation processing unit 13 evaluates Eu (k2) for each of the N3b calculation processing regions R3min (k2) (k2 = 1, 2,..., N3b) for each edge projection line candidate. Is obtained as a total sum TEu (hereinafter referred to as a comprehensive evaluation function TEu).

  The plurality of edge projection line candidates virtually set by the edge projection line estimation processing unit 13 in the processing of STEP 35 are edge projection lines having a plurality of types of positions and directions. Specifically, these edge projection line candidates are set as follows, for example.

  That is, referring to FIG. 17, the edge projection line estimation processing unit 13 has the same number of left and right edges of the edge estimation region R3u as regions obtained by combining N3b calculation processing regions R3min (k2). Set multiple points at a time. For example, N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) and N3a points are set at equal intervals on the left and right sides of the edge estimation region R3u. To do.

  Then, the edge projection line estimation processing unit 13 sets a total of N3a × N3a straight lines connecting the set points on the left end of the edge estimation region R3u and the set points on the right end as edge projection line candidates.

  The edge projection line candidates shown in FIG. 17 representatively show a part of the edge projection line candidates set in this way.

  Supplementally, the number of points set on the left and right sides of the edge estimation region R3u may not be the same as the number N3a of candidate positions v (k1), for example, a number smaller than N3a. There may be. The positions of the points set in the left and right edges of the edge estimation region R3u (positions in the vertical direction) are positions within the range between v (1) and v (N3a). If present, the position may be different from the candidate position v (k1).

  Further, a plurality of edge projection line candidates may be set by a combination of the position of one point on the reference image through which the edge projection line candidate passes and the direction (tilt angle) of the edge projection line candidate.

  The value of the comprehensive evaluation function TEu corresponding to each edge projection line candidate set as described above is calculated as follows.

  That is, the edge projection line estimation processing unit 13 first determines, for each edge projection line candidate, each of the N3b calculation processing regions R3min (k2) (k2 = 1, 2,..., N3b). The position in the vertical direction of the reference image (the position of the edge projection line candidate in each calculation processing region R3min (k2)) is calculated.

  Then, the edge projection line estimation processing unit 13 calculates the value of the evaluation function Eu (k2) in each calculation processing region R3min (k2) for each of the edge projection line candidates, as calculated in the calculation processing region R3min ( It is determined based on the position of the edge candidate line candidate at k2) and the N3a evaluation functions Eu (k2) _1 to Eu (k2) _N3a calculated by the processing of STEP 32 in the calculation processing region R3min (k2). .

  Specifically, for any one edge projection line candidate, for example, the position (position in the v-axis direction) of the edge projection line candidate in the k2th computation processing region R3min (k2) is vx, and N3a pieces Of the candidate positions v (1) to v (N3a) is v (k1x). In this case, the value of the evaluation function Eu (k2) _k1x calculated corresponding to the k2th arithmetic processing region R3min (k2) and the candidate position v (k1x) is the k2th arithmetic processing region R3min (k2). ) At the edge projection line candidate evaluation function Eu (k2).

  If there is no candidate position matching vx, for example, the candidate position closest to vx is defined as v (k1x), and the edge projection line candidate is evaluated in the k2th arithmetic processing region R3min (k2). The value of the function Eu (k2) is determined.

  Alternatively, the value of the evaluation function Eu (k2) corresponding to vx is calculated from the value of the evaluation function Eu (k2) corresponding to each of two or more candidate positions before and after vx by interpolation processing such as linear interpolation. You may do it. In this case, the value of the calculated evaluation function Eu (k2) is determined as the value of the evaluation function Eu (k2) of the edge projection line candidate in the k2th arithmetic processing region R3min (k2). You just have to do it.

  Next, the edge projection line estimation processing unit 13 positions the edge projection line candidates in each of the N3b calculation processing regions R3min (k2) (k2 = 1, 2,..., N3b) for each edge projection line candidate. The value of the comprehensive evaluation function TEu corresponding to the edge projection line candidate is calculated by adding together the values of the evaluation functions Eu (1) to Eu (N3b).

  After executing the processing of STEP 35 as described above, the edge projection line estimation processing unit 13 then executes the processing of STEP 36. In STEP 36, the edge projection line estimation processing unit 13 determines an estimated edge projection line as a line obtained by estimating the actual edge projection line L52r based on the comprehensive evaluation function TEu for each edge projection line candidate calculated in STEP 35. .

  Here, when one edge projection line candidate matches the actual edge projection line L52r, in principle, an evaluation function Eu (k2) relating to the position of the edge projection line candidate in each arithmetic processing region R3min (k2). ) Is the minimum (the value closest to “0”). Therefore, the value of the comprehensive evaluation function TEu corresponding to the edge projection line candidate is smaller than the value of the comprehensive evaluation function TEu corresponding to other edge projection line candidates.

  Therefore, in STEP 36, the edge projection line estimation processing unit 13 sets the edge projection line candidate having the minimum value of the corresponding comprehensive evaluation function TEu among all the edge projection line candidates set in STEP 35 as the estimated edge projection line. decide.

  Thus, for example, as illustrated in FIG. 18, the point at the position v (k1a) on the left end side of the edge estimation region R3u and the position v (k1b) on the right end side (in the example shown, v (k1b) = Edge projection line candidates connecting the points of v (k1a)) are determined as estimated edge projection lines.

  The above is the processing of the edge projection line estimation processing unit 13 executed when an ascending step portion (ascending staircase 50) exists in the imaging regions of the cameras 3R and 3L.

  Next, the processing of the edge projection line estimation processing unit 13 (the processing of STEPs 37 to 51 in FIG. 12) executed when there is a stepped portion (for example, the descending staircase 50) in the imaging region of the cameras 3R and 3L will be described. To do.

  When the stepped portion is a descending stepped portion, the edge projection line estimation processing unit 13 is an image processing region for estimating an actual edge projection line in STEP 37 as in STEP 22, first, in the reference image. An edge estimation region R3d is set.

  Here, in the following description, for the sake of convenience of explanation, it is assumed that the descending step portion present in the imaging regions of the cameras 3R and 3L is the descending staircase 50 as shown in FIG. Further, an actual edge projection line L52r formed by projecting the tip side edge 52 of any one upper step surface 51 of the descending staircase 50 (for example, the upper step surface closest to the robot 1) onto the reference image is estimated. It is assumed that the edge projection line estimation processing unit 13 executes the processing to be performed.

  Then, similarly to the description of the ascending stepped portion, an upper stage tread 51 having a tip side edge 52 corresponding to the actual edge projection line L52r to be estimated at the tip, and a lower stage tread 51 below the first stage, Represented using reference numerals 51p and 51q, respectively.

  The edge estimation region R3d set as the reference image in STEP 37 is a rectangular region set as shown in FIG. The edge estimation region R3d has a lower end position within a vertical width of the upper stage tread projection area AR2p (for example, a position below the upper end of the upper stage tread projection area AR2p by a predetermined amount), Further, the position of the upper end of R3d is arranged so as to be a position within the vertical width of the lower stage tread projection area AR2q (for example, a position above the lower end of the lower stage tread projection area AR2q by a predetermined amount). In this case, the vertical width of the edge estimation region R3d is determined in accordance with the vertical interval between the lower stage tread projection area ARq and the upper stage tread projection area ARp.

  In addition, the edge estimation region R3d is arranged such that its horizontal range (the range between the left end and the right end) coincides with the horizontal range of the lower-stage tread projection area AR2q and the upper-stage tread projection area AR2p. Is done. In this case, the lateral width of the edge estimation region R3d is determined to be the same width (predetermined width) as the lateral width of the lower tread projection area AR2q and the upper tread projection area AR2p.

  The lateral width of the edge estimation region R3d may be slightly smaller than the lateral width of the lower stage tread projection area AR2q and the upper stage tread projection area AR2p. In that case, the edge estimation region R3d may be arranged so that the lateral range thereof falls within the lateral range of the lower stage tread projection area AR2q and the upper stage tread projection area AR2p.

  Through the processing of STEP 37 described above, the edge estimation region R3d has a single actual edge projection line L52r to be estimated (between the upper end side and the lower end side) inside the edge estimation region R3d, and the edge The upper edge and the lower edge of the estimation area R3d are set in the reference image so as to be included in the lower tread projection area AR2q and the upper tread projection area AR2p, respectively.

  The edge projection line estimation processing unit 13 executes the processing of STEP38 after STEP37. In this STEP 38, the edge projection line estimation processing unit 13 performs processing similar to that in STEP 23, as shown in FIG. 20, a plurality of candidates for the position of the actual edge projection line L52r in the vertical direction (v-axis direction) of the reference image. (N3a) candidate positions v (k1) (k1 = 1, 2,..., N3a) are determined.

  In this case, the number N3a of candidate positions v (k1) is generally different from the number N3a in the processing of STEP23.

  In this case, N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) are within the vertical range (range between the upper end and the lower end) of the edge estimation region R3d. The positions are different in the vertical direction of the reference image by predetermined intervals (for example, by one pixel). Of these candidate positions v (k1), the first candidate position v (1) as the uppermost candidate position is a predetermined interval (for example, one pixel interval) from the upper end of the edge estimation region R3d. And the N3a-th candidate position v (N3a) as the lowermost candidate position is determined to be an upper position by a predetermined interval (for example, an interval of one pixel) from the lower end of the edge estimation region R3d. Is done.

  Next, the edge projection line estimation process part 13 performs the process of STEP39. This process is the same as the process of STEP 24, and “1” is set as the initial value of k1.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP40. In STEP 40, the edge projection line estimation processing unit 13 matches the position of the actual edge projection line L52r in the vertical direction of the reference image with the k1th candidate position v (k1) (k1 is a currently set value). Assuming that this is done, two projection transformation target regions R3d1 and R3d2 are set in the edge estimation region R3d as illustrated in FIG.

  When the stepped portion present in the imaging regions of the cameras 3R and 3L is a stepped portion that is descending (here, the descending staircase 50), the tip end surface and the kicking surface of the stepped nose of the stepped portion are included in the reference image and the reference image. Is not shown. For this reason, in this case, there are two projective transformation target areas to be set in the edge estimation area R3d, namely R3d1 and R3d2.

  More specifically, these two projective transformation target regions R3d1 and R3d2 are vertically arranged with a virtual edge projection line L52 (k1) extending in the horizontal direction at the k1th candidate position v (k1) as a boundary line. This is an area set to be adjacent. In this case, the upper projection conversion target region R3d1 has an upper end side and a lower end side that coincide with the upper end side of the edge estimation region R3d, the virtual edge projection line L52 (k1), In addition, the horizontal range is a region that matches the horizontal range of the edge estimation region R3d.

  Therefore, in the projection transformation target region R3d1, the vertical width and the position of the lower side edge (the vertical position) are the positions of the virtual edge projection line L52 (k1) (= v (k1)). It is an area set so as to change according to. This projection transformation target area R3d1 is an area included in the lower tread projection area AR2q when the virtual edge projection line L52 (k1) matches the actual edge projection line L52r.

  The lower projection transformation target area R3d2 has an upper edge side and a lower edge side that correspond to the virtual edge projection line L52 (k1) and the lower edge side of the edge estimation area R3d, respectively. In addition, the horizontal range is a region that matches the horizontal range of the edge estimation region R3d.

  Accordingly, in the projection transformation target area R3d2, the vertical width and the position of the upper side edge (the vertical position) are the positions of the virtual edge projection line L52 (k1) (= v (k1)). It is an area set so as to change according to. This projective transformation target area R3d2 is an area included in the upper tread projection area AR2p when the virtual edge projection line L52 (k1) matches the actual edge projection line L52r.

  After executing the processing of STEP 40 as described above, the edge projection line estimation processing unit 13 then executes the processing of STEP 41. In this STEP 41, it is assumed that each of the two projection transformation target areas R3d1, R3d2 set in the edge estimation area R3d is a projection area of the plane part, and the plane portions corresponding to the projection transformation target areas R3d1, R3d2 respectively. Plane parameter vectors ↑ md1 and ↑ md2 are determined for the plane that includes.

  In this case, the plane parameter vectors ↑ md1 and ↑ md2 respectively corresponding to the projective transformation target areas R3d1 and R3d2 correspond to the plane parameter vector ↑ m2q and the upper stage tread projection area AR2p corresponding to the lower tread projection area AR2q, respectively. The plane parameter vector is determined to be a vector matching the m2p.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP42. In this STEP 42, the edge projection line estimation processing unit 13 for each projection transformation target region R3d1, R3d2, images of the projection transformation target regions R3d1, R3d2 and images of the corresponding regions in the reference image. An error image representing an error from the projective transformation image obtained when projective transformation is performed is created.

  In this case, the error image corresponding to each of the projection transformation target regions R3d1, R3d2 is created by the same process as the error image creation process in STEP 27, using the corresponding plane parameter vectors ↑ md1, ↑ md2. Is done.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEPs 43 and 44. In STEP 43, the edge projection line estimation processing unit 13 increases the number k1 of the candidate position v (k1) used for processing by “1”. In STEP 44, the edge projection line estimation processing unit 13 determines whether or not the increased k1 value exceeds N3a, which is the total number of candidate positions v (k1).

  If the determination result in STEP 44 is negative, the edge projection line estimation processing unit 13 executes the processes in STEP 40 to 42 using the k1th candidate position v (k1) after the increase.

  As described above, the above error image is created corresponding to each of N3a candidate positions v (k1) (k1 = 1, 2,..., N3a).

  If the determination result in STEP 44 is affirmative, the edge projection line estimation processing unit 13 next executes the process in STEP 45. In STEP 45, as shown in FIG. 21, the edge projection line estimation processing unit 13 includes a plurality of (N3b) arithmetic processing regions R3min (k2) (k2 = 1, 2) elongated in the vertical direction in the edge estimation region R3d. ,..., N3b) are set. Each arithmetic processing region R3min (k2) (k2 = 1, 2,..., N3b) is a narrow rectangular region (strip shape) similar to that described in connection with the processing of STEP 30. The setting method is the same as in STEP 30.

  Note that the number N3b of the arithmetic processing areas R3min (k2) may not be the same as the number of arithmetic processing areas R3min (k2) set in STEP 30.

  Supplementally, the processing for setting N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) in STEP38 is the same as that described for STEP30. It can be said that this is a process of setting N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) for the region R3min (k2).

  In STEP 40, the process of setting the projective transformation target areas R3d1 and R3d2 according to v (k1), in other words, the projective transformation target areas R3d1 and R3d2 are set for each arithmetic processing area R3min (k2). It can be said that it is processing to do.

  The processing of STEP 45 does not need to be performed after STEP 38 or STEP 40. For example, in STEP 37, setting processing of the arithmetic processing region R3min (k2) may be performed.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP46. In this process, “1” is set as the initial value of k2.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEP 47. In STEP 47, the edge projection line estimation processing unit 13 determines the N3a candidate positions v (k1) (k1 = 1) in the k2th (k2 is the currently set number) arithmetic processing region R3min (k2). , 2,..., N3a), the value of the evaluation function Ed (k2) representing the suitability of each candidate position v (k1) with respect to the position of the actual edge projection line L52r (position in the vertical direction) is determined in STEP32. It is calculated by the same process as in the case of the process.

  However, in this case, since there are two projection transformation target areas R3d1 and R3d2 set in the edge estimation area R3d, the evaluation function Ed corresponding to each candidate position v (k1) in the arithmetic processing area R3min (k2). The value of (k2) _k1 is calculated as follows.

  That is, the value of the evaluation function Ed (k2) _k1 corresponding to each candidate position v (k1) in the arithmetic processing region R3min (k2) is the projection transformation target region R3d1 (hereinafter referred to as this) set according to v (k1). Of the error image corresponding to R3d1 (k1)), the value of the error function Dd1 (k2) _k1 calculated from the image of the portion included in the calculation processing region R3min (k2), and v (k1) The error calculated from the image of the portion included in the calculation processing region R3min (k2) in the error image corresponding to the projection transformation target region R3d2 (hereinafter referred to as R3d2 (k1)) set according to It is calculated as a value obtained by linearly combining (adding in this embodiment) the value of the function Dd2 (k2) _k1.

  More specifically, the pixel values at the respective pixel positions Q of the error image corresponding to each of the projection transformation target areas R3d1 and R3d2 set according to v (k1) are respectively set as I1derr (Q), I2derr (Q), I3derr (Q). Further, the total number of pixels in the projection transformation target areas R3d1 (k1) and R3d2 (k1) in the arithmetic processing area R3min (k2) is Nd1 (k2) _k1 and Nd2 (k2) _k1. At this time, error functions Dd1 (k2) _k1 and Dd2 (k2) _k1 are calculated by the following equations (5-1) and (5-2), respectively. Then, the evaluation function Ed (k2) _k1 is calculated from the values of Dd1 (k2) _k1 and Dd2 (k2) _k1 by the following equation (6).

Dd1 (k2) _k1 = (1 / Nd1 (k2) _k1) · ΣI1derr (Q) (5-1)
Dd2 (k2) _k1 = (1 / Nd2 (k2) _k1) · ΣI2derr (Q) (5-2)
Ed (k2) _k1 = Dd1 (k2) _k1 + Dd2 (k2) _k1 (6)

  In this case, ΣI1derr (Q) in equation (5-1) is the projection transformation target region R3d1 (k1) (calculation processing region R3min (k2) and projection transformation target region R3d1 (k1) in the computation processing region R3min (k2). The sum of the pixel values I1derr (Q) of the error image in the overlapping region).

  In addition, ΣI2derr (Q) in Expression (3-2) is the projection transformation target region R3u2 (k1) (the computation processing region R3min (k2) and the projection transformation target region R3u2 (k1) in the computation processing region R3min (k2). The sum of the pixel values I2derr (Q) of the error image in the overlapping region).

  In STEP 47, as described above, the fitness (actual edge) of each of the N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) in the k2th arithmetic processing region R3min (k2). The values of N3a evaluation functions Ed (k2) _k1 (k1 = 1, 2,..., N3a) each representing the degree of validity as the position of the projection line L52r are calculated.

  Next, the edge projection line estimation processing unit 13 executes the processing of STEPs 48 and 49. In STEP 48, the edge projection line estimation processing unit 13 increases the number k2 of the calculation processing region R3min (k2) to be processed by “1”. In STEP 49, the edge projection line estimation processing unit 13 determines whether or not the increased k2 value has exceeded N3b, which is the total number of calculation processing regions R3min (k2).

  If the determination result in STEP 49 is negative, the edge projection line estimation processing unit 13 executes the processing in STEP 47 for the k2th arithmetic processing region R3min (k2) after the increase.

  As described above, each of N3a candidate positions v (k1) (k1 = 1, 2,..., N3a) and N3b operation processing regions R3min (k2) (k2 = 1, 2,..., N3b). ), A total of N3a × N3b evaluation function Ed (k2) _k1 values are calculated.

  Thus, for each arithmetic processing region R3min (k2), a plurality of candidate positions v (k1) (k1 = 1, 2,..., N3a at predetermined intervals in the vertical direction (v-axis direction) of the reference image. ) And the value of the evaluation function Ed (k2) _k1 representing the fitness of each candidate position v (k1) (the fitness of the actual edge projection line L52r in the k2th arithmetic processing region R3min (k2)) Data representing the relationship between the two will be obtained.

  If the determination result in STEP 49 is affirmative, the edge projection line estimation processing unit 13 next executes the process in STEP 50. In STEP 50, the edge projection line estimation processing unit 13 virtually sets a plurality of edge projection line candidates in the edge estimation region R3d, similarly to the processing in STEP 35. Then, the edge projection line estimation processing unit 13 evaluates the evaluation function Ed (k2) in each of the N3b arithmetic processing regions R3min (k2) (k2 = 1, 2,..., N3b) for each edge projection line candidate. A comprehensive evaluation function TEd that is the sum of the values of is obtained.

  The processing for setting a plurality of edge projection line candidates in STEP 50 and the processing for calculating the value of the comprehensive evaluation function TEd corresponding to each edge projection line candidate are performed by the same processing as in STEP 35.

  Next, the edge projection line estimation process part 13 performs the process of STEP51. The processing of STEP 51 is performed in the same manner as the processing of STEP 36. That is, the edge projection line estimation processing unit 13 determines an edge projection line candidate having a minimum value of the corresponding comprehensive evaluation function TEd among all the edge projection line candidates set in STEP 50 as an estimated edge projection line.

  The above is the processing of the edge projection line estimation processing unit 13 that is executed when the stepped portion (the descending staircase 50) exists in the imaging regions of the cameras 3R and 3L.

  When there is a stepped portion in the imaging area of the cameras 3R and 3L by the processing of the edge projection line estimation processing unit 13 described above, the stepped portion is an ascending stepped portion and a descending stepped portion as viewed from the robot 1. In any case, an actual edge projection line formed by projecting the leading edge of the upper tread surface of the stepped portion onto the reference image is estimated.

  In the description of the processing of the edge projection line estimation processing unit 13 described above, an estimated edge projection line corresponding to one arbitrary front end edge 52 is obtained by the processing of STEPs 22 to 36 or the processing of STEPs 37 to 51. However, the processing of STEPs 22 to 36 or the processing of STEPs 37 to 51 is repeated for each front edge 52 of each upper tread surface 51 projected on the reference image, so that each of the images shown in the reference image is repeated. Estimated edge projection lines corresponding to the leading edge 52 can be respectively obtained.

  When the processing of the edge projection line estimation processing unit 13 is completed, the arithmetic processing device 10 next executes the processing of the stepped portion space arrangement determining unit 14.

  The stepped portion space arrangement determining unit 14 includes the plane parameter vector ↑ m2 of each plane projection area AR2 extracted by the process of the plane projection area extraction unit 12 and the estimated edge obtained by the process of the edge projection line estimation processing unit 13. Using the projection line, a spatial arrangement position (arrangement position viewed in the robot coordinate system) of the stepped portion imaged by the cameras 3R and 3L is determined.

  In this case, in this embodiment, the stepped portion space arrangement determining unit 14 uses, as data indicating the spatial arrangement of the stepped portion, for example, each upper step side tread of the stepped portion in the spatial advance planned area of the robot 1. The spatial arrangement position of the front end side edge and the base end side edge of each lower stage tread and the spatial posture (normal direction) of each tread are determined.

  These data are the estimated edge projection line determined on the reference image, the plane parameter vector ↑ m2 (↑ m2p) of the upper tread surface having the leading edge corresponding to the estimated edge projection line, and the upper tread surface It is determined using the plane parameter vector ↑ m2 (↑ m2q) of the lower tread on the lower stage.

  Specifically, the stepped portion space arrangement determining unit 14 sets the estimated edge projection line in the reference image to a plane defined by the plane parameter vector ↑ m2p of the upper tread surface having the tip side edge corresponding to the estimated edge projection line. The line formed by back projection and the line that matches the leading edge are determined. The line is determined as a line on the intersection line between the plane including the estimated edge projection line on the imaging plane of the reference image and the optical center C of the reference camera 3 and the plane defined by the plane parameter vector ↑ m2p.

  Further, the stepped portion space arrangement determining unit 14 projects the tip side edge whose spatial arrangement has been determined as described above onto a plane defined by the plane parameter vector ↑ m2q of the lower step side tread in a predetermined direction. Is determined as the base end edge of the lower tread surface. In this case, the predetermined direction is specifically the direction orthogonal to the plane defined by the plane parameter vector ↑ m2p of the upper tread surface having the tip side edge, or the plane defined by ↑ m2p. The direction intersects at a predetermined angle close to 90 degrees.

  When the plane projection area extraction unit 12 extracts a plane projection area corresponding to the planar portion of the step portion corresponding to the tip surface or the kick surface of the stair nosing, the plane projection is performed. The predetermined direction is determined so that the normal direction of the plane including the distal end edge and the proximal end edge coincides with or substantially coincides with the normal direction of the plane defined by the plane parameter vector corresponding to the region. May be.

  In addition, the step space arrangement determination unit 14 determines the postures (normal directions) of the upper step surface and the lower step surface of the step portion in directions that match the directions of the plane parameter vectors ↑ m2p and ↑ m2q, respectively. In general, the vertical direction is not necessarily determined).

  As described above, the spatial arrangement positions of the leading edge of each upper stage tread and the base end edge of each lower stage tread of the stepped portion, and the spatial posture (normal direction) of each tread are determined.

  The robot 1 recognizes the position of the stepped portion relative to the robot 1 and the shape of the stepped portion with respect to the robot 1 and performs operations such as raising and lowering the stepped portion.

  According to the present embodiment described above, a plurality of arithmetic processing regions R3min (k2) (k2 = 1, 2,..., N3b) having a small width in the horizontal direction and elongated in the vertical direction are formed in the reference image in the horizontal direction. It is set to line up. Then, for each arithmetic processing area, each of the plurality of candidate positions v (k1) (k1 = 1, 2,..., N3a) in the vertical direction of the reference image is matched to the position of the actual edge projection line L52r. The evaluation function Eu (k2) _k1 or Ed (k2) _k1 representing the degree) is calculated by an arithmetic process using projective transformation.

  Thereby, in each of the plurality of calculation processing regions R3min (k2) (k2 = 1, 2,..., N3b) arranged in the horizontal direction, the vertical position and the evaluation function Eu (k2) _k1 or Ed (k2) _k1 Data representing the relationship with the value of is obtained.

  Further, a plurality of edge projection line candidates whose positions and directions are different from each other are virtually set in the edge estimation region R3u or R3d, and each of the edge projection line candidates is set in each calculation processing region R3min (k2). The value of the comprehensive evaluation function TEu or TEd obtained by synthesizing the value of the evaluation function Eu (k2) _k1 or Ed (k2) _k1 at the position of is calculated. This comprehensive evaluation function TEu or TEd is the inverse of the combined fitness obtained by synthesizing the fitness of the position of the edge projection line candidate in each arithmetic processing region R3min (k2) (the fitness of the actual edge projection line L52r). It corresponds to a numerical value.

  Then, the edge projection line candidate that minimizes the value of the comprehensive evaluation function TEu or TEd (in other words, the edge projection line candidate that maximizes the combined fitness) is determined as the estimated edge projection line.

  By determining the estimated edge projection line in this way, it is possible to determine an estimated edge projection line with high reliability as an estimation of the actual edge projection line.

  In particular, when an ascending step portion such as ascending staircase 50 exists in the imaging regions of the cameras 3R and 3L, edge estimation is performed on three projective transformation target regions R3u1, R3u2, and R3u1 depending on the candidate position v (k1). Since it is set in the operation area R3u as described above (and thus set in each arithmetic processing area R3min (k2)), it is adjacent to the value of each evaluation function Eu (k2) _k1 with the position of the candidate position v (k1) as a boundary. It is possible to reflect not only the projection conversion target areas R3u1 and R3u2 to be processed but also the pixel values of the image of the projection conversion target area R3u3 corresponding to the lower tread. That is, the value of each evaluation function Eu (k2) _k1 can be calculated by reflecting the pixel values of many pixels included in each calculation processing region R3min (k2).

  Therefore, it is possible to improve the reliability of the value of each evaluation function Eu (k2) _k1 representing the degree of suitability of the position of the candidate position v (k1) in each arithmetic processing region R3min (k2). As a result, the reliability of the estimated edge projection line can be improved.

  In the present embodiment, the stepped portion presence detection unit 11 and the planar projection region extraction unit 12 are provided with a distance measuring device such as a laser distance measuring device in the robot 1 by processing using a stereo image and projective transformation. In addition, it is possible to detect the presence of a stepped portion and to extract a planar projection region. For this reason, the stepped portion can be recognized with an inexpensive configuration.

  Here, the correspondence relationship between the embodiment described above and the present invention will be supplemented.

  In the present embodiment, the stepped portion presence detecting means in the present invention is realized by the processing of the stepped portion presence detecting unit 11. In this case, the stepped portion presence detection unit 11 includes a function as the stepped portion type determining unit in the present invention, and the stepped portion type determining unit is realized by the processing of STEP4.

  Further, the plane projection area extraction means in the present invention is realized by the processing of the plane projection area extraction unit 12. In this case, the local region R2 (i) (i = 1, 2,..., N2) corresponds to the planar projection region extracting local region in the present invention.

  Further, the evaluation function calculation means and the estimated edge projection line determination means in the present invention are realized by the processing of the edge projection line estimation processing unit 13. More specifically, an evaluation function calculating unit is realized by the processes of STEPs 22 to 34 and STEPs 37 to 49, and an estimated edge projection line determining unit is realized by the processes of STEPs 35 and 36 and STEPs 50 and 51.

  In this case, the projective transformation target regions R3u1, R3u2, and R3u3 are respectively the first edge estimation projective transformation target region, the second edge estimation projective transformation target region, and the third edge estimation projective transformation target. The projection conversion target areas R3d1 and R3d2 correspond to the fourth edge estimation projection conversion target area and the fifth edge estimation projection conversion target area, respectively. The plane parameter vectors ↑ mu1, ↑ mu2, ↑ mu3, ↑ md1, and ↑ md2 correspond to the projective transformation plane parameters in the present invention.

  Further, the stepped portion space arrangement determining means in the present invention is realized by the processing of the stepped portion space arrangement determining unit 14.

  Next, some modifications of the embodiment described above will be described.

  In the embodiment, the two-legged mobile robot 1 is exemplified as the moving body, but the moving body may be a robot having three or more legs, or a moving body including wheels and crawlers.

  In the embodiment, the camera 3R out of the cameras 3R and 3L is used as the reference camera 3. However, the camera 3L may be used as the reference camera.

  Further, in the above embodiment, when there are ascending steps in the imaging areas of the cameras 3R and 3L, the three projection transformation target areas R3u1, R3u2, and R3u3 depending on the candidate position v (k1) are used as the edge estimation area. Although R3u is set, only the two projection transformation target areas R3u1 and R3u2 are set as the edge estimation area R3u (and thus set in each calculation processing area R3min (k2)). Good.

  In the embodiment, the plurality of candidate positions in each calculation processing region R3min (k2) are the same for all the calculation processing regions R3min (k2). However, the candidate positions for each calculation processing region R3min (k2) are determined. You may make it differ.

  In the embodiment, in the processing of the planar projection area extraction unit 12, the positions of the plurality of local areas R2 (i) (i = 1, 2,. However, a plurality of local regions R2 (i) may be arranged in the vertical direction at each of a plurality of positions in the horizontal direction of the reference image.

  In such a case, the change in the distribution of the planar portion in the vertical direction can be grasped at a plurality of positions in the horizontal direction of the reference image. For this reason, when each tread of the stepped portion existing in the imaging area of the cameras 3R and 3L is relatively largely inclined with respect to the lateral direction of the reference image, each step of the stepped portion is reflected to reflect the inclination. A planar projection area corresponding to a planar portion such as a tread can be extracted. Therefore, it is possible to extract a planar projection region having a shape in which the upper and lower sides are inclined with respect to the horizontal direction of the reference image in accordance with the inclination of the planar portion.

  DESCRIPTION OF SYMBOLS 1 ... Legged mobile robot (moving body), 3R, 3L ... Camera (stereo camera), 4 ... Projector, 11 ... Step part presence detection part (step part presence detection means, step part type judgment means), 12 ... Planar projection Area extraction unit (planar projection area extraction unit), 13 ... edge projection line estimation processing unit (evaluation function calculation unit, estimated edge projection line determination unit), 14 ... stepped part space arrangement determining unit (stepped part space arrangement determining unit), STEP4: Stepped portion type determining means, STEP22 to 34, STEP37 to 49 ... Evaluation function calculating means, STEP35, 36, 50, 51 ... Estimated edge projection line determining means, 50 ... Stair (stepped portion), 51 ... Tread, 52 ... Tip side edge.

Claims (8)

It exists in the moving environment of a moving body equipped with a stereo camera, and the stereo camera acquires the spatial arrangement position of the stepped part that has one or more pairs of the lower tread and the upper tread above it A step difference recognition device that recognizes using a standard image and a reference image constituting a stereo image,
In a state where the stepped portion is imaged by the stereo camera, different plane portions including at least two plane portions respectively corresponding to the upper step surface and the lower step surface of the step portion are projected from the reference image. A plane projection area extracting means for extracting a plurality of plane projection areas, which are areas, and determining plane parameters representing a spatial position and orientation of a plane including a plane portion corresponding to each plane projection area;
Each of which includes an actual edge projection line formed by projecting the leading edge of the upper tread surface of the stepped portion onto the reference image, and has a shape that is elongated in the vertical direction of the reference image in the horizontal direction of the reference image. A plurality of parallel arithmetic processing areas are set in the reference image, and a plurality of candidate positions of the actual edge projection line in the vertical direction of the reference image in each of the multiple arithmetic processing areas are set, Evaluation function calculation that calculates the value of the evaluation function representing the degree of suitability of each candidate position with respect to the position of the actual edge projection line for each arithmetic processing region using the reference image and the reference image and the determined plane parameter Means,
The combined adaptability obtained by combining the estimated edge projection line, which is a line obtained by estimating the actual edge projected line, with the adaptability of the position of the estimated edge projected line in each of the plurality of arithmetic processing regions is the highest. An estimated edge projection line determination means for determining based on the calculated evaluation function,
As data for recognizing the spatial arrangement position of the stepped portion, at least data representing the spatial position and direction of the tip side edge of the upper tread of the stepped portion is used as the determined estimated edge projection line and the A stepped portion space arrangement determining means for determining based on the determined plane parameter;
The evaluation function calculating means calculates each candidate position in each calculation processing area in order to calculate a value of an evaluation function representing the degree of suitability of each candidate position with respect to the position of the actual edge projection line for each calculation processing area. A process for setting a plurality of projection conversion target areas for edge estimation including at least two projection conversion target areas adjacent as boundaries according to each candidate position, and a plane parameter for projection conversion corresponding to each projection conversion target area for edge estimation Processing according to the plane parameters determined by the plane projection area extraction means, and for each set of calculation processing areas and each candidate position, pixel value distribution of the image of each edge estimation projective transformation target area, The image of the region in the reference image corresponding to the projection conversion target region for edge estimation is changed in accordance with the plane parameter for projection conversion corresponding to the projection conversion target region for edge estimation. A process of calculating an error function value representing an error from the pixel value distribution of the projective transformation image obtained in the case, and a plurality of sets set in each calculation processing area for each set of each calculation processing area and each candidate position Stepped portion recognition characterized by executing a process of calculating a value obtained by linearly combining the error function values calculated corresponding to each of the projection conversion target areas for edge estimation as the value of the evaluation function apparatus. The step difference recognition apparatus according to claim 1,
The plane projection area extracting means sets a plurality of plane projection area extraction local areas whose positions in the reference image are different from each other to the reference image, and images of the plane projection area extraction local areas and the plane projection A plane parameter for plane projection area extraction, which is a plane parameter that defines projective transformation with the image of the area in the reference image corresponding to the local area for area extraction, is a pixel of the image of the local area for plane projection area extraction. An error between the value distribution and the pixel value distribution of the image obtained when projective transformation is performed on the image of the area in the reference image corresponding to the local area for plane projection area extraction according to the plane parameter for plane projection area extraction The plane parameter for plane projection area extraction corresponding to each of the local areas for plane projection area extraction included in each plane projection area. It stepped portions recognition apparatus characterized by plane defined to extract the planar projection area from the reference image so as to match one another by. In the step recognition apparatus according to claim 1 or 2,
The plurality of candidate positions set in each calculation processing area by the evaluation function calculation means are N3a (N3a: integer greater than or equal to 2) candidate positions common to all calculation processing areas, and the evaluation function calculation means includes: In order to calculate the value of the error function corresponding to the k1th candidate position (any one of the integers from k1: 1 to N3a) and each calculation processing area, the projection transformation plane parameter Images in the combined projective transformation target region obtained by synthesizing the projection conversion target regions for edge estimation of the calculation processing regions that are identical to each other, and an image of the region in the reference image corresponding to the composite projected transform target region A process for obtaining an error image with an image obtained by performing projective transformation according to the projective transformation plane parameter corresponding to the composite projective transformation target region is executed, and the k1th candidate position is obtained using the error image. Calculating the value of the error function corresponding to the respective processing area stepped portion recognition apparatus according to claim. In the level difference part recognition device according to any one of claims 1 to 3,
It further comprises a stepped portion presence detecting means for detecting whether or not a stepped portion is present in the imaging area of the stereo camera,
The processing of the planar projection area extraction means, the evaluation function calculation means, and the step portion arrangement determination means needs to detect that a step portion is present in the imaging area of the stereo camera by the step portion presence detection means. It is a process executed as a condition,
The stepped portion presence detection means sets a stepped portion presence detection area for detecting whether or not a stepped portion is present in the imaging area of the stereo camera to the reference image, and the stepped portion presence detection A step portion presence detection plane parameter, which is a plane parameter that defines projective transformation between the image of the target region and the image of the region in the reference image corresponding to the step portion presence detection region. An error between the pixel value distribution of the image of the image area and the pixel value distribution of the image obtained when projective transforming the image of the area corresponding to the area for detecting the presence of the stepped portion according to the planar parameter for detecting the presence of the stepped portion A step portion is present in the imaging region of the stereo camera based on the direction of the normal direction of the plane indicated by the determined step portion presence detection plane parameter. Stepped portions recognition device and detecting whether dolphin not. In the level | step difference recognition apparatus of any one of Claims 1-4,
In the state where the stepped portion is imaged by the stereo camera, further comprising a stepped portion type determining means for determining whether the stepped portion is an ascending stepped portion or a descending stepped portion,
The evaluation function calculating means includes
When the stepped portion type determining means determines that the stepped portion is an ascending stepped portion, in the process of setting the plurality of calculation processing regions in the reference image, the upper end of each calculation processing region is the stepped portion. The plurality of lines of arithmetic processing are positioned in the planar projection area corresponding to the upper stage tread of the section and the lower end of each computation processing area is positioned in the plane projection area corresponding to the lower stage tread of the step In the process of setting a region and setting the plurality of edge estimation projection transformation target regions in each of the calculation processing regions, the first and second edge estimation projection transformation target regions adjacent to each other with the candidate positions as boundaries Set on the upper side of the arithmetic processing region, and between the first and second edge estimation projective transformation target regions, the second edge estimation projective transformation target region is the lower region. Depending on the candidate position The third projective transformation target area for edge estimation with interval set on the lower side of the processing area,
When the stepped portion type determining means determines that the stepped portion is a descending stepped portion, in the processing for setting the plurality of calculation processing regions in the reference image, the upper end of each calculation processing region is on the lower side. The plurality of calculation processing areas are set so that the lower end of each calculation processing area is located in the planar projection area corresponding to the tread, and the lower end of each calculation processing area is positioned in the flat projection area corresponding to the upper tread. In the process of setting the plurality of edge estimation projective transformation target regions in the processing region, the fourth and fifth edge estimation projective transformation target regions adjacent to each other at the candidate positions are set in the calculation processing region. The step part recognition apparatus characterized by the above-mentioned. In the level | step-difference part recognition apparatus of Claim 5,
The evaluation function calculating means sets edge estimation plane parameters corresponding to each edge estimation projective transformation target area when the step difference type determining means determines that the step difference is an ascending step difference. In the processing, the projection conversion plane parameter corresponding to each of the first edge estimation projection conversion target area and the third edge estimation projection conversion target area respectively corresponds to the upper tread of the stepped portion. The plane parameter determined for the plane projection area to be set and the plane parameter determined for the plane projection area corresponding to the lower tread of the stepped portion are set, and the second edge estimation projective transformation target As the projection conversion plane parameter corresponding to the region, the projection conversion plane parameter corresponding to the first edge estimation projection conversion target region is used. Stepped portions recognition apparatus characterized by setting the plane parameters representing the position and orientation of the planes that intersect at predetermined angles relative to a plane defined by. In the level difference recognition apparatus according to any one of claims 1 to 6,
The stepped portion space arrangement determining means is configured such that the tip side edge of the upper stepped tread of the stepped portion intersects the plane defined by the plane parameter corresponding to the upper stepped tread at a predetermined angle. A step recognition unit comprising: means for determining a line projected on a plane defined by the plane parameter corresponding to the lower step surface of the step portion as a boundary line on the base end side of the lower step surface. In the level | step difference recognition apparatus of any one of Claims 1-7,
The step recognition apparatus, wherein the moving body is equipped with a projector that gives a texture to an imaging region of the stereo camera during imaging by the stereo camera.

Images