WO2005087452A9 - Robot device, behavior control method for the robot device, and moving device - Google Patents

Abstract

A robot device observes the external world by a stereo vision system (1) and outputs stereo data (D1) as a distance image, the stereo data (D1) being three-dimensional distance information calculated by a parallax of both eyes. A plane detector (2) detects planes from the distance image to recognize the planes present in the environment. From plane data (D2), a stair recognizer (3) extracts a plane which the robot can climb up and down, recognizes a stair from that plane, and outputs stair data (D4). A stair climb-up/-down controller (4) outputs a behavior control command (D5) for realizing the movement for climbing up and down the stair by using the stair data (D4). This enables a mobile body itself to obtain information on a stair to autonomously perform the movement of stair climb up and down.

Description

 Specification

 ROBOT DEVICE, ITS OPERATION CONTROL METHOD, AND MOVING DEVICE TECHNICAL FIELD

 TECHNICAL FIELD [0001] The present invention relates to a robot apparatus, a moving apparatus, and a method for moving up and down a staircase that have moving means such as legs and that can move up and down a plurality of steps.

 This application claims priority on the basis of Japanese Patent Application No. 2004-077214 filed on March 17, 2004 in Japan, and these applications are incorporated herein by reference. Is done.

 Background art

 Conventionally, a plurality of technologies that enable a robot apparatus including a moving apparatus having moving means such as wheels to perform a stepped environment and a stair ascending / descending operation are disclosed. For example, a method of moving up and down using the known staircase information shown in FIG. 1 having node data of the target object for recognizing the shape feature point of the target object (Patent No. 3176701) or FIG. As shown in the figure, there is a method in which a large number of contact sensors 602 are covered with a protective film 603, and the stair ascending / descending operation is performed using a foot that is adhered to the entire back surface of the foot 601 in a matrix shape ( (Patent No. 3278467). Furthermore, as shown in FIG. 3, there is a method of moving up and down the stairs by installing an infrared sensor on the side of the sole and applying a landmark tape to the stairs (Japanese Patent No. 3330710). As shown in Fig. 3A, this is a paint that absorbs light, such as black paint, by providing multiple light sensor detectors 682 on the left and right side parts of the foot 622R / L of a robot device that can walk on two legs. By using Landmarker 680, which is a surface area of a predetermined width made up of straight lines, the relative direction with respect to Landmarker 680 is detected by comparing paired sensor outputs. Is. By using such a foot 622R / L and a land marker 680, the position of the staircase 690 can be recognized as shown in FIG. 3B.

However, since the technology described in Japanese Patent No. 3176701 is based on the known staircase information, it is difficult to apply to an autonomous robot device that cannot be adapted in an unknown environment. is there. Japanese Patent No. 3278467 and In the technology described in Japanese Patent No. 3330710, since the stairs are moved up and down using a plurality of sensors provided on the soles or the sides of the soles, the information on the stairs cannot be acquired until the landing. Therefore, observation from a distance is impossible. For this reason, it cannot be used when it is predicted that there will be a staircase in front of you to some extent.

Disclosure of the invention

Problems to be solved by the invention

 The present invention has been proposed in view of such a conventional situation, and a robot apparatus and a moving apparatus that allow a moving body to acquire information on steps and autonomously move up and down the stairs, and It is an object of the present invention to provide an operation control method for a robot apparatus.

 In order to achieve the above-described object, a robot apparatus according to the present invention detects one or more planes included in an environment from three-dimensional distance data in a robot apparatus that can be moved by a moving means, and Plane detection means for outputting as information, stair recognition means for recognizing a stair having a movable plane from the plane information, and outputting stair information including tread information and kicking information on the tread of the stair; and Based on the staircase information, it is determined whether or not it is possible to move up and down, and if it is determined that it can be moved up and down, the stair lift control means controls the stair lift operation by positioning autonomously with respect to the tread. It is characterized by having.

 In the present invention, in a robot apparatus that can be moved with, for example, legs as a moving means, the sole of the step is placed on the tread from the tread information on the tread of the staircase, for example, the size and position. If it is determined that it is possible to move, it is determined whether it is possible to move to the tread of the height from the information on the kicking up indicating the step of the stairs, By positioning autonomously, it is possible to go up and down stairs.

 In addition, it is possible to have a distance measuring means for acquiring the three-dimensional distance data, and when the stairs are detected, it is possible to move up and down autonomously.

Further, the staircase recognition means is different from the staircase detection means for detecting a step having a movable plane from the given plane information and outputting the pre-integration staircase information in terms of time output from the staircase detection means. Statistically processing multiple pre-integrated staircase information Stairs integration means that outputs the combined integrated staircase information as the above staircase information, for example, when the field of view is narrow! /, Robotic devices, etc. Even so, by using integrated staircase information that is statistically integrated temporally, accurate and high-frequency recognition results can be obtained.

 Further, the stair detection means recognizes the size and spatial position of the tread based on the plane information, outputs the tread information as a recognition result as the pre-integration stair information, and the stair integration means If a tread group consisting of two or more treads that have an overlap area greater than a predetermined threshold and the relative height difference is less than or equal to a predetermined threshold is detected from the tread information that moves forward and backward, It is possible to integrate so that all treads are included, and at the time of integration, it is possible to obtain recognition results over a wide range by integrating all treads that are selected to be integrated. .

 Furthermore, the staircase recognition means can recognize the size and spatial position of the tread based on the plane information and use the tread information as the tread information. The tread information is at least in front of the tread with respect to the moving direction. Information on the front edge indicating the boundary of the front and the information on the back edge indicating the boundary on the back side, and the front edge and the back edge of the tread are recognized. Recognize the stairs and make it possible to move up and down.

 In addition, right margin information and left margin information indicating margin areas that are adjacent to the left and right sides of the safety area, which is an area between the front edge and the back edge, and are estimated to have a high probability of being movable, Reference point information indicating the center of gravity of the area estimated as the tread based on the plane information described above, and three-dimensional coordinate information of the point group constituting the plane serving as the tread can be included. By using these tread information, the stair climbing operation can be controlled more accurately.

Further, the staircase recognition means can calculate a polygon by extracting a plane boundary based on the plane information, and can calculate the tread information based on the polygon. For example, when the field of view is narrow, the three-dimensional When distance data is highly reliable, the polygon can be a convex polygon area that circumscribes the boundary of the plane extracted based on the plane information, and is actually detected. It can be a region including a plane. On the other hand, there is a lot of noise In the case of distance data, etc., the polygon can be a convex polygon area inscribed in the boundary of the plane extracted based on the plane information, and is an area included in the plane that is actually detected. In this way, it is possible to accurately detect the tread by cutting the noise part.Furthermore, after moving to a predetermined position facing the back edge on the moving surface that is currently moving, control is performed so that the lifting operation is executed. For example, when the front edge and the back edge overlap, such as a staircase with a small kick-up, the back edge can be moved to the target and moved up and down. Similarly, the front edge may be moved to the target and the lifting operation may be fi.

 If the back edge on the moving surface that is currently moving cannot be confirmed, the stair lift control means moves to a predetermined position facing the front edge on the next step surface that is the target of the next lift operation. For example, if you move the floor and detect a staircase, the back edge of the floor may not overlap the front edge of the first step of the staircase. In such a case, it is possible to move up and down by moving to the first stage of the stairs, that is, the front edge of the next tread surface to be lifted.

 The stair lift control means detects a tread surface to be moved next, and performs a series of operations of moving to a predetermined position facing the tread surface to be moved to perform a lift operation. Each time it moves to a new tread, it can be moved up and down by performing a search 'align' approach on the tread.

 Furthermore, when the next step or the next step to be moved cannot be detected from the current position, the above-mentioned stair lift control means searches for the next step to be moved that has been acquired in the past. It is possible to obtain information on the steps up or down several steps in advance, so that the robot device cannot obtain the latest information of its own! Can also be moved up and down.

In addition, the stair lift control means moves to a predetermined position facing the back edge on the current moving surface, detects the next tread surface to be moved, and moves to a predetermined position facing the front edge on the tread surface. And so as to perform a lifting operation that moves to the tread. Controllable force S. By using the front edge and back edge, both edges are parallel! /, Na! /, Even if it is a spiral staircase, it can move up and down.

 In addition, the lifting control means can control the lifting operation using a parameter that defines the position of the moving means with respect to the tread surface. It can be determined based on the height. And, it is possible to have a parameter switching means that changes the numerical value of the above parameter between the climbing step and the descending operation, and it is only necessary to change the parameter whether climbing the stairs or descending. It can be controlled similarly.

 The plane detection means is extracted by the line segment extraction means for extracting a line segment for each distance data point group estimated to be on the same plane in the three-dimensional space, and the line segment extraction means. Plane area expanding means for extracting a plurality of line segments estimated to belong to the same plane from the line segment group and calculating a plane from the plurality of line segments;

 The line segment extraction means can adaptively extract line segments according to the distribution of distance data points, and the line segment extraction means are arranged on the same straight line when the three-dimensional distance data is on the same plane. The line segment is extracted using this, but at this time, the distribution of the distance data points differs due to the influence of noise, etc., so the line segment is extracted adaptively according to the distribution of this distance data (Adaptive Line Fitting) enables accurate line segment extraction robust to noise, and planes are obtained from a large number of extracted line segments by the line segment expansion method. However, it is possible to accurately extract a plane without using one plane or multiple planes even though there is only one plane. Further, the line segment extracting means extracts a distance data point group estimated to be on the same plane based on the distance between the distance data points, and based on the distribution of the distance data points in the distance data point group, Whether the distance data point group is on the same plane can be estimated again. The distance data point group is once extracted based on the distance of the distance data point in the three-dimensional space, and then based on the distribution of the data points. By estimating again whether or not they are on the same plane, it is possible to make a precise spring.

Further, the line segment extraction means extracts a first line segment from the distance data point group estimated to be on the same plane, and the distance from the first line segment in the distance data point group. Is the most When the distance data point is a large point of interest and the distance is less than or equal to a predetermined threshold, the distance data point group force second line segment is extracted, and the distance data point is on one side of the second line segment. It is determined whether or not there is a predetermined number or more. If there is a predetermined number or more, the distance data point group can be divided at the point of interest. If the line connecting the end points of the point group is the first line segment, and there is a point with a large distance, a second line segment is generated by, for example, the least square method, and one of the points in the second line segment is generated. When there are multiple data points in succession, it can be assumed that the data point group has, for example, a zigzag shape with respect to the line segment, and therefore the extracted data point group is biased. Therefore, it is possible to divide the data point group at the point of interest or the like.

 Further, the plane area expanding means selects one or more line segments estimated to belong to the same plane, calculates a reference plane, and determines a line segment estimated to belong to the same plane as the reference plane. It is possible to search the segment group as an extension line segment, repeat the process of updating the reference plane with the extension line segment and expanding the area of the reference plane, and output the updated plane as an updated plane. The plane area expansion process and the plane update process can be performed using line segments belonging to the same plane.

 Further, in the distance data point group belonging to the updated plane, if there is a distance data point whose distance from the updated plane exceeds a predetermined threshold, this is removed! Since the updated plane is obtained as an average plane of all the line segments belonging to it, the distance data points greatly deviated from this are excluded, and the data point group is obtained again. By obtaining the plane, it is possible to obtain a detection result in which the influence of noise and the like is further reduced.

 Furthermore, the plane area expanding means can estimate whether or not the line segment belongs to the same plane as the reference plane based on an error between the plane determined by the line segment and the reference plane. Based on the mean square error, etc., it is possible to determine the force that is the effect of noise and whether it is a different plane, and more accurately detect the plane.

The motion control method for a robot apparatus according to the present invention is a motion control method for a robot apparatus that can be moved by a moving means. One or more planes included in the environment are detected from three-dimensional distance data and used as plane information. From the plane detection process to output and the plane information A staircase recognition process that recognizes a staircase having a movable plane and outputs step information on the tread surface of the staircase and kick-up information, and determines whether or not the step can be raised or lowered based on the staircase information. When it is determined that the lifting / lowering operation is possible, there is a stair lifting / lowering control step for autonomously positioning with respect to the tread and controlling the stair lifting / lowering operation.

 The moving device according to the present invention is a moving device that can be moved by the moving means, and detects one or a plurality of planes included in the environment from the three-dimensional distance data, and outputs the plane information as plane information. Step recognition means for recognizing a stair with a movable plane from the plane information and outputting step information on the tread of the stair and information on kicking, and whether or not the stair can be raised or lowered based on the stair information. When it is determined that the elevating operation is possible, the apparatus has a stair ascending / descending control means for autonomously positioning with respect to the tread and controlling the stair ascending / descending operation.

 According to the present invention, for example, from the tread information on the size and position of the tread of the staircase, the sole of the tread is the tread. It is determined whether it is possible to move on the surface of the tread from the information on the kicking up that indicates the step difference of the stairs. In the case of autonomous positioning, the power to climb up and down the stairs is S Kanakura.

 Still other objects of the present invention and advantages obtained by the present invention will become more apparent from the embodiments described below with reference to the drawings.

Brief Description of Drawings

 FIG. 1 is a diagram for explaining a conventional lifting operation.

FIG. 2 is a diagram for explaining a conventional lifting operation.

FIG. 3A and FIG. 3B are diagrams for explaining a conventional lifting operation.

4] FIG. 4 is a perspective view showing an overview of the robot apparatus according to the embodiment of the present invention.

FIG. 5 is a diagram schematically showing a joint degree-of-freedom configuration included in the robot apparatus.

FIG. 6 is a schematic diagram showing a control system configuration of the robot apparatus. [FIG. 7] FIG. 7 is a functional block diagram showing a system for executing processing from the stereo data until the stair ascending / descending operation is developed from the stereo data.

 [FIG. 8] FIG. 8A is a schematic view showing a state where the robot apparatus is photographing the outside world, and FIG. 8B is a view showing the size of the sole of the robot apparatus.

 [Fig. 9] Fig. 9 is a diagram for explaining staircase detection, in which Fig. 9A is a view of the staircase from the front, Fig. 9B is a view of the staircase from the side, and Fig. FIG.

[FIG. 10] FIG. 10 is a diagram for explaining another example of staircase detection. FIG. 10A is a diagram of the stairs viewed from the front, FIG. 10B is a diagram of the stairs viewed from the side, and FIG. Is a view of the stairs from an angle.

 FIG. 11 is a diagram showing an example of the result of detecting the stairs in FIG. 9. FIG. 11A is a schematic diagram showing an image when the stairs in FIG. 9 is photographed, and FIGS. 11B to 11D are diagrams. FIG. 11B is a diagram showing three-dimensional distance data acquired from the image shown in FIG. 11A.

 12 is a diagram showing an example of the result of detecting the stairs in FIG. 10, FIG. 12A is a schematic diagram showing an image when the stairs in FIG. 10 are photographed, and FIGS. 12B to 12D are FIG. 12B is a diagram showing three-dimensional distance data acquired from the image shown in FIG. 12A.

13] Fig. 13A is a schematic diagram showing an image of a staircase, and Fig. 13B is a diagram showing the results of detecting four planar areas A, B, C, and D from the 3D distance data obtained from Fig. 13A. Is

. It is a figure which shows an example of the result of having detected the stairs.

 FIG. 14 is a functional block diagram showing a staircase recognizer.

 FIG. 15 is a flowchart showing a procedure of staircase detection processing.

 FIG. 16A and FIG. 16B are schematic diagrams showing polygons.

 FIG. 17 is a schematic diagram for explaining Melkman's algorithm.

 FIG. 18A and FIG. 18B are schematic diagrams for explaining a method of obtaining a polygon by Sklansky's algorithm.

 [FIG. 19] FIG. 19 is a schematic diagram for explaining a problem that occurs with a non-convex polygonal staircase. FIG. 19A is a diagram showing an input plane, and FIG. 19B is a convex hull. It is a figure which shows the polygonal representation result of the non-convex polygon-shaped staircase.

Fig. 20 is a schematic diagram showing a method for obtaining a polygon that includes an input plane by smoothing. FIG. 20A is a diagram showing an input plane, FIG. 20B is a diagram showing a smoothed polygon obtained by removing discontinuous gaps from the polygon showing the input plane, and FIG. 20C is a diagram. It is a figure which shows the polygon further smoothed by the line fitting with respect to the polygon obtained by 20B.

FIG. 21 is a diagram showing a program example of a process for obtaining a polygon including an input plane by gap removal and smoothing by line fitting.

 [FIG. 22] FIGS. 22A and 22B are schematic diagrams for explaining the method of calculating the staircase parameters. FIG. 23 is a schematic diagram for explaining the tread surface and the staircase parameters that are finally recognized. is there.

 FIG. 24A and FIG. 24B are schematic diagrams showing stairs.

 FIG. 25 is a flowchart showing a method of staircase integration processing.

 FIG. 26 is a schematic diagram for explaining a process of integrating overlapping staircase data.

 FIG. 27 is a diagram for explaining an align operation.

 FIG. 28 is a schematic diagram for explaining an approach operation.

 [FIG. 29] FIG. 29 is a flowchart showing the steps of a stair climbing operation.

 FIG. 30 is a flowchart showing a search “align” approach processing method.

 FIG. 31 is a flowchart showing a method for a lifting operation process.

 [FIG. 32] FIG. 32 is a schematic diagram showing a staircase surface recognized or to be recognized by the robot apparatus.

 [FIG. 33] FIG. 33 is a flowchart showing a method of a lifting operation process.

 FIG. 34 is a flowchart showing a method for a lifting operation process.

FIG. 35A is a diagram for explaining the relationship between the tread and the sole recognized by the robot apparatus, and FIG. 35B is a diagram showing the dimensions of each part.

 [FIG. 36] FIG. 36 is a diagram obtained by tracing a photograph of the robot apparatus performing the ascending / descending operation.

[Fig.37] Fig. 37 traces the image of the robot device moving up and down. FIG.

 FIG. 38 is a diagram showing a relationship between a single stepped portion and the sole of the robot apparatus.

 FIG. 39 is a diagram showing a relationship between a single recess and a sole of the robot apparatus.

FIG. 40 is a functional block diagram showing the flat surface detection apparatus in the present modification. 41] FIG. 41 is a diagram for explaining a robot apparatus having means for applying a texture.

42] FIG. 42 is a diagram for explaining the plane detection method by the line segment expansion method in this modification.

 FIG. 43 is a flowchart showing plane detection processing by a line segment expansion method.

 FIG. 44 is a flowchart showing details of processing in the line segment extraction unit in the present modification.

 [FIG. 45] FIG. 45 is a diagram showing the distribution of distance data points. FIG. 45A shows a case where the data distribution is zigzag with respect to the line segment, and FIG. It is a schematic diagram showing a case where it is uniformly distributed in the vicinity!

 FIG. 46 is a flowchart showing a Zig-Zag-Shape discrimination method in the present modification.

 FIG. 47 is a diagram showing a program example of the Zig-Zag-Shape discrimination process.

FIG. 48 is a block diagram illustrating a processing unit that performs Zig-Zag-Shape discrimination processing. 49] FIG. 49 is a schematic diagram for explaining the area expansion processing in this modification. FIG. 50 is a flowchart showing the process of searching for a region type and the procedure of the region expansion process in the region expansion unit in this modification.

51] Fig. 51 shows an example in which the mean square error rms of the plane equation is different even if the distance between the end point and the straight line is the same. Fig. 51A shows that the line segment deviates from the plane due to the effects of noise, etc. FIG. 51B is a schematic diagram showing a case where there is another plane to which the line segment belongs.

FIG. 52 is a diagram showing a region type selection process.

FIG. 53 is a diagram showing an area expansion process.

54] Fig. 54A is a schematic diagram showing the floor surface when the robot device is standing and looking down on the floor surface. In the equation diagram, Fig. 5 4B, the vertical axis is x, the horizontal axis is y, and the z-axis is expressed by the shading of each data point. FIG. 54C is a diagram showing a straight line detected from a group of data points assumed to exist on the plane, and FIG. 54C is a diagram showing a planar region obtained by the region expansion process from the straight line group shown in FIG. 54B.

 [FIG. 5 5] FIG. 5 5 is a diagram for explaining the difference in results between the plane detection method in the present modification and the conventional plane detection method when a step is placed on the floor surface. 5 5 A is a schematic diagram showing an observed image, FIG. 5 5 B is a diagram showing experimental conditions, FIG. 5 5 C is a diagram showing a result of plane detection by the plane detection method in this modification, and FIG. FIG. 5D is a diagram showing the result of plane detection by the conventional plane detection method.

 [Fig. 5 6] Fig. 5 6 A is a schematic diagram showing an image of the floor surface, and Fig. 5 6 B and Fig. 5 6 C are three-dimensional distances obtained by imaging the floor surface shown in Fig. 5 6 A. FIG. 11 is a diagram showing a line segment detected by line segment detection of the present modification and a line segment detected by conventional line segment detection from distance data points in the horizontal direction and vertical direction from the data line.

BEST MODE FOR CARRYING OUT THE INVENTION

 Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. In this embodiment, the present invention is applied to an autonomously operable robot apparatus equipped with a step recognition device that recognizes a step such as a stair existing in the surrounding environment.

 The robot apparatus according to the present embodiment uses distance information obtained by stereo vision or the like.

The stairs are recognized from multiple planes extracted from (distance data), and the stairs can be moved up and down using the stairs recognition results.

 (1) Robot equipment

 Here, as an example of such a mouth pot device, a biped walking type robot device will be described as an example. This robotic device is a practical mouth pot that supports human activities in various situations in the living environment and other daily lives, and can act according to the internal state (anger, sadness, joy, fun, etc.) This is an entertainment robot that can express the basic operations performed by. Here, a bipedal walking robot device will be described as an example, but the staircase recognition device is not limited to a bipedal walking robot device. The raising / lowering operation can be executed.

Replacement paper (Rule 26) FIG. 4 is a perspective view showing an overview of the robot apparatus according to the present embodiment. As shown in FIG. 4, the robot apparatus 201 has a head unit 203 connected to a predetermined position of the trunk unit 202, two left and right arm units 204R / L, and two left and right leg units 2. 05R / L is concatenated (where R and L are suffixes indicating right and left, respectively, the same applies hereinafter).

 FIG. 5 schematically shows the joint degree-of-freedom configuration of the robot apparatus 201. The neck joint that supports the head unit 203 has three degrees of freedom: a neck joint axis 101, a neck joint pitch axis 102, and a neck joint pole axis 103.

 In addition, each arm unit 204R / L constituting the upper limb includes a shoulder joint pitch axis 107, a shoulder joint roll axis 108, a brachial arm axis 109, an elbow joint pitch axis 110, a forearm arm axis 1 11, It comprises a wrist joint pitch axis 1 12, a wrist joint roll wheel 113, and a hand part 114. The hand 114 is actually an articulated multi-degree-of-freedom structure including a plurality of fingers. However, since the movement of the hand 114 has little contribution or influence on the posture control or walking control of the robot apparatus 201, it is assumed in this specification that the degree of freedom is zero for simplicity. Therefore, each arm has 7 degrees of freedom.

 The trunk unit 202 has three degrees of freedom: the trunk pitch axis 104, the trunk roll axis 105, and the trunk axis 106.

 Each leg unit 205R / L constituting the lower limb includes a hip joint axis 115, a hip joint pitch axis 116, a hip joint roll axis 117, a knee joint pitch axis 118, and an ankle joint pitch axis 119. The ankle joint roll shaft 120 and the sole 121 are configured. In the present specification, the intersection of the hip joint pitch axis 116 and the hip joint roll axis 117 defines the hip joint position of the robot apparatus 201. The sole 121 of the human body is actually a structure including a multi-joint / multi-degree-of-freedom sole, but in this specification, for the sake of simplicity, the sole of the robot apparatus 201 has zero degrees of freedom. . Therefore, each leg is composed of 6 degrees of freedom.

In summary, the robot apparatus 201 as a whole has a total of 3 + 7 X 2 + 3 + 6 X 2 = 32 degrees of freedom. However, the robot device 201 for entertainment is not necessarily limited to 32 degrees of freedom. It goes without saying that the degree of freedom, that is, the number of joints, can be increased or decreased as appropriate according to the design constraints and required specifications. Absent.

 Each degree of freedom of the robot apparatus 201 as described above is actually implemented using an actuator. Due to demands such as eliminating external bulges on the exterior and approximating the human body shape, biped walking and leg, and posture control for unstable structures, the actuator is small and lightweight. It is preferable that

 Such a robot apparatus includes a control system that controls the operation of the entire robot apparatus, for example, the trunk unit 202. FIG. 6 is a schematic diagram showing a control system configuration of the robot apparatus 201. As shown in Fig. 6, the control system controls the whole body coordinated movement of the robot device 201, such as the drive of the thought control module 200 that controls emotional judgment and emotional expression in response to user input, etc., and the actuator 350. The motion control module 300

 The thought control module 200 includes a central processing unit (CPU) 211, a random access memory (RAM) 212, a read only memory (ROM) 213, and an external storage device (node, ('Disk' drive etc.) This is an independent information processing device that consists of 214 etc. and can perform self-contained processing within the module.

 This thought control module 200 determines the current emotion and intention of the robot device 201 in accordance with stimuli from the outside, such as image data input from the image input device 251 and sound data input from the sound input device 252. To do. That is, as described above, by recognizing the user's facial expression from the input image data and reflecting the information on the emotion and intention of the robot apparatus 201, it is possible to express an action according to the user's facial expression. . Here, the image input device 251 includes a plurality of CCD (Charge Coupled Device) cameras, for example, and can obtain a distance image from images captured by these cameras. The audio input device 252 includes a plurality of microphones, for example.

The thought control module 200 issues a command to the motion control module 300 to execute an action or action sequence based on decision making, that is, movement of the limbs. Controls 201 whole body coordination This is an independent drive type information processing apparatus that is composed of a CPU 311, a RAM 312, a ROM 313, an external storage device (hard 'disk' drive, etc.) 314, etc. and can perform self-contained processing in a module. Further, the external storage device 314 can store, for example, walking patterns calculated offline, target ZMP trajectories, and other action plans.

 The motion control module 300 includes an actuator 350 that realizes the degree of freedom of joints distributed throughout the body of the robot apparatus 201 shown in FIG. 5, and a distance measurement sensor (not shown) that measures the distance to the object. , Posture sensor 351 for measuring the posture and inclination of the trunk unit 202, ground contact confirmation sensor 352, 353 for detecting the floor or landing of the left and right soles, load sensor provided on the sole 121 of the sole 121, battery Various powers such as a power supply control device 354 that manages the power source of the power supply S, etc. are connected via a bus interface (I / F) 310. Here, the posture sensor 351 is configured by, for example, a combination of an acceleration sensor and a gyro sensor, and the grounding confirmation sensors 352 and 353 are configured by a proximity sensor, a micro switch, or the like.

 Thought control module 200 and motion control module 300 are built on a common platform and are interconnected via bus' interfaces 210 and 310.

Yes

 The motion control module 300 controls the whole body coordinated motion by each of the actuators 350 that embodies the action instructed by the thought control module 200. That is, the CPU 311 extracts an operation pattern corresponding to the action instructed from the thought control module 200 from the external storage device 314, or internally generates an operation pattern. The CPU 311 then sets the foot movement, ZMP trajectory, trunk movement, upper limb movement, waist horizontal position, height, etc. according to the specified movement pattern, and instructs the movement according to these settings. The command value to be transferred is transferred to each actuator 350.

In addition, the CPU 311 detects the posture and inclination of the trunk unit 202 of the robot device 201 based on the output signal of the posture sensor 351, and each leg unit 205R / L detects the posture of the trunk unit 202 based on the output signals of the grounding confirmation sensors 352 and 353. By detecting whether the leg is a free leg or a standing leg, the whole body cooperative movement of the robot apparatus 201 can be adaptively controlled. Further In addition, the CPU 311 controls the posture and operation of the robot apparatus 201 so that the ZMP position is always directed toward the center of the ZMP stable region.

 In addition, the motion control module 300 returns to the thought control module 200 the power at which the behavior as intended determined by the thought control module 200 is expressed, that is, the processing status. In this way, the robot apparatus 201 can determine the self and surrounding conditions based on the control program and can act autonomously.

 (2) Mouth pot device operation control method

 In the robot apparatus described above, the stereo vision system is mounted on the head unit 203, and the three-dimensional distance information of the outside world can be acquired. Next, it is suitably mounted on such robotic devices, etc., and the plane is detected using the 3D distance data acquired by the surrounding force of the robotic device stereo vision system. A series of processes for recognizing staircases based on the results and performing stair climbing using the staircase recognition results will be described.

 FIG. 7 is a functional block diagram showing a system for executing a process from the stereo data until the stair ascending / descending operation is manifested from the stereo data. As shown in Fig. 7, the robot apparatus receives stereo data D1 from stereo vision system 1 (Stereo Vision System) 1 and stereo vision system 1 as distance data measurement means for acquiring 3D distance data. Plane Segmentation / Extractor 2 that detects the plane in the environment from the data D1, Stair Recognition 3 that recognizes the stair from the plane data D2 output from the flat detector 2, and Stair recognition A stair climbing controller (Stair Climber) 4 for outputting an operation control command D5 for performing stair climbing operation using the stair data D4 which is a recognition result recognized by the unit 2 is provided.

The robot apparatus first observes the outside world by stereo vision and outputs stereo data D1, which is three-dimensional distance information calculated by binocular parallax, as an image. That is, image input from two left and right cameras corresponding to human eyes is compared for each pixel neighborhood, the distance from the parallax to the target is estimated, and 3D distance information is output as an image (distance image) . By detecting a plane from the distance image by the plane detector 2, a plurality of planes existing in the environment can be recognized. Furthermore, the stair recognizer 3 From these planes, the plane on which the robot can move up and down is extracted, the stairs are recognized from the plane, and the stairs data D4 is output. Then, the stair lift controller 4 uses the stair data D4 to output a motion control command D5 that realizes the stair lift operation.

 FIG. 8A is a schematic diagram showing a state where the robot apparatus 201 is photographing the outside world. When the floor surface is the XY plane and the height direction is the z direction, as shown in FIG. 8A, the visual field range of the robot apparatus 201 having the image input unit (stereo camera) in the head unit 203 is as follows. 2 This is the predetermined range in front of 01.

 The robot apparatus 201 has a software configuration in which the CPU 211 described above receives a color image and a parallax image from the image input apparatus 251, sensor data such as all joint angles of each actuator 350, and the like, and executes various processes. Realize.

 The software in the robot apparatus 201 of the present embodiment is configured in units of objects, recognizes the position, movement amount, surrounding obstacles, environment map, etc. of the robot apparatus, and the action that the robot apparatus should finally take. The ability to perform various recognition processes that output action sequences for As the coordinates indicating the position of the robot apparatus, for example, a world-standard camera coordinate system (hereinafter also referred to as absolute coordinates) having a predetermined position based on a specific object such as a landmark as the origin of the coordinates, Two coordinates are used: a robot center coordinate system (hereinafter also referred to as relative coordinates) centered on the robot device itself (coordinate origin).

 In the stereo vision system 1, a robot center coordinate system in which the robot apparatus 201 is fixed at the center by using the joint angle calculated from the sensor data at the time when image data such as a color image and a parallax image from a stereo camera is captured. Is converted to the coordinate system of the image input device 251 provided in the head unit 203. In this case, in this embodiment, a homogeneous transformation matrix of the camera coordinate system is derived from the robot center coordinate system, and a distance image composed of the homogeneous transformation matrix and the corresponding three-dimensional distance data is output. To do.

The robot apparatus according to the present embodiment can recognize a staircase included in its own field of view, and can use the recognition result (hereinafter referred to as staircase data) to move up and down the staircase. Therefore, for the stair ascending / descending operation, the robot apparatus determines whether the size of the staircase is smaller than the size of its own sole or whether the height of the staircase can be climbed or lowered. It is necessary to make various judgments about the size of the stairs. Here, in the present embodiment, a case will be described in which the size of the sole of the robot apparatus is FIG. 8B. That is, as shown in FIG. 8B, assuming that the forward direction of the robot device 201 is the X-axis direction and the method that is parallel to the floor surface and orthogonal to the X direction is the y direction, the y direction of both feet when the robot device 201 stands upright The width of the feet is the size of the sole, the front part from the ankle (joint part of the leg and the sole) is the front sole width foot_fr ₀ nt_size, the rear part from the ankle is the sole behind the sole The width is foot_back_size.

 Examples of steps detected by the robot apparatus 201 from the environment include those shown in FIG. 9 and FIG. 9A and 10A are views of the stairs viewed from the front, FIGS. 9B and 10B are views of the stairs viewed from the side, and FIGS. 9C and 10C are views of the stairs viewed from an oblique direction.

 Here, the surface used by humans, robotic devices, etc. to move up and down the stairs (the surface on which the foot or movable leg is placed) is the tread, and the height from one tread to the next tread (stair 1 The height of the step) is called kicking up. In the following, the stairs will be counted as the first and second steps as you climb from the side closer to the ground.

 The staircase ST1 shown in Fig. 9 is a three-step staircase, and the tick height of the stepped up 4cm, 1 and 2 steps is 30cm wide, 10cm deep, only the 3rd step tread, which is the top step, the width It is 30cm and the depth is 21cm. The staircase ST2 shown in Fig. 10 is also a three-step staircase with a kick height of 3 cm, the size of the tread on the 1st and 2nd steps is 33 cm in width, 12 cm in depth, and the third step on the top. Only 33cm wide and 32cm deep. The result of the robot device's recognition of these stairs will be described later.

 The plane detector 2 detects a plurality of planes existing in the environment from the distance information (stereo data D1) from which the distance measuring instrument force such as stereo vision is also output, and outputs the plane data D2. As a plane detection method, a well-known plane detection technique using Hough transform can be applied in addition to the line segment expansion method described later. However, in order to detect multiple planes like staircases from noise-containing distance data, it is possible to detect planes accurately by performing plane detection using the line segment expansion method described later.

FIG. 11 and FIG. 12 are diagrams showing an example of the result of detecting the stairs. FIGS. 11 and 12 are examples in which plane detection is performed by acquiring three-dimensional distance data from images obtained by capturing the stairs shown in FIGS. 9 and 10 by a plane detection method described later. That is, FIG. FIG. 11B to FIG. 11D are diagrams showing three-dimensional distance data acquired from the image shown in FIG. 11A. 12A is a schematic diagram showing an image when the level of FIG. 10 is photographed, and FIGS. 12B to 12D are diagrams showing three-dimensional distance data acquired from the image shown in FIG. 12A. As shown in Figs. 11 and 12, in all cases, all treads can be detected as flat surfaces. Fig. 11B shows an example in which the treads of the first, second, and third tiers are detected from the bottom. FIG. 12B also shows that a part of the floor is successfully detected as another plane! /.

 That is, as shown in FIG. 13A, for example, when a plane detection is performed from a distance image obtained by capturing the staircase ST2, the areas A to D are the floor surface, the first step, the second step, and the third step, respectively, as shown in FIG. 13B. It is detected as a plane indicating the tread surface of the eye. Point groups shown in the same area included in each of the areas A to D indicate distance data point groups estimated to form the same plane. The plane data D2 thus detected by the plane detector 2 is input to the staircase recognizer 3, and the shape of the step, that is, the size of the tread, the height of the staircase (the size of the kick-up), and the like are recognized. Here, the staircase recognizer 3 in the present embodiment is a force that will be described in detail later. The boundary on the near side (side closer to the robot apparatus) of the region (polygon) included in the tread recognized by the robot apparatus 201 (Front Edge) (hereinafter referred to as “Front Edge FE”) and the boundary (Back Edge) (hereinafter referred to as “Back Edge BE”) of the rear side of the tread (the side far from the robotic device) as staircase data recognize. Then, the stair lift controller 4 controls the stair lift operation using the stair data.

 Next, the stair climbing control method of the robot apparatus will be specifically described. In the following, the first is the staircase recognition method of the robot device, the second is the stair ascending / descending operation using the recognized staircase, and finally the plane detection method by the line segment expansion method as a specific example of the plane detection method. I will explain.

FIG. 14 is a functional block diagram showing the staircase recognizer shown in FIG. As shown in Figure 14, the staircase recognizer 3 includes a stair extraction 5 that detects staircases from the plane data D2 output from the flat detector 2, and the staircase data detected by the staircase detector 5. Stair Merging 6 that performs D3 time series data, that is, a plurality of staircase data D3 detected at different times, and more accurately recognizes staircases. The staircase data D4 integrated by the integrator becomes the output of the staircase recognizer 3.

 The staircase detector 5 detects the staircase from the plane data D2 input from the plane detector 2.

The plane data D2 input from the plane detector 2 has the following information for each plane, and plane data for each plane detected from the image captured by the stereo vision system 1 is input. Is done.

 That is, the plane data D2 is

1—1: The number of points that make up the plane ^ number of supporting points;

 1 2: Point at the center of the plane

 1-3: plane parameter (normal vector, distance from origin)

1 -4: Boundary of polygons composing the plane

It has information composed of

 Based on this plane data, the robot device selects a plane that is substantially horizontal to the grounding surface such as the floor surface or the tread surface on which it is grounded, and calculates the following information (hereinafter referred to as the “step parameter”). That is,

 2—1: Front edge FE, Back edge BE

2—2: Stair height

It is.

The front edge FE and the knock edge BE recognized by the robot device indicate the boundary (line) of the tread surface as described above. When the robot device faces, the polygonal V and robot The side boundary (front side boundary) is the front edge FE, and the side far away from the robot unit (back side boundary) is the back edge BE. As will be described later, for example, a minimum polygon including all points constituting a plane can be obtained and used as the front or back boundary. The information on the front edge FE and the back edge BE can be information on these end points. In addition, information such as the width W (width) of the staircase and the length of the staircase (length) can be obtained from the polygon. The height of the staircase (pick-up) is calculated by using the center point of the plane of the given plane data D2 as the height difference between the center points of the two planes or when obtaining the above polygon. Using the center of gravity of It is possible to make a difference in height. The kick-up may be the difference in height between the back edge BE at the front stage and the front edge FE at the rear stage.

 Further, in the present embodiment, in addition to the front edge FE and the back edge BE, an area adjacent to the left and right of the area sandwiched between the front edge FE and the back edge BE (safety area) is movable. Recognize a region with a high probability as a margin (region). How to obtain these will be described later. By obtaining this margin, it is possible to widely recognize the tread area that is estimated to be movable. Furthermore, a set of information (stair parameter) such as the number of data points constituting the tread and the information of the reference point that defines one center of gravity as described above can be used as the staircase data D3.

 A plane (stair) satisfying the following conditions is extracted from the above staircase data.

3—1: The length of front edge FE and back edge BE is equal to or greater than the specified threshold.

3- 2: The height of the stairs is below a predetermined threshold

 In addition,

3- 3: Stair width W (width) is more than a predetermined threshold

3-4: Stair length L (length) is greater than or equal to the specified threshold

It is preferable to extract those satisfying simultaneously.

 FIG. 15 is a flowchart showing the steps of the staircase detection process of the staircase detector 5. As shown in Fig. 15, first, based on the plane parameters of the input plane data, whether or not the input plane is a plane that can be walked or moved, such as whether or not it is horizontal with the ground plane, for example. Judge (Step Sl). Here, the condition of what plane is horizontal or movable may be set according to the function of the robot apparatus. For example, if the plane vector of the input plane is η (η, η, n), it is horizontal if | sin n |> min

 X y z — Can be judged as 1 z th. Here, min is a threshold for judging the horizontal plane.

 th

For example, min = 80 ° in consideration of accuracy such as distance data to be used and plane detection.

 th

If it is tilted to ± 10 ° with respect to the plane, it can be detected as being horizontal. Or, for example, if you can walk or move even if there is an inclination of about ± 30 °, you can extract the plane of those angular ranges! / ,. If it is determined in step S 1 that it is not horizontal! /, (Step SI: No), it will output that the detection has failed and the processing will be terminated. The process for the plane data is executed.

 Next, if the plane is horizontal (step SI: Yes), processing is performed to recognize the plane boundary (shape). Here, for example, Sklansky's algorithm (J. Sklansky, "Measuring concavity on a rectangular mosaic, IEE ^ frans and omput.21, 1974, pp.1355-1364) and Melkman's analogy (Melkman A.," On-line Construction A polygon that includes the input plane is obtained by a convex hull such as the Convex Hull of a Simple Polygon Information Processing Letters 25, 1987, p. ll) or by smoothing by noise removal (step S2). Then, step parameters such as a front edge and a back edge are obtained from the boundary lines before and after the polygon (step S3). In the present embodiment, the width W (width) and the length (length) in the plane showing the stepped tread are obtained from the boundary lines of the front edge and the back edge, and these values are larger than a predetermined threshold value. Judge whether it is! / (Step S4). If the tread width and length are not more than the predetermined threshold! / (Step S4: No), the robot device is not a movable plane, and the next plane data is processed again from step S1. repeat.

 If the width and length of the plane are greater than or equal to the predetermined threshold (Step S4: No), it is determined that the tread is movable and the left and right margins (Left Margin, Right Margin) are calculated (Step S5). Is output as staircase data D3.

 Next, a method for obtaining a polygon including the input plane by the convex hull in step S2 will be described. FIG. 16 is a schematic diagram showing a convex polygon. FIG. 16A shows that all supporting points determined to belong to one input plane (same plane and included in a continuous area). FIG. 16B shows a convex polygon obtained from the figure shown in FIG. 16A. The convex polygon shown here can be obtained by using a convex hull for obtaining a minimum convex set including a given plane figure (an area including a supporting point). As will be described later, the point indicated by G is used when determining the width W of the tread, and indicates, for example, a point (reference point) such as the center of gravity of the region including the supporting point.

Examples of algorithms for obtaining a convex polygon using such a convex hull include Melkman's algorithm and Sklansky's algorithm as described above. Figure 17 shows Melkman's It is a schematic diagram for demonstrating an algorithm. As shown in Fig. 17, three points PI, P2, and P3 are extracted from a given figure, and line segments connecting ^, ,, and P2 are drawn, and ^, ΡΙ, Ρ3, Ρ2, Ρ3 are drawn. Draw a line straight through. As a result, it is divided into five areas AR;! To AR5, including the three points, PI, Ρ2, Ρ3 forces, and the triangle AR4. Then, the convex polygon is updated by repeating the process of determining which region the next selected dot 4 is included and re-forming the polygon. For example, if Ρ4 exists in the area AR1, the area surrounded by the line segments connected in the order of P1, Ρ2, Ρ4, and Ρ3 becomes the updated convex polygon. In addition, when the point Ρ4 exists in the areas AR3 and AR4, the areas surrounded by the line segments connected in the order of P1, Ρ2, Ρ3, and Ρ4, respectively, are connected in the order of P1, Ρ4, Ρ2, and Ρ3. The convex polygon is updated as an area surrounded by the line segment. On the other hand, if the point P is included in the area AR4, that is, the convex polygon, the convex polygon is not updated, and if there is a point P4 in the area AR2, except for the point P3, PI, P2, and P3 The convex polygon is updated as an area surrounded by line segments connected in order. In the present embodiment, the force S is used to generate a convex polygon for all supporting points in consideration of the area included in each point.

 FIG. 18 is a schematic diagram for explaining a method of obtaining a polygon by Sklansky's algorithm. The polygon extracted by Sklansky's algorithm is a force called Weakly Externally Visible Polygon. The amount of calculation is small compared to the above-mentioned Sklansky's Al ratio, so high-speed computation is possible.

 When obtaining a convex polygon that includes a given figure, a half line is drawn from an arbitrary point X on the boundary of the given figure 131 to a circle 132 containing the figure 131, as shown in FIG. 18A. At this time, if a half line that does not cross the figure 131 among the half lines drawn from the point X to the circle 132 can be drawn, it is assumed that this point constitutes the boundary of the convex polygon. On the other hand, as shown in Fig. 18B, when a half line is drawn on a circle 134 including figure 133 from any other point y on the boundary of given figure 133, a half line that does not cross figure 133 is drawn. I can't. In this case, the other point y does not constitute a boundary of the convex polygon. As described above, when it is determined whether or not each point sequentially constitutes the boundary of the convex polygon and the figure from only the selected point is obtained, the figure as shown in FIG. 16A is obtained.

By calculating the convex polygon that encompasses this figure, the convex polygon in Figure 16B can be obtained. The Here, in this embodiment, when the convex polygon is obtained from FIG. 16A in consideration of the accuracy, characteristics, etc. of the stereo vision system 1, as shown in FIG. 16B, the convex circumscribing the figure in FIG. 16A is obtained. Although the description will be made assuming that a polygon is obtained, it is a matter of course that a convex polygon inscribed in the figure of FIG. 16A may be obtained in consideration of the accuracy and characteristics of the camera. Also, use these methods according to the degree of inclination of the plane and the surrounding conditions.

 In addition, when a polygon including the input plane is obtained by a convex hull in step S2 in FIG. FIG. 19 is a schematic diagram showing this problem. FIG. 19A is an input plane, and stepO is a non-convex polygonal step. Figure 19B shows the polygonal representation result of stepO by the convex hull, and there is a big difference from the desired result for the non-convex part. As a method of handling such non-convex polygons, a method of obtaining polygons by smoothing by gap removal and line fitting can be considered. A method for obtaining a polygon that includes the input plane by gap removal and smoothing by line fitting is described. FIG. 20 is a schematic diagram showing smoothing. FIG. 20A shows all supporting points determined to belong to one input plane (distances included in a continuous area on the same plane). Figure 20B shows an input polygon that is a region that contains data points, and Figure 20B shows a smoothed polygon (gap) that removes discontinuous gaps from the polygon that represents the input plane (close gaps). Fig. 20C shows the polygon obtained by smoothing the polygon obtained by line fitting (fit line segments) to the polygon obtained in Fig. 20B (smoothed polygon: smoothed polygon). It is. Here, FIG. 21 is a diagram showing a program example of a process for obtaining a polygon that includes an input plane by gap removal and smoothing by line fitting. Close gaps processing for removing discontinuous gaps from the polygon, and The Fit line segments process is shown in which the obtained polygon is further smoothed by line fitting.

First, the gap removal method will be described. Select three consecutive vertices from the vertices that represent the polygon, and if this central point is far from the straight line connecting the end points, remove this central point. Continue for the remaining vertices until there are no more points to remove. Next, a line fitting method will be described. Three consecutive vertices are selected from the vertices representing the polygon, and a straight line approximating these three points and the error between this straight line and the three points are obtained by the least square method. All the approximate lines and errors obtained are arranged in ascending order of error, and if the error is smaller than a certain threshold, the center point is removed and the position of the end point is recalculated using the approximate line. This process continues until there are no more points to remove.

 Next, the process in step S3 in FIG. 15 will be described. First, the above-mentioned staircase parameters are calculated from the obtained polygons (FIGS. 16B and 20C). FIG. 22 is a schematic diagram for explaining a method for calculating the stair parameter. As shown in FIG. 22A, it is assumed that the region is surrounded by the obtained polygon 140 force points 141 to 147. Here, when viewed from the robot apparatus 201, the line segment forming the front boundary of the polygon 140 is the front edge FE, and the line segment forming the back boundary is the back edge BE.

 The width W of the stair tread is the length of the line connecting the center point C of the front edge FE and the reference point G

 FE

Is dl and the length of the line segment connecting the center point C of the back edge BE and the reference point G is d2.

 BE

The width can be W = dl + d2.

 Here, the reference point G should be the approximate center of the tread surface, for example, the center point of all supporting points, the center of gravity of the polygon 140, the end points of the front edge FE and the back edge BE. Or the center of gravity of the safety area 152 shown in FIG. 22B.

 Based on the front edge FE, the back edge BE, and the reference point G obtained in this way, the length L and width W of the stairs are obtained and the process of step S4 is executed. The length L of the stairs is, for example, the shorter of the lengths of the front edge FE and the back edge BE, or the front edge FE including the left and right margins shown below and the back edge BE including the left and right margins. Use the power S to make the longer one.

 Next, the margin calculation method in step S5 will be described. FIG. 23 is a schematic diagram for explaining the tread surface and the step parameter finally recognized. In this embodiment, as shown in FIGS. 22B and 23, margins M and M are provided at the left and right end portions of the safety region 152, and the region 151 including the left and right margins M and M is finally formed on the tread. As

1 2 1 2

It shall be recognized. Left and right margins M and M are front edge FE and back edge If polygons protrude outside the safety area 152 specified by BE, first select those points. For example, in Fig. 22A,

 2

 Points 142, 143 and 146. Find right margin M adjacent to the right side of safety area 152

 On the other hand, the point 142, which is the point farthest from the safety area 152, is selected, and a perpendicular is drawn from this point 142 to the front edge FE and the back edge BE. Then, it is assumed that the region 151 surrounded by the perpendicular line, the front edge FE, and the back edge BE is recognized as a tread. The margin may be obtained by simply drawing a line segment that passes through the point 142 and intersects the front edge FE or the back edge BE.

 Here, as shown in FIG. 22B, the length of the left margin M on the same straight line as the front edge FE is lmf, and the length of the left margin M on the same straight line as the back edge BE is lbm. Similarly, let rfm and rbm be the length of the right margin M on the same straight line as the front edge FE and knock edge BE, respectively.

 2

 The effect of recognizing the stairs by finding the front edge FE and the back edge BE from the polygon in this way will be described. 24A and 24B are schematic diagrams showing two types of stairs. FIG. 24A shows a staircase having a rectangular tread surface as shown in FIG. 9 and FIG. 10, while FIG. 24B shows a spiral staircase. In the spiral stage as shown in Fig. 24B, the back edge BE is not parallel to the front edge FE. Therefore, for example, an algorithm that simply extracts a rectangular region from a detected plane may not be applicable. Therefore, as in the present embodiment, by obtaining a polygon from the detected plane and obtaining the front edge FE and the back edge BE, the robot apparatus can move up and down even in such a spiral staircase. Become.

Next, the staircase integrator 6 shown in FIG. 14 will be described. The staircase integrator 6 inputs the staircase data (stair parameter) D3 detected by the staircase detector 5, and integrates the staircase data D3 over time, so that more accurate and high-level staircase information can be obtained. It is an estimate. For example, when the robot device has a narrow field of view, the entire staircase may not be recognized at a time. In such a case, for example, in the old stair data such as the previous frame and the new stair data such as the current frame, a set of spatially overlapping stairs is searched and overlapped. New by integrating stairs Define a virtual staircase. By continuing this process until there are no overlapping stairs, it is possible to recognize the correct stairs.

 FIG. 25 is a flowchart showing a method of staircase integration processing in the staircase integrator 6. First, the current stair data (New Stairs) and the old stair data (Old Stairs) are input (Step S11), and all the new stair data and old stair data are made into one set (ujnion) (Step S12). ). In the combined staircase data set, the spatially overlapping staircase data is searched (step S13), and if there are overlapping sets (step S14: Yes), those staircases are searched. And are registered in the staircase data set (step S15). Then, the processing in steps S13 and S14 is continued until there is no spatially overlapping staircase pair (step S14: No), and finally the updated staircase data set is output as staircase data D4.

 FIG. 26 is a schematic diagram for explaining the processing in step S13 for integrating overlapping staircase data. FIG. 26 shows spatially overlapping staircase data ST11 and ST12. To determine whether or not there is a spatial overlap, for example, the height difference (distance) at the reference point G of the two staircase data ST11 and ST12 overlaps the tread area including the left and right margins. The size of the area can be used. That is, the difference in height between the centroids G and G of the two stairs is below the threshold (maxdz), and

 11 12

If the size of the overlapping area is greater than or equal to the threshold (minarea), it can be determined that the treads indicated by these two staircase data overlap! In this case, as shown in the lower diagram of FIG. 26, the staircase data ST11 and ST12 are integrated to obtain the center of gravity G.

 13 Staircase data ST13.

 Here, when integrating, the area of the outer frame including the staircase data ST11 and ST12 is set as step ST13, and the area including the safety area excluding the left and right margins of the staircase data ST11 and the staircase data ST12 before integration is integrated. The new safety area 165 later is set, and the area obtained by removing the safety area 165 from the staircase data ST13 is defined as margins M and M. Safety area after integration 165

 1 2

Thus, the integrated front edge FE and back edge BE can be obtained.

That is, the two end points of the front edge FE in the combined staircase data ST13 are the left and right ends of the front edge FE of the staircase data ST11 and the front edge FE of the staircase data ST12. By comparing the points, the right end point 163 is on the right side, and the left end point is on the left side. In addition, the position of the line of the front edge FE is the line position closer to the robot device (front side) by comparing the front edge FE of the staircase data ST11 and the front edge FE of the staircase data ST12. Similarly, the position of the back edge BE on the far side is selected, and its left and right end points 161 and 162 are selected so as to spread further left and right.

 The integration method is not limited to this. In this embodiment, taking into account the field of view of the robot device, the square area determined by the front edge FE and the back edge BE, and the force S that is integrated so that the integrated data ST13 is the largest, for example, If the field of view is sufficiently wide or the accuracy of the distance data is sufficiently high, the area obtained by simply combining the two staircase data can be used as the integrated staircase data. Further, the reference point G after integration can be obtained by taking a weighted average according to the ratio of the number of supporting points included in the staircase data ST11 and the staircase data ST12.

 Next, the stair lift controller 4 shown in FIG. 7 will be described. The stair lift controller 4 performs control for the robot device to actually lift and lower the stair using the stair data D4 integrated and detected by the stair detector and stair integrator 6. This lifting control includes the operation of searching for stairs.

 The stair-climbing operation realized in this embodiment can be achieved by constructing the following five state machines.

 4-1: Search operation

 In order to acquire a distance image with the stereo vision system mounted on the head of the robotic device, shake the head to look around and collect environmental information.

 4 2: Align operation

It faces the stairs and moves to a certain fixed distance. FIG. 27 is a diagram for explaining the alignment operation. In FIG. 27, it is assumed that the area 170 is the first step of the stair tread recognized by the robot apparatus. In the align operation, the robot moves to a target position (hereinafter referred to as an align position) that is a predetermined distance ad (align_distance) in the direction orthogonal to the center point of the front edge FE of the tread 170. In this case, if the current position of the robot device is point 171 and the target align position is point 172, the distance between the two If There there angular difference between the direction facing the direction and mouth bot device perpendicular to the away predetermined threshold 01 _ (s! /, Ru if and full opening Ntoejji FE is above a predetermined threshold max_ _a, the robot object It is determined that the alignment operation has been completed when the movement is started to the alignment position 172, and when these conditions are satisfied, for example, m _ax _d = 3 (cm), max_a = 2 ° Force S

 4-3: Approach operation

 In the approach operation, the robot apparatus moves to just before the stairs. FIG. 28 is a schematic diagram for explaining the approach operation. As shown in Figure 28,

When the robot apparatus 201 moves to the alignment position 172, which is the target position separated by Align_distance, and completes the alignment operation, the center point C of the front edge FE of the tread 170 is aligned with the robot apparatus 201 in order to move up and down the stairs. And the distance is a predetermined value ax (

 FE

Move to the target position (hereinafter referred to as approach position) that is approach.x).

 4—4: Climb movement

 The stairs are moved up and down based on the stairs data obtained by stairs recognition. If it moves to the next tread (step) and the next tread is observed, it continues to move up or down. By continuing this operation until the next step is eliminated, the stair climbing operation is realized.

 4-5: Finish operation

 When climbing stairs by climbing, make sure that you are at the top and move to the center of the top. If the climbing operation is performed by climbing, or if it is a step ST1 as shown in Fig. 9, for example, if it is descending by changing the direction, it should be recognized that it has descended to the floor. The stairs ascent and descent operation ends.

The operation method of the above-described stair climbing process will be described in more detail. FIG. 29 is a flowchart showing the steps of the stair climbing operation. As shown in Fig. 29, when the stair lift control process is started, the search (Search) 'Align (Approach) operation searches for the staircase and confronts the searched staircase. The robot moves to the predetermined position (aligned) and executes an approach operation approaching the first step of the stairs (step S21). If this search 'align' approach is successful (step S22: Yes) Go up and down (step S23) and output success. If the approach fails (step S22: No), the failure is output and the process is terminated. In this case, repeat from step S21 again.

 Here, the sequence of the search alignment approach in step S21 is the same as the control that is normally used by the robot device to reach the object or destination. For example, there are the following methods. FIG. 30 is a flowchart showing the search “align approach method”.

As shown in FIG. 30, when the search “align approach” is started, the search operation (1) is executed (step S32). In the search operation (1), the head is swung to collect as much information as possible. As a result, it is determined whether there are stairs that can be raised and lowered around (step S32). Here, using the height n of the plane that forms the first tread among the detected stairs, if the height satisfies step_min_z <n <step_max_z, it is determined that the ascending / descending is possible. If there is a stair that can be moved up and down (step S32: Yes), in order to avoid recognizing the stair nearby, an align operation is performed to move to a predetermined distance (align_distan _Ce ) (step S33). ) Re-recognize the stairs that are going up and down (step S34). The operation in step S34 is the search operation (2).

 Then, it is reconfirmed whether or not the stairs can be moved up and down (step S35). If the search operation (2) is successful, the stairs that are re-recognized are confronted with a predetermined distance. Check whether or not the movement to the line position has been completed, that is, whether or not the alignment operation in step S33 has been successful (step S36). Steps S35 and S36: Yes), execute an approach to the front edge of the first step (step S37). On the other hand, if there is no stairs that can be moved up and down in step S35, the process returns to step S31, and in step S36, the alignment operation is successful and V ,!

Next, the stair climbing operation in step S22 will be described. Ascending / descending operation is performed when the robot device itself can recognize the upper or lower step (hereinafter referred to as the next step) from the current tread, and when the next step cannot be recognized from the current moving surface. If two or more stages, upper stage or lower stage (hereinafter referred to as two or more stages ahead) can be recognized Lifting / lowering motion processing 2 and lifting / lowering motion processing 3 when a plurality of steps can be recognized. FIG. 31, FIG. 33, and FIG. 34 are flowcharts showing the processing methods of the lifting operation processing;! Here, in the following description, the currently moving stage (including the floor) is indicated by st-0, the next stage is step_l, the next stage is step_2, and the next stage is step-m. And

 First, the elevating operation process when the tread of the next stage (step-1) can be observed and recognized will be described. As shown in FIG. 31, when the ascending / descending operation process 1 is started, an operation of climbing / descending the stairs (crime operation (1)) is executed (step S41). In this climb operation (1), since the height n of the staircase can be recognized in the above step S32, the positive / negative judgment of this height n is made z z

By doing so, the elevation mode is switched. That is, if n <0, it is a descending action and z

 , If n> 0, the climbing operation is performed. When climbing up and down, as described later,

The control parameter value used for the system operation is different. That is, the climbing operation and the descending operation can be switched and executed only by switching the control parameter.

 Then, it is determined whether or not the climb operation (1) is successful (step S42). If the climb operation (1) is successful (step S42: Yes), the search operation (3) is executed (step S43). This search operation (3) is a process that moves the head unit equipped with the stereo vision system, acquires the surrounding distance data, and detects the next staircase, and is almost the same as the search operation (2). Operation processing.

 Then, as in step S35, when the search operation (3) is successful (step S44: Yes), the processing of step S41 is repeated again. If the stairs is not searched for (Step S44: No), it is judged that the stairs have been climbed up or down, and a finishing operation such as moving to a predetermined position such as the center of the stairs is executed (Step S45), the process is terminated. Next, the climb operation (1) in step S41 will be described in detail. FIG. 32 is a schematic diagram showing a staircase surface that the robot apparatus recognizes or intends to recognize. As shown in FIG. 32, for example, it is assumed that the sole 121L / R of the currently moving robot apparatus is on the tread 181. In FIG. 32, the safety region between the front edge FE and the back edge BE and the left and right margins M and M adjacent thereto are recognized as the tread. This

 1 2

In this case, the robot apparatus can recognize the tread surface 182 of the next next stage (st mark-2). Here, it is assumed that there is a gap 184 between the tread 181 and the tread 182 due to the kicking up or the like. To do. In the lifting / lowering operation (1), it is possible to determine whether it is possible to move (climb operation) from the tread 181 of the current stage (st mark-0) to the tread 182 of the next stage (st mark-1). Those that meet the criteria shall be deemed movable.

 5—1: The front edge of the tread 182 of the next stage (st mark-1) The deviation of the angle sufficiently close to the FE is below the predetermined threshold

 5-2: The size of the tread 182 of the next stage (st mark-1) is sufficiently large

 5—3: Front edge FE force is also the distance to the rear end of the sole 121L / R. Front_x is specified Control parameter in lift mode front_x_limi is large

 5—4: Back edge BE force is also the distance to the front edge of the sole 121L / R. Back_x is the control parameter in lift mode back_x_limi is large

 Here, when the tread 183 of the next step (st mark-2) can be observed, the height zl at the reference point of the tread 18 2 of the next step (step-1) and the next step (st mark-2) From the difference in height z2 at the reference point of tread 183 (z2−zl), climbing from tread 182 of the next step (step-l) to tread 18 3 of the next step (st mark-2) It is possible to determine whether the vehicle is down or down. If the tread surface 183 of the second step (step-2) cannot be recognized, the current lifting state may be maintained.

 If it is determined from the above 5— ;! to 5—4 that the climb operation is possible, the climb operation to climb up or down the stairs is executed. The robot apparatus aligns with the back edge BE of the tread 181 at the tread 181 of the current stage (st mark-0). Here, between tread 181 and tread 182

 0

When the gap 184 is large, after moving to the front edge FE of the tread 182 of the next stage (st mark-1), it moves to the next stage and aligns with its back edge BE. In the next climb operation, the alarm is applied to the front edge FE of the tread 183 of the next step (step-2).

 2 in, move to tread 183 and align to back edge BE. That is, for example,

 2

The climb operation is aligned with the front edge FE of the tread on the next stage, and the climb operation is performed until it aligns with the back edge BE of the moved tread.

 When the gap 184 is small, it is assumed that the back edge BE of the tread 181 of the current step (step-0) and the front edge FE of the tread 182 of the next step substantially coincide with each other.

0 1

It is also possible to perform an align operation only on one of the edges. That is, the current tread 181 Align to the back edge BE and move to the next tread 182.

0 1 Aligning force, aligning to the front edge FE of the next step tread 182 and moving to the next step tread, and then aligning to the front edge FE of the next step tread 183. In this case, the climb operation is a process of omitting the process of running to the front edge FE of the next stage and executing the align operation to the back edge BE of the tread moved to the next stage and moved.

 The above-described lifting operation processing 1 can be applied when the robot apparatus can observe the tread surface of the next movable step (st mark-1) during the lifting operation. For example, if it is a biped walking robot device, it is necessary to have a stereo vision system 1 that can look down on its own feet.

 Next, unlike the lifting / lowering process 1 when the tread of the next stage is observable / recognizable, the current stage (due to restrictions on the movable angle of the connection between the head unit and the trunk unit of the robotic device) The tread of the next stage (st mark-1) cannot be observed / recognized from the tread at st mark-0), and the next step (st mark-2) or beyond (st mark-m) Lifting process 2 and 3 when the step surface can be recognized will be described. First, the case where the tread surface of the second step (st mark-2) can be recognized will be described. As shown in FIG. 32, when the lifting / lowering operation process 2 is started, the search operation ( 4) is executed (step S51). In this search operation (4), the stereovision system 1 observes and recognizes the tread of the second step (step-2). This search operation (4) is the same processing as step S43 described above, except that the tread of the second step ahead (st mark-2) is recognized.

 Then, the climb operation (2) is executed (step S52). The climb operation (2) is the same operation as the climb operation (1). In this case as well, switching between climbing and descending stairs in climbing is also determined by the height n of the tread on the next step (step-1).

I can do it. The tread on the next stage (st mark-1) is the tread that was moved in time before the tread on the current stage (step-0)! /, And on the tread on the next stage! / It is.

If the climbing operation is successful (step S53: Yes), if the result of the search in step S51 is that a staircase two steps ahead (st mark-2) is detected (step S54: Yes), it is Update as the next step (st mark-l = st mark -2) (step S56) and repeat the process from step S51. In step S51, the tread of the second step (step-2) is detected in the search operation (4). If not (Step S55: No), the finish operation is executed (Step S55), and the process is terminated.

 In addition, even if it is possible to observe the tread of the third and subsequent steps described here as being able to observe the tread of the second step ahead, the same processing may be performed.

 Next, the up and down motion when the treads up to multiple steps (hereinafter referred to as m steps) can be observed and recognized will be described as up and down motion 3.

 As shown in Fig. 34, the search operation (5) is performed first when the ascending / descending operation is performed when! The search operation (5) is basically the same as step S51, except that the tread surface up to the recognizable m-step ahead (step-m) is the observation target! /.

 Then, the climb operation (3) is executed for k stages (step S62). In the climb operation (3), the lift mode can be determined from the difference in height between the multiple treads that have been observed to date. In other words, if the height z-z of the i-th and i- 1st treads is negative, the operation goes down the stairs, and if it is 0 or positive, the operation mode should go up the stairs. The information on the tread moving in this climb operation (3) is the data that has been observed m m ahead of the current stage.

 If the lifting / lowering operation is successful (step S63: Yes), it is determined whether or not the staircase data ahead of the current tread has been observed, that is, whether m + n> k (step S64). , M + n> k (step S64: Yes), step- (k + l) to step- (n + m) are replaced with step_l to st- (m + n— k) (n = m) Update (step S66) and repeat the process from step S61. On the other hand, if m <0 (step S64: No), there is no tread surface to be moved next, so the finish operation is executed (step S65), and the process ends. As explained above, in steps 1 to 3, the steps for climbing and going down the stairs are the same by simply changing the control parameters used for climbing and descending in climbing. Can be executed. The control parameter used for the stair climbing operation is for regulating the position of the sole of the robot device relative to the current tread.

FIG. 35A is a diagram for explaining the relationship between the tread and the sole recognized by the robot apparatus. FIG. 35B is a diagram illustrating an example of control parameters used for the climb operation.

 The control parameters shown in FIG. 35A are as follows.

 step_min: Minimum difference in height difference between the current stage and the next stage (kick-up) st mark _max: Height difference (kick-up) between the current stage and the next stage can be raised / lowered Maximum value ad (align_distance): Distance between the front edge FE and the robot device at the alignment position

 ax (approach_x): Distance between the front edge FE and the robot device at the approach position

 front_x_limit: Limit value of the distance between the front edge FE on the tread and the rear end of the sole 121 ^ minimal χ-value)

 back_x_limit: Limit value of the distance between the back edge BE on the tread and the front edge of the sole 121 (maximal x-vaiue

 back_x_desired: Desired value of distance between back edge BE and front edge of sole 121

When climbing stairs, kicked is assumed if step_min_z less not considered there is a step (staircase), kicked is greater than Step_max_ _Z judges that stair climbing impossible. Similarly, if you go down the stairs, when kicked up it is greater than the step_max_ _Z is, shall not be regarded as stairs, in the case step_min_z greater than, also of the judges that stair climbing are not allowed.

align_distan _Ce is a parameter that is used only when aligning, and is used when starting up / down stairs operation that performs climbing / descending stairs, that is, when raising / lowering the first stage. . Similarly, it is a parameter used only when approach_x¾, an approach operation, and is used when starting a stair climbing operation that performs a climbing / descending operation.

The front_x_limit and back_x_limit define the relationship between the tread surface that the robot device recognizes! /, and the sole of the robot device. The distance between the back edge BE and front edge FE of the tread surface and the end of the sole In other words, if a large part of the tread when moving to that tread is smaller than these values, it is impossible to move to that tread or even if it can move It is determined that the lifting / lowering operation is impossible. Here, in the climbing operation, the negative values of front_x_limit and back_x_limit indicate that the tread surface is smaller than the sole! In other words, in climbing motion, even if the tread is smaller than the sole of the foot, it can be moved by half the IJ.

 back_x_desired indicates the distance between the back edge BE at the position where the robot device wants to align with the back edge BE on the current tread surface and the front end of the sole, as shown in Fig. 35B. If so, back_x_desired is in front of the knock edge BE, and in this embodiment, 15 mm before the back edge BE. On the other hand, if it is descending, the sole protrudes from the back edge BE. In this embodiment, it is a position that protrudes 5 mm. This is because climbing requires a certain distance to move up and move to the next stage, while descending does not require such a distance and seems to protrude from the tread. This is because it is easier to observe / recognize the tread on the next stage or later.

 FIG. 36 and FIG. 37 are traces of images of the robot apparatus actually performing the ascending / descending operation using the control parameters shown in FIG. Figure 36 shows the movement of the robot device up the stairs. Step S31 search operation (1) Execution state (No. 1), Step S33 alignment operation execution (No. 2), Step S3 2 search operation (2) Execution state (numbered from top to bottom) No. 3), execution of approach operation in step S37 (No. 4), search operation in step S 51 (4) execution (No. 5), climb operation in step S52 (2) execution (No. 6), Continued climb operation (2) of step S52, aligning with the back edge BE of the current tread (No. 7), search operation of step S51 (4) Execution (No. 8), ································ If the next step is not observed after performing the search operation (4) (No. 17), perform the stairs lift operation end (finish) operation! /, Shows the state (Νο · 18).

Fig. 37 shows the descending motion. Similar to the climbing motion of Fig. 36, the search motion (No. 1, No. 4, No. 7, No. 10, No. 13, No. 16), climb Repeated operation (including align operation) (No. 5, No. 6, No. 8, No. 9, No. 11, No. 12, No. 14, No. 15) and the next tread is not observed The finish operation is performed at that time (No. 18) End the operation.

 Next, a modification of the present embodiment will be described. In the above description, the operation of climbing up and down the stairs has been described. However, if the algorithm according to the control method of the present embodiment is applied, not only the stairs composed of a plurality of steps but also the steps composed of only one step. In addition, even if there is a single stepped recess, if the step is below a predetermined value, movement is possible.

 First, the operation of climbing a single step will be described. FIG. 38 is a diagram showing the relationship between a single step and the sole of the robot apparatus. In FIG. 38, reference numeral 191 denotes a step portion whose height from the floor (z = 0) is zl = 30. Here, the robot device has a step portion 191 that is the next step (step l). Explain when moving from the bottom to the top of the page. When the height of the moving surface at the lower side of the paper surface is z0 = 0 and the height of the moving surface at the upper side of the paper surface is z2 = 0 with respect to the step 191, the moving surface of the height ζθ is currently moved. Zl—z0 = 30> 0, so the movement to the next step 191 is judged to be a climbing movement, and z2—zl = —30 to 0, so the next region from step 191 It can be determined that the movement to is an action of getting off. Based on this judgment, the control parameters can be changed! Here, when the control parameters shown in FIG. 35B are used, ront_x_limit and back_x_limit in the case of the climbing motion are both negative and the step 121 of the robot apparatus has a stepped portion as shown in FIG. Indicates that it can be moved even if it is out of 191.

Next, the operation of descending into a single depression (concave) will be described. FIG. 39 is a diagram showing the relationship between a single recess and the sole of the robot apparatus. In FIG. 39, reference numeral 192 denotes a concave portion having a floor surface (z = 0) force and a height of zl = —30. Here, the concave portion 192 where the robot device is the next stage (step l) is shown. A case of moving from the lower side to the upper side of the paper will be described. The height of the moving surface at the lower side of the paper with respect to the concave portion 192 is z0 = 0, and the height of the moving surface at the upper side of the paper is z2 = 0, and the moving surface at the height ζθ is currently moving. If z 1—ζ0 = —30 to 0, the movement to the next recess 192 is judged to move down, and z2 − zl = 30> 0, so the movement from the recess 192 to the next region It is determined that the movement is a climbing action. Therefore, as in the case of the step 191, in the climb operation according to this judgment, What is necessary is just to change the value of the control parameter mentioned above. Here, when the control parameters shown in FIG. 35B are used, ront_x_limit and back_x_limit in the case of the descending motion are both positive values, and the bottom 121 of the robot apparatus is recessed as shown in FIG. It is judged that movement is possible only when it is smaller than 191.

 In the present embodiment, a plane that can be determined to be movable, such as horizontal, is extracted from the detected plane, and the tread surface of the polygonal force stair including that area is recognized. Then, using the information on the treads such as the polygonal front edge FE and back edge BE and the staircase information including the height from the floor, the stairs are moved up and down. In the ascending / descending operation, a search operation is performed on the moving tread, and an align operation is performed on the front edge FE of the tread that has been searched or the back edge BE on the current moving surface, and the next moving surface and the current Judging whether to move up or down based on the difference in height from the moving surface and switching control parameters, it is possible to move up and down not only staircases made of normal rectangular treads but also spiral stairs. At the same time, the climbing and descending operations can be executed in the same procedure by simply changing the control parameters. Therefore, it is possible to move not only to stairs but also to a single step or a single recess by the same control method.

Yes

 In staircase recognition, the recognized staircases are integrated over time, so the reason is, for example, that the staircase is larger than the size of the mouth bot device, or that the position of the stereo vision system installed in the robot device is limited. Even in a robotic device with a limited field of view, staircase information can be recognized over a high frequency range. In addition, when moving up and down using this staircase information, even if the next tread cannot be observed or recognized due to the position restriction of the stereo vision system, it has been observed or recognized in the past. Using the staircase information, you can move up and down in the same way.

The plane detection device in this modification can reliably detect a plurality of planes by the line segment expansion method even when there are a plurality of planes, such as stairs, in addition to the dominant plane in the field of view. In line segment extraction that is performed when a plane is detected, plane detection results that are robust against measurement noise can be obtained by adaptively fitting line segments according to the distribution of the points in the distance data. . FIG. 40 is a functional block diagram showing the flat panel detector in the present modification. As shown in FIG. 40, the flat panel detector 100 is a stereo vision system (Stereo Vision System) 1 as a distance data measurement means for acquiring 3D distance data and a distance image composed of 3D distance data. It has a plane detection unit 2 that detects existing planes by the line segment expansion method. The plane detection unit 2 selects a distance data point group estimated to be in the same plane from the distance data points constituting the image, and extracts a line segment for each distance data point group; An area expansion unit 2b that detects one or a plurality of plane areas existing in the image from a line segment group that includes all line segments extracted by the line segment extraction unit 2a included in the image. The area expansion unit 2b selects any three line segments estimated to exist on the same plane from the line segment group, and obtains a reference plane from these. Then, it is determined whether or not the line segments adjacent to the selected three line segments belong to the same plane as this reference plane. If it is determined that they belong to the same plane, the line segment as the area expansion line segment is determined. The reference plane is updated by and the reference plane area is expanded.

 The line segment extraction unit 2a extracts a distance data point group that is estimated to be on the same plane in the three-dimensional space in each data column for each column or row in the distance image, and this distance data point group. Generate one or more line segments according to the distribution of distance data points. In other words, if it is determined that the distribution is biased, it is determined that the data point group is not on the same plane, the data point group is divided, and whether the distribution is biased again for each of the divided data point groups. The determination process is repeated, and if there is no bias in the distribution, a line segment is generated from the data point group. The above processing is performed for all data strings, and the generated line segment group D11 is output to the area expansion unit 2b.

 In this line segment group D11, the area expanding unit 2b selects three line segments estimated to belong to the same plane, and obtains a plane serving as a reference plane from these. The range image is expanded to multiple areas by expanding the area of this plane area by integrating the line segments that belong to the same plane as the area area. And plane group D2 is output.

The robotic device 201 is an important tool for walking such as staircases, floors, and walls when plane information such as obstacle avoidance and stair climbing is required or by performing these processes periodically. Get face information.

 Here, in order to acquire such 3D distance data by the stereo vision system 1, a pattern (texture) is required on the surface of the staircase ST2. In other words, since it can be obtained by parallax by two cameras, the parallax cannot be calculated if there is no pattern, and the distance cannot be measured accurately. That is, the measurement accuracy of distance data in a stereo vision system depends on the texture to be measured. Note that parallax refers to the difference between a point in the space that is mapped to the left eye and right eye, and changes according to the distance from the camera.

 Therefore, as shown in FIG. 41, the head unit of the robot apparatus is provided with a stereo camera 11R / L that constitutes a stereo vision system and, for example, infrared light or the like as projection means is also applied to the head unit or the like. A light source 12 for output is provided. This light source 12 projects (irradiates) an object, a wall, and other objects with a random pattern PT by applying a pattern, staircase ST3, and other textures! It works as a pattern giving means. In addition, if the random pattern PT can be formed and the distance image can be acquired, the means for applying the random pattern PT is not limited to a light source that projects infrared light, for example, the robot device itself forms a pattern on the object. Although it may be written, if it is infrared light, it is invisible to the human eye, but it can give a pattern that can be observed by a CCD camera or the like mounted on the robot device.

 Next, the plane detection unit 2 of the plane detection apparatus 100 will be described. The plane detection unit 2 detects a plane using the line segment expansion method, and FIG. 42 is a diagram for explaining a plane detection method based on the line segment expansion method. In the plane detection by the line segment expansion method, as shown in FIG. 42, first, in the image 11 taken from the focal point F, processing is performed on the data column in the row direction or the column direction. For example, in a row of pixels in an image (image row), a distance data point that is estimated to belong to the same plane by using a straight line if the distance data points belong to the same plane. Generate a line segment consisting of. Then, in the obtained line segment group consisting of a plurality of line segments, a plane is estimated and detected based on the line segment group that constitutes the same plane.

FIG. 43 is a flowchart showing plane detection processing by the line segment expansion method. Shown in Figure 43 First, a distance image is input (step S71), and a line segment is obtained from data points estimated to belong to the same plane in each pixel column in the row direction (or column direction) of the distance image (step S71). 72). Then, a line segment estimated to belong to the same plane is extracted from these line segment groups, and a plane composed of these line segments is obtained (step S 73). In step S73, first, a region to be a plane seed (hereinafter referred to as a seed region) is selected, and the corresponding region type is selected. In this selection, three line segments including one line in the upper and lower adjacent row directions (or left and right adjacent column directions) are on the same plane. Here, the plane to which the selected region type consisting of the three line segments belongs is used as a reference plane, and a plane obtained by averaging from the three line segments is obtained. An area composed of three line segments is defined as a reference plane area. Then, it is determined whether the straight line composed of the pixel columns in the row direction (or the column direction) adjacent to the selected region type and the reference plane are the same plane by comparing the spatial distances. If there is, add the adjacent line segment to the reference plane area (area expansion process), update the reference plane to include the added line segment (plane update process), and add this to the plane area. Repeat until there is no line segment on the same plane in the adjacent data string. Then, the region type is searched and the plane update and the region expansion process are repeatedly executed until there are no seed regions (three line segments). Finally, from the obtained plurality of region groups, those constituting the same plane are connected. In this modified example, a plane recalculation process for obtaining a plane again by removing a line segment that deviates from the plane by a predetermined threshold or more from the group of line segments belonging to the obtained plane is further provided as step S74. The details will be described later.

Here, the process of detecting line segments from 3D distance data and combining the areas into the same plane as one plane is the plane detection process by the conventional line segment expansion method. In this modification, The line segment extraction method in step S72 is different from the conventional one. In other words, as described above, even if an attempt is made to generate a line segment so as to fit the distance data point as much as possible, if the threshold value is not changed according to the accuracy of the distance data, an over-segmentation Or under-segmentation, etc. In this variation, in this line segment extraction, a method for adaptively changing the threshold according to the accuracy of distance data and noise is analyzed by analyzing the distribution of distance data. To do.

 Hereinafter, the plane detection method based on the line segment expansion method shown in FIG. 43 will be described in more detail. As described above, the line extraction unit (Line Extraction) 2a receives the three-dimensional distance image from the stereo vision system 1 and inputs each column or each row of the distance image on the same plane in the three-dimensional space. Detect the estimated line segment. In this line segment extraction, due to measurement noise, etc., the problem of over-segmentation and under-segmentation, that is, it is recognized as one plane even though it is originally multiple planes. In order to avoid the problem of being recognized as multiple planes even though it is a single plane, an algorithm (Adaptive Line Fitting) that adaptively fits line segments according to the distribution of data points is introduced. In Adaptive Line Fitting, the line segment extraction unit 2a first extracts a line segment as the first line segment using a relatively large threshold value, and then extracts the data belonging to the extracted first line segment. The distribution of the data point group with respect to the line segment as the second line segment obtained from the point group by the least square method described later is analyzed. In other words, it roughly estimates whether or not they exist on the same plane, extracts a data point cloud, analyzes whether or not there is a bias in the distribution of data points in the extracted data point cloud, and exists on the same plane Rethink whether or not

 In this modification, the distribution of the data points is analyzed, and if the data point group applies to the zig-zag-shape described later, the data point group is divided as the distribution is biased. By repeating this process, an algorithm that adaptively extracts line segments from noise contained in the data point group shall be used.

FIG. 44 is a flowchart showing details of the process in the line segment extraction unit 2a, that is, the process of step S72 in FIG. First, distance data is input to the line segment extraction unit 2a. From the input distance data, for example, a pixel column in the row direction (data points that are estimated to exist on the same plane in the three-dimensional space at the data point 歹 are extracted. The same plane in the three-dimensional space. The data point group that is estimated to exist above is, for example, a set of data points whose distance in the three-dimensional space between data points is less than a predetermined threshold, such as distance force S of adjacent data points, for example, 6 cm or less. This is extracted as a data point cloud (Ρ [0 · · · η-1]) (step S81), and this data point cloud Ρ [0 · · · · n-1] is checked whether the number of samples n included is the minimum number of samples required for processing (minimum required value) min_n (step S82), and the number of data n is less than the required minimum value min_n V, In the case (S82: YES), an empty set is output as the detection result and the process is terminated.

 On the other hand, if the number of samples n is greater than or equal to the required minimum value min_n (S82: NO), the data point group P

A line segment (string) L1 connecting one end point P [0] of [0 ·· η-1] and the other end point P [n-1] is generated as the first line segment. Then, from the data point group Ρ [0 ·· η_1], the distance from the line segment L1 is the largest! /, The data point is searched as the point of interest brk, and the distance dist is calculated (step S83). If the maximum distance dist is larger than the data point group division threshold max_d (S84: YES), the data point group data point group Ρ [0 ··· n-1] is assigned to the target point (division point) brk. The data points are divided into two data points ρ [0 · • brk] and P [brk '.. N-1] (step S88).

 On the other hand, if the maximum distance dist is smaller than the data point group division threshold max_d! / (S84: NO), an optimal line is obtained from the data point group Ρ [0 ··· n-1] by the least square method described later. The minute equation line is obtained (step S85), and the line segment L2 indicated by this equation line is generated as the second line segment. Then, it is checked whether the data point group Ρ [0 ·· η−1] is a Zig-Zag-Shape described later for this line segment L2 (step S86). In the case (S86: NO), the obtained line segment equation line is added to the line segment extraction result list (step S87), and the process is terminated.

 If it is determined in step S86 that the line segment obtained in step S85 is Zig-Zag-Shape (S86: YES), the process proceeds to step S88 as in step S84 described above, and the distance dist in step S83. At the point of interest brk, the data point cloud is converted into two data point clouds P

Divide into [0 ·· 'brk] and P [brk' ·· η-1]. When two data point groups are obtained in this step S88, each of them is recursively processed from step S81 again. Then, this process is repeated until all the divided data points are not divided, that is, until all the data point groups have passed through step S87, and as a result, all line segments are registered. Get a list. By such processing, it is possible to accurately detect a line segment group consisting of a plurality of line segments by eliminating the influence of noise from the data point group [0... N-1] force.

In step S83, a line segment L1 connecting the end points of the data point group Ρ [0 ·· η_1] is generated. However, for example, the line segment L1 may be obtained from the data point group Ρ [0 ·· η_1] by least squares as necessary, such as the distribution and properties of the data point group Ρ [0 ·· η_1]. . In this modification, the point of interest brk is one point having the maximum distance from the line segment L1 connecting the end points.For example, the distance from the line segment obtained by the least square as described above is the maximum. If there are multiple points with distances greater than or equal to the data point group division threshold max_d, the data point group Ρ [0 ·· η_1] should be divided at all those points or at one or more selected points. It may be.

 Next, the least squares line segment generation method (Least-Squares Line Fitting) in step S85 will be described. Given n data points Ρ [0 ··· η-1], we show how to find a straight line equation that best fits the data points. The model of the linear equation is expressed by the following equation (1).

Country cos + ysin + = 0 In this case, at one point (x, y) of n data points Ρ [0 ·· η_1], the sum of errors between the model of the linear equation and the data points is It can be expressed by (2).

[Equation 2]

^E flt ⁼ L ( ^x i∞ & + y _t sia + d) ... (2)

 ϊ

 The straight line that best fits the data point group can be obtained by minimizing the sum of the errors in equation (2) above. Α and d that minimize Equation (2) can be obtained as shown in (3) below using the mean and variance covariance matrix of data point group P.

[Equation 3]

1 _! -25

^ ~ 2 ^tan ~ ~ ~ ' ^{d =} ~ ^{[ -x os +} y ^sina ) --- (3)

 However,

s ^ = ∑ (Xi -x) ² = £

ii Next, the Zig-Zag-Shape discrimination method in step S86 will be described! In this Zig-Zag-Shape discrimination, when n data points Ρ [0 ·· η_1] and line Line, d), xcosa + ycosa + d = 0 force S, the data points 点[0 ··· η_1] is the force that intersects the straight line as shown in Fig. 45, and whether the data points are uniformly distributed as a result of noise, for example, as shown in Fig. 45B. It is to be determined. Basically, the number of data points Ρ [0 · η−1] appearing continuously on one side of the line Line is counted, and if it appears continuously beyond a certain constant, zig-zag -shape can be determined. In the case of Fig. 45A, it is necessary to divide the data point group P [i] in order to obtain a straight line that better fits the data point group Ρ [0 ·· η-1]. FIG. 46 is a flowchart showing the Zig-Zag-Shape discrimination method.

 First, a data point group Ρ [0 ·· η_1] and a straight line Line, d, σ) are input (step S90). Here, σ indicates the standard deviation of the point sequence. Next, it is determined whether or not the standard deviation σ is larger than a predetermined threshold th_ (j. If the standard deviation σ is smaller than the threshold th_ (j (step S9 l: No), the floating point of the arithmetic unit is determined. In order to avoid the influence of error detection due to calculation error, the discrimination is terminated. Then, the discrimination processing is continued only when the standard deviation σ is larger than the threshold th_ (r. Next, the data point group Ρ [0 ··· The force that the first data point P [0] of η_1] is on which side of the line is judged by sing (sdist (P [0] >>), and the result is substituted into val and val and

 0 0 Counter that counts the number of consecutive data points on the same side (hereinafter referred to as continuous point counter)

This count value is called a count value count. ) Is set to 1 (step S92). Where sign (x) is a function that returns the sign (+ or-) of the value of X, and sdist (i) is calculated as P [i] .xcos a + P [i] .ycos a + d It shows the positive and negative distance from the i-th data point in the straight line. That is, in Val, which side of the straight line is the data point P [0]

 0

The sign of + or is substituted by.

Next, a count value i of a counter for counting data points (hereinafter referred to as a data point counter, and this count value is referred to as a count value i) is set to 1 (step S93). When the count value i of the data point counter is smaller than the number of data n (step S94: YES), the data point P [i] that is the data point of the next data (hereinafter referred to as i-th) is a straight line. Is determined by sing (sdist (P [i])), and the result is assigned to val (step S95). Then, val obtained in step 92 and val obtained in step S95 are compared, and val and val

 0 If different from 0 (step S96: NO), substitute val for val and count value of continuous point counter

 0

Substitute 1 for count (step S98), increment the count value i of the data point counter (step S100), and return to the processing from step S94.

 On the other hand, if val and val are the same in step S96 (step S96: YES), the data

 0

The points P [i-1] and P [i] are determined to be on the same side with respect to the straight line Line, and the count value count of the continuous point counter is incremented by 1 (step S97). Further, it is determined whether the count value count of the continuous point counter is larger than the minimum number of data points min_c for determining Zig-Zag-Shape (step S99). YES),

Judge as Zig-Zag-Shape, output TRUE and end the process. On the other hand, if the count value count of the continuous point counter is smaller than the minimum number of data points min_c (step S99: NO), proceed to step S100 and increment the count value i of the data point counter (step S 1 00), and repeats the processing from step S94.

 The processing from step S94 is continued until the count value i of the data point counter reaches the data point n, and when the count value i> = n, FALSE is output and the processing is terminated.

 With this zigzag discriminant processing, when n data points Ρ [0 · · · η_1] and a straight line Line (a, d): xcos a + ycos a + d = 0 are given, this data point It can be judged whether the group intersects zig-zag with respect to the straight line Line. As a result, as described above, it is possible to determine whether or not the data point cloud should be divided in step S86. The data point cloud intersects zig-zag with respect to the straight line obtained by the least square. If it is determined that the data point group is to be divided, it is determined that the data point group should be divided, and the process proceeds to step S88. The data point group can be divided using the point of interest brk as the division point. Note that the processing from step S91 to step S100 can also be expressed as shown in FIG.

In addition, such Zig-Zag-Shape discrimination processing can be performed not only by an arithmetic unit but also by hardware. FIG. 48 is a block diagram illustrating a processing unit that performs Zig-Zag-Shape discrimination processing. As shown in Fig. 48, the Zig-Zag-Shape discrimination processing unit 20 receives n data point groups P [0 • · · η_1], and sequentially places each data point P [i] on either side of the straight line. Determine if located The direction discrimination unit 21 that outputs the discrimination result Val, the delay unit 22 for comparing the next data with the result of the direction discrimination unit 21, and the direction discrimination result Val at the data point P [i] The comparison unit 23 that compares the direction discrimination result Val at the data point P [i-1] and the comparison unit 23

 0

When Val = Val, the continuous point counter 24 increments the count value and the continuous point

 0

The comparison unit 25 compares the count value count of the counter 24 with the minimum number of data points min_c read from the minimum number of data points storage unit 26.

 The operation in this Zig-Zag-Shape discrimination processing unit is as follows. That is, the direction discriminating unit 21 obtains a straight line from the data point group Ρ [0 ·· η−1] by the least square method, and obtains a positive / negative distance between each data point P [i] and the straight line. , The positive and negative signs are output. The delay unit 2 2 receives data until the timing at which the positive / negative sign of the next data point P [i] is input when the positive / negative sign for the distance to the line Line of the data point P [i-1] is input. Is stored. The comparison unit 23 compares the positive and negative signs of the data point P [i] and the data point P [i−1], and outputs a signal for incrementing the count value count of the counter 24 if they are the same sign. If the sign is different, a signal that assigns 1 to the count value count is output. The comparison unit 25 compares the count value count with the minimum number of data points min_c, and if the count value count is greater than the minimum number of data points min_c, the data point group Ρ [0 ··· η-1] is zigzag. A signal indicating is output.

 Next, the region expansion unit (Region Growing) 2b shown in FIG. 40 will be described. The area extension unit 2b receives the line segment group obtained by the line segment extraction unit 2a as input, determines which plane each of these line segments belongs to by applying a plane sequence to the plane of the point sequence (Plane Fitting), and gives A region composed of line segments is separated into a plurality of planes (planar regions). The following method is used to separate the planes.

First, three adjacent line segments estimated to be on the same plane are searched from the given line segment group. The plane (reference plane) obtained from these three line segments is the seed of the plane, and the region containing these three line segments is called a seed region. Then, the line segments adjacent to this region type are sequentially judged by whether or not the line segments are in the same plane as the reference plane by applying the plane fitting to the plane of the point sequence (Plane Fitting). If it is determined that it is included, add this line segment to the area type as a line segment for area expansion While enlarging the area, recalculate the equation of the reference plane, including the line segment for enlarging the area. By such processing, all line segments are distributed to any area (plane).

Yes

 FIG. 49 is a schematic diagram for explaining the region expansion processing. As shown in FIG. 49, when a stair 31 composed of a plurality of planes exists in the image 30, for example, three line segments 32a to 32c indicated by bold lines are selected as region types. The region consisting of these three line segments 32a to 32c is the region type. First, one plane (reference plane) P is obtained from these three line segments 32a to 32c. Next, in the data string 33 or 34 adjacent to the outermost line segment 32a or 32c of the region type outside the region type, a line segment that is the same plane as the plane P is selected. Here, it is assumed that the line segment 33a is selected. Next, a plane P ′ composed of these four line segments is obtained, and the reference plane P is updated. Next, if the line segment 34a is selected, a plane P ′ ′ composed of five line segments is obtained, and the plane P ′ is updated. By repeating this, the second tread of the stairs 31 is obtained as a plane 45 surrounded by a broken line. In this way, the region enlargement process is performed until there is no line segment to be added using the selected region type as a seed. Then, when there are no more line segments to add, the process of searching for three line segments that are region types from the image 30 again and executing region enlargement processing is repeated, and three line segments that are region types are found. Repeat step S3 in Fig. 43 until no more.

 Next, a method of estimating a plane equation composed of data points Ρ [0 ··· η_1] (Plane Fitting), a method of selecting a region seed (Selection of seed region), a region seed Region growth processing (Region growing) in which the region is expanded from the above, and post processing (Post processing) that recalculates the obtained plane equation except for those with large errors are described.

 The point P in the three-dimensional space is represented by P = (x, y, z), and the plane equation is expressed by the following equation (4) by its normal vector n (nx, ny, nz) and the non-negative constant d. It is represented by

[Expression 4 xn _x + yn „+ zn _z + d = 0---(4) Here, a stereo camera cannot observe a plane passing through the focal point, that is, Since the plane does not pass through the focal point, d ≠ 0. Therefore, the plane can be obtained as a value that minimizes the value shown in the following equation (5) by the least square method.

[Equation 5: fit (n, d) = _i (pjn + d) ² -... (5) The optimal solution is obtained as n = m / II m II, d = — 1 / || m ||. Where || · || is the size of vector, m is easy as shown in (6-1) below using Cramer's rule to solve simultaneous linear equations by determinants Is the solution of the linear system obtained by

[Equation 6]

A m = b-.- (6-1)

 here,

^{A ¾ ΣPiPi b = ΣPi -.} (6 - 2) This solution or added a new data point or even der connexion when or deleted data points, A represented by the above formula ½- 2) It is possible to recalculate the plane parameters simply by updating the values of and b. Furthermore, in the case of the line segment extraction method in this modification, two moments (first moment: average, second moment: variance) of n data points are known E (p), E (pp)

 T

These can be used to update A and b as shown in (7) below, and can be extended to plane update processing for n data points.

[Equation 7]

AA + nE (pp ^r ), b <-b + «E (p) · b ₇ )

 Once the plane parameters n and d are calculated, the root mean square (RMS) of the plane equation indicating the degree of deviation of the n data point groups from the plane equation can be calculated from the obtained plane equation. residual) (hereinafter referred to as rms) can be calculated by the following equation (8). In this case as well, the following equation (8) is obtained by using the above two moments of n data points.

[Equation 8] (p 广 .p „) =) ² _{> th} one…

As shown in (8) above, the square mean error rms (p · '· ρ) of the plane equation is 0 if each data point is on the obtained plane. Indicates that the data points fit well on the plane! /.

 Next, a method for searching a seed region and a method for updating a plane while enlarging a region from the region type will be described. FIG. 50 is a flowchart showing a procedure of region type search processing and region expansion processing. As shown in FIG. 50, in selecting the region type, first, three line segments (1, 1, 1) adjacent to the data column in the row direction or the column direction used for the line segment extraction, The pixel position in each line segment (1, 1), (1, 1) is the above data.

1 2 3 1 2 2 3

A search is made for duplicates in the direction orthogonal to the data row (step S 101). Each data point has an index indicating the pixel position in the image. For example, when the data point is a line segment in the data column in the row direction, whether or not the index is compared is duplicated in the column direction. Compare When this search is successful (step S102: YES), the above formula (1) is calculated using the above formula (7). As a result, the plane parameters n and d can be determined and used to calculate the mean square error (1, 1, 1) of the plane equation shown in the above equation (8).

 one two Three

Step S103). And the mean square error rms (l, 1, 1) of this plane equation is, for example, lcm

 one two Three

If the threshold is smaller than the predetermined threshold th 1, select these three line segments as the region type rms

(Step S104). If it is greater than the predetermined threshold th 1, the process returns to step S101 again.

Thus, a line segment satisfying the above condition is searched. In addition, the line segment selected as the region type is removed from the list of line segment groups so that it is not used for other plane expansions.

 The region is expanded from the selected region type by the line segment expansion method. That is, first, a line segment that is a candidate to be added to the region type region is searched (step S 105). This area includes an updated area type, which will be described later, when the area type has already been updated. The candidate line segment is the line segment (1) adjacent to the line segment (for example, 1) included in the region type region.

14 As above, it is necessary that the pixel positions of these line segments overlap each other. If the search is successful (step S106: YES), the mean square error rms (1) of the plane equation is Calculate and determine whether this is less than a predetermined threshold th 2 (step S 107), and a smaller rms

In this case, the plane parameter is updated (step S108), and the processing from step S105 is repeated again. Here, the process is repeated until there are no candidate line segments. When there are no more candidate line segments (step S106: NO), the process returns to step S101, and the region type is searched again. Then, when there are no region types included in the line segment group (step S102: NO), the plane parameters obtained so far are output and the process is terminated.

 Here, in this modified example, the region type is searched, whether or not the three line segments belong to the same plane, and whether to belong to the reference plane or the updated plane that has been updated when performing the region expansion process. The above equation (8) is used to determine whether or not. That is, only when the mean square error rms of the plane equation is less than a predetermined threshold value (th_rms), the line segment (group) is estimated to belong to the same plane, and the plane including the line segment is again defined as a plane. Is calculated. By determining whether or not they belong to the same plane using the mean square error rms of the plane equation in this way, it is possible to accurately determine the plane even when it is more robust to noise and includes fine steps. Can be extracted. The reason will be described below.

FIG. 51 is a diagram showing the effect, and is a schematic diagram showing an example in which the mean square error rms of the plane equation is different even if the distance between the end point and the straight line is equal. Here, as in Non-Patent Document 4, when the region expansion process is performed, if the value of the distance D between the end point of the target straight line (line segment) and the plane P is smaller than a predetermined threshold, the target When the region expansion process is performed assuming that the line segment is the same plane as the plane P, a straight line La intersecting the plane P (Fig. 51A) and a straight line Lb parallel to the plane P and shifted by a predetermined distance (Fig. 51B) Are used to update the plane P as well. Here, when the root mean square error rms of the plane equation is calculated, the square mean error rms (La) of the plane equation obtained from the straight line La in Fig. 51A is compared to 2 in the plane equation obtained from the straight line Lb in Fig. The root mean square error rms (Lb) is larger. That is, when the straight line La and the plane P intersect as shown in Fig. 51A, the mean square error rms of the plane equation is relatively small and often has an effect of noise, whereas as shown in Fig. 51B. In this case, the mean square error rm s of the plane equation is large, and there is a high probability that the straight line Lb is not the same plane as the plane P but a different plane P ′. Therefore, when it is necessary to accurately determine the plane from an environment that includes multiple planes, the root mean square error rms of the plane equation is calculated as in this modification. When the value is less than a predetermined threshold, it is preferable to determine the same plane. Depending on the environment and the nature of the distance data, if the distance between the end point of the line segment and the plane is less than or equal to the predetermined threshold value, the line segment may be included in the plane, or a combination of these. Good

Yes

 Once the surface parameters n and d are calculated, the mean square error rms of the plane equation is updated from the two moment values obtained during line segment extraction for the data point group. It can be easily calculated by the above equation (8).

 In addition, the above-described region type selection method can also be expressed as shown in FIG. overlapd, 1) indicates that the position between the end points in the line vectors 1 and 1 included in each image row is a straight line j k j k

A tuttle is a function that outputs true when overlapping at an orthogonal position. FitPlaned, 1, 1) finds the solution of Am = b by the above equations (4) to (7) and sets the plane parameters n and d

 one two Three

The line vector 1, 1, 1 is transformed into the plane by A, calculated by the above equation (8).

 one two Three

Is a function that

 rms (l, 1, 1) is expressed as 2 in the plane equation on all three lines using equation (6) above.

 one two Three

This function calculates the value of the root mean square error rms. Also, removed, 1, 1) is lines [i], lines

 one two Three

 [i + l] l, lines [i + 2] are divided by lines 1, 1, 1, respectively, which are selected to constitute the region type.

2 1 2 3, which prevents these straight lines from being used again for calculations. Further, the area expansion process can also be expressed as shown in FIG. In Fig. 53, A and b are the matrix and vector shown in the above equation (6-1), respectively, and add (A, b, 1) is a straight line between A and b by the above equation (8). It is a function that adds the moment of. Solve (A, b) calculates m satisfying Am = b, and calculates the plane parameters n and d by the above equations (4) to (7). select (open) is a function that selects one element arbitrarily, such as the first one, such as the first one. Index (l) is a function that returns an index of 1 in a pixel column or row.

 1 1

Is a number. Also, neighbor (index) is a function that returns an index adjacent to the given index, for example, {index-1, index + 1}.

Further, as described above, in the present modification, after the region expansion process is performed in step S73 of FIG. 43 to update the plane equation, the process of calculating the plane equation again (Post processing) is performed in step S74. . In this recalculation process, for example, the above-mentioned The distance data point or line segment that is determined to belong to the plane indicated by the plane equation that is updated and finally obtained is calculated from the plane of the distance data point or line segment. Except for this, the plane equation is updated again, and the effect of noise is further reduced by the force S.

 Next, step S74 will be described in detail. Here, we explain how to calculate the plane equation again in two steps. First, in the distance data point (pixels) of the boundary of each plane detected in step S73, if a data point closer to the adjacent plane than the currently belonging plane is detected, the data Process to include the point toward the adjacent plane. In addition, if a data point that does not belong to any plane and has a plane whose distance is less than a relatively large threshold, such as 1.5 cm, can be detected, the data point is included in that plane. These processes can be executed by searching for data points near the boundary of each planar area. When the above processing is completed, the plane equation is calculated again.

 Next, when the distance between each data point and the plane exceeds a relatively small threshold, such as 0.75 cm, in the vicinity of the boundary of each area of the plane recalculated as described above, those Execute processing to discard data points. As a result, the plane area is slightly reduced, but a more accurate plane can be obtained. After deleting the distance data point, find the plane again and repeat this process. As a result, the plane can be obtained very precisely. Next, the result obtained by each process is shown. Fig. 54A is a schematic diagram showing the floor surface when the robot device is standing down, and Fig. 54B is a graph in which the vertical axis represents x, the horizontal axis represents y, and the z-axis represents the density of each data point. 3D is a diagram showing three-dimensional distance data, and further shows a straight line detected from a data point group that exists in the same plane by line segment extraction processing from a pixel column in the row direction. FIG. 54C shows a planar region obtained by the region expansion process for the straight line group force shown in FIG. 54B. Thus, it can be seen that there is only one plane (floor surface) in the field of view of the robot apparatus, that is, the floor surfaces are all detected as the same plane.

Next, Fig. 55 shows the results when one step is placed on the floor. As shown in FIG. 55A, on the floor surface F, one step ST3 is placed. FIG. 55B is a diagram showing experimental conditions. When the distance force ¾ax_d between the target point and a straight line (line segment) is exceeded, the data point group is divided. Ma The extraction success / failure (horizontal) indicates the number of successful plane detections using line segment expansion, which performs a total of 10 line segment extractions for each data column in the row direction. Correct extraction (vertical) indicates success or failure of extraction for each data column in the column direction. No. l to No. 5 are the conditions for plane detection processing by the conventional line segment expansion method that does not incorporate the Zig-Zag-Shape discrimination processing described above, and No. 6 is

The conditions of the plane detection method in this modified example in which the Zig-Zag-Shape discrimination process is performed are shown. 55C and 55D are diagrams showing the results of plane detection by the line segment expansion method. The results of plane detection by the method in this modification example and the results of plane detection by the conventional line segment expansion method, respectively. (Comparative example) is shown. As shown in Fig. 55B, in the conventional method, when the threshold parameter max_d for estimation is increased (max_d = 25, 30) in line fitting, the detection rate decreases and the threshold max_d is decreased (max_d = 10, 15) and the detection rate is improved. On the other hand, it can be seen that by introducing the zigzag verification processing as in the present invention, an excellent detection result is exhibited even when a large threshold value max_d = 30 is set.

 That is, if the threshold value max_d is increased, the influence of noise is reduced, but line segment extraction becomes difficult. If the threshold value max_d is reduced, erroneous detection increases due to the influence of noise. Figures 56B and 56C show the case where 3D distance data is acquired from the image of the floor shown in Figure 56B. The left figure shows an example in which a line segment is extracted from a pixel column (distance data string) in the row direction, and the right figure shows an example in which a line segment is extracted from a pixel column (distance data string) in the column direction. As shown in Fig. 56B, if the threshold max_d is decreased, the effect of noise increases, and line segments cannot be detected well, especially in the far field where the effect of noise is large. On the other hand, as shown in FIG. 56C, when the zigzag discrimination processing is further added to the conventional line segment extraction, even if the threshold max_d is increased, the line segment is detected even in a distant area where the influence of noise is larger. The power that is detected.

 As a result, plane detection can be performed by acquiring three-dimensional distance data from images obtained by photographing different stairs as described above. For example, as shown in FIGS. 11 and 12, all the treads can be detected as planes in any case. In FIG. 12B, a part of the floor is successfully detected as another plane!

According to this modification, when performing plane detection by the line segment expansion method, a large threshold value is initially set. If the line segment is zigzag even if it does not have a data point that exceeds the threshold by Zig-Zag-Shape discrimination processing, the line segment is divided into multiple planes that are not noise. Since it is assumed that the line segment is divided, it is possible to accurately detect a plurality of planes from distance information including noise.

 In this way, since a small step can be detected with high accuracy, it is possible to recognize, for example, a staircase in an environment in which the robot apparatus can move, and if this is a biped robot apparatus, this detection result can be used. The stairs can be moved up and down.

 Furthermore, it is no longer misidentified that the uneven floor composed of a plurality of planes is a plane that can be walked, and the movement of the robot apparatus and the like are further simplified.

 Of course, the present invention is not limited to the above-described modifications, and various modifications can be made without departing from the spirit of the present invention. In addition, one or more of the above-described plane detection process, staircase recognition process, and stair climbing control process can be realized by executing a computer program on a computing unit (CPU) even if it is configured with hardware. May be. In the case of a computer program, it can be provided by being recorded on a recording medium, or can be provided by being transmitted through the Internet or other transmission media.

Claims

The scope of the claims

 [1] 1. In robotic devices that can be moved by moving means,

 Plane detecting means for detecting one or more planes included in the environment from the three-dimensional distance data and outputting as plane information;

 Stair recognition means for recognizing a stair having a movable plane from the plane information and outputting step information on the tread of the stair and information on kicking, and whether or not the stair can be raised or lowered based on the stair information If it is determined that it is possible to move up and down, it has a stair lift control means that autonomously positions it relative to the tread surface and controls the stair lift operation.

 A robot apparatus characterized by that.

[2] 2.Has distance measurement means to obtain the above 3D distance data

 The robot apparatus according to claim 1, wherein the robot apparatus is characterized in that:

[3] 3. A leg is provided as the moving means and can be moved by the leg.

 The robot apparatus according to claim 1, wherein the robot apparatus is characterized in that:

[4] 4. The above staircase recognition means is

 Stair detection means for detecting a stair having a movable plane from given plane information and outputting the pre-integration stair information;

 And a step integration unit that outputs integrated step information as the step information by statistically processing a plurality of pre-integration step information output from the step detection unit.

 The robot apparatus according to claim 1, wherein the robot apparatus is characterized in that:

[5] 5. The stair detection means recognizes the size and spatial position of the tread based on the plane information, outputs the tread information as a recognition result as the pre-integration stair information, and the stair integration means. Is a case where a tread group consisting of two or more treads that have overlapping areas that are larger than a predetermined threshold and whose relative height is less than or equal to a predetermined threshold is detected from tread information that fluctuates in time. 5. The robot apparatus according to claim 4, wherein the tread group is integrated so as to become one tread including all of the tread groups.

[6] 6. The staircase recognition means determines the size and spatial position of the tread based on the plane information. Recognize and use the above tread information

 The robot apparatus according to claim 1, wherein the robot apparatus is characterized in that:

[7] 7. The above tread information includes at least information on the front edge indicating the front boundary of the tread and the back edge indicating the back boundary with respect to the moving direction.

 The robot apparatus according to claim 6, wherein:

[8] 8. The above tread information is a margin area that is estimated to be movable because it is an area adjacent to the left and right sides of the safety area that is sandwiched between the front edge and the back edge. Right margin information and left margin information indicating

 The robot apparatus according to claim 7, wherein the robot apparatus is characterized in that:

[9] 9. The tread information includes reference point information indicating the center of gravity of the area estimated as the tread based on the plane information.

 The robot apparatus according to claim 7, wherein the robot apparatus is characterized in that:

[10] 10. The above reference point information has a high probability of being movable because it is the center of gravity of the safety area indicating the area between the front edge and the back edge, and the safety area and areas adjacent to both sides thereof. The position information of the reference point consisting of the center of gravity of the tread area consisting of the margin area estimated as! /, Or any of the center points obtained from the point cloud constituting the tread plane is provided. The robot device according to claim 9 in the range.

[11] 11. The robot device according to claim 7, wherein the tread information includes three-dimensional coordinate information of a point group constituting a plane to be a tread.

[12] 12. The staircase recognition means calculates a polygon by extracting the boundary of the plane based on the plane information, and calculates the tread information based on the polygon.

 The robot apparatus according to claim 1, wherein the robot apparatus is characterized in that:

[13] 13. The polygon is a convex polygon area circumscribing the boundary of the plane extracted based on the plane information.

 13. The robot apparatus according to claim 12, wherein the robot apparatus is characterized in that:

[14] 14. The polygon is a convex polygon region inscribed in the boundary of the plane extracted based on the plane information.

13. The robot apparatus according to claim 12, wherein the robot apparatus is characterized in that:

15. The robot apparatus according to claim 12, wherein the polygon is a non-convex polygon region obtained by smoothing a boundary of a plane extracted based on the plane information.

 [16] 16. For each plane, the plane information is selected from a normal vector, boundary information indicating the boundary, barycentric information indicating the barycentric position, area information indicating the size, and flatness. Have information on

 The robot apparatus according to claim 1, wherein the robot apparatus is characterized in that:

[17] 17. The above-mentioned stair-climbing control means controls to execute a lifting operation after moving to a predetermined position facing the back edge on the moving surface that is currently moving.

 4. The robot apparatus according to claim 3, wherein

[18] 18. If the back edge on the moving surface that is currently moving cannot be confirmed, the stair lift control means moves to a predetermined position in front of the front edge on the next step surface that is the target of the next lift operation. And then control to perform the lifting operation

 18. The robot apparatus according to claim 17, wherein the robot apparatus is characterized in that:

[19] 19. The above-mentioned stair climbing control means detects the tread surface to be moved next, and performs a series of operations of moving to a predetermined position facing the tread surface to be moved to perform the lifting operation. Do

 4. The robot apparatus according to claim 3, wherein

[20] 20. If the next step or next tread surface to be moved next cannot be detected from the current position, the above-mentioned stair climbing control means obtains the stair information power acquired in the past for the next step to be moved. Search for treads

 20. The robot apparatus according to claim 19, wherein the robot apparatus is characterized in that:

[21] 21. The stair-climbing control means detects a tread surface to be moved next after moving to a predetermined position facing the back edge on the current moving surface, and detects a predetermined surface facing the front edge on the tread surface. Move to a position and control to move up and down on the tread

 4. The robot apparatus according to claim 3, wherein

[22] 22. The stair lift control means controls the lift operation using a parameter that defines the position of the moving means relative to the tread. 4. The robot apparatus according to claim 3, wherein

[23] 23. The robot device according to claim 22, wherein the parameter is determined based on a foot raising height or a leg lowering height of the leg portion.

[24] 24. Has parameter switching means for changing the numerical values of the above parameters depending on the movement up and down the stairs.

 23. The robot apparatus according to claim 22, wherein the robot apparatus is characterized in that:

[25] 25. The plane detection means is extracted by a line segment extraction means for extracting a line segment for each distance data point group estimated to be on the same plane in a three-dimensional space, and the line segment extraction means. Plane area expanding means for extracting a plurality of line segments estimated to belong to the same plane from the group of line segments and calculating a plane from the plurality of line segments;

 The robot apparatus according to claim 1, wherein the line segment extraction means adaptively extracts a line segment according to a distribution of distance data points.

[26] 26. The line segment extraction means extracts a distance data point group that is estimated to be on the same plane based on the distance between the distance data points, and determines the distribution of the distance data points in the distance data point group. 26. The robot apparatus according to claim 25, wherein the power of whether or not the distance data point group is on the same plane is estimated again based on the following.

[27] 27. The line segment extraction unit extracts a line segment from the distance data point group estimated to be on the same plane, and the distance between the distance data point group and the line segment is the largest. If the data point is a point of interest, and the distance is less than or equal to a predetermined threshold value, it is determined whether or not the distribution of the distance data points in the distance data point group is biased. Divide the distance data point cloud by point

 26. The robot apparatus according to claim 25, wherein

[28] 28. The line segment extracting means extracts a first line segment from the distance data point group estimated to be on the same plane, and the first line segment of the distance data point group is extracted. If the distance data point with the largest distance is the point of interest, and the distance is less than or equal to a predetermined threshold, a second line segment is extracted from the distance data point group, and one side of the second line segment is extracted. It is determined whether or not there are more than a predetermined number of distance data points. If there are more than a predetermined number of data points, the distance data point group is divided at the point of interest. 26. The robot apparatus according to claim 25, wherein

[29] 29. The plane area expanding means selects one or more line segments estimated to belong to the same plane, calculates a reference plane, and calculates a line estimated to belong to the same plane as the reference plane. The segment is retrieved from the line segment group as an extension line segment, the process of updating the reference plane with the extension line segment and expanding the area of the reference plane is repeated, and the updated plane is updated to the updated plane. Output as

 26. The robot apparatus according to claim 25, wherein

[30] 30. In the distance data point group belonging to the updated plane, if there is a distance data point whose distance from the updated plane exceeds a predetermined threshold, this is excluded from the distance data point group It further has plane recalculation means for calculating the plane again.

 30. The robot apparatus according to claim 29, wherein

[31] 31. The plane area expanding means estimates whether or not the line segment belongs to the same plane as the reference plane based on an error between the plane determined by the line segment and the reference plane.

 26. The robot apparatus according to claim 25, wherein

[32] 32. In an operation control method of a robot apparatus movable by a moving means,

 A plane detection step of detecting one or more planes included in the environment from the three-dimensional distance data and outputting as plane information;

 A staircase recognition step for recognizing a staircase having a movable plane from the plane information and outputting step information about the tread surface of the staircase and kicking information, and whether or not the stairs can be raised or lowered based on the staircase information. , And if it is determined that the elevating operation is possible, a stair ascending / descending control process for autonomously positioning with respect to the tread to control the stair ascending / descending operation

 An operation control method for a robot apparatus, comprising:

[33] 33. Having a distance measurement process for acquiring the above three-dimensional distance data

 33. The operation control method for a robot apparatus according to claim 32, wherein

[34] 34. The moving means is a body leg.

 33. The operation control method for a robot apparatus according to claim 32, wherein

[35] 35. The above staircase recognition process is A staircase detection step of detecting a staircase having a movable plane from given plane information and outputting pre-integration staircase information;

 A step integration step of outputting the integrated staircase information that is integrated by statistically processing a plurality of pre-integration staircase information that is output in the staircase detection step as the staircase information.

 33. The operation control method for a robot apparatus according to claim 32, wherein

[36] 36. In the staircase detection step, the size and spatial position of the tread are recognized based on the plane information, and the tread information that is the recognition result is output as the pre-integration staircase information. In the process, when a tread group consisting of two or more treads having an overlapping area larger than a predetermined threshold and having a relative height difference equal to or less than a predetermined threshold is detected from tread information that moves in time, 36. The operation control method for a robot apparatus according to claim 35, wherein the steps are integrated so as to be one tread including all the tread groups.

[37] 37. In the step recognition process, the size and spatial position of the tread are recognized based on the plane information and used as the tread information.

 33. The operation control method for a robot apparatus according to claim 32, wherein

[38] 38. The tread information includes at least information indicating a front edge indicating a front boundary of the tread and a back edge indicating a back boundary with respect to the moving direction.

 38. The operation control method for a robot apparatus according to claim 37, wherein:

[39] 39. In the staircase recognition step, a plane boundary is extracted based on the plane information to calculate a polygon, and the tread information is calculated based on the polygon.

 33. The operation control method for a robot apparatus according to claim 32, wherein

[40] 40. In the above-described stair lift control process, control is performed to execute a lift operation after moving to a predetermined position against the back edge on the currently moving moving surface.

 35. The operation control method for a robot apparatus according to claim 34, wherein

[41] 41. If the back edge on the moving surface that is currently being moved cannot be confirmed in the above-mentioned step-up / down control process, move to a predetermined position facing the front edge of the next step surface that is the target of the lifting / lowering operation. And then control to perform the lifting operation

41. The operation control method for a robot apparatus according to claim 40, wherein:

[42] 42. In the above-described step-up / down control step, a step surface to be moved next is detected, and a series of operations for moving to a predetermined position facing the step surface is performed to control to perform the up-and-down operation. 35. The operation control method for a robot apparatus according to claim 34.

 [43] 43. In the above-mentioned step up / down control process, if the next step or the next step to be moved cannot be detected from the current position, the stair information force acquired in the past is the next step to be moved. Search for

 43. The operation control method for a robot apparatus according to claim 42, wherein

 [44] 44. In the above-mentioned step up / down control process, after moving to a predetermined position facing the back edge on the current moving surface, the next tread surface to be moved is detected, and the predetermined step facing the front edge on the tread surface is detected. Move to the position and control to perform the lifting operation to move to the tread

 35. The operation control method for a robot apparatus according to claim 34, wherein

[45] 45. In the above-mentioned step-up / down control process, the lifting / lowering operation is controlled using a parameter for defining the position of the moving means relative to the tread.

 35. The operation control method for a robot apparatus according to claim 34, wherein

[46] 46. Has a parameter switching process that changes the numerical values of the above parameters depending on the movement up and down the stairs.

 46. The operation control method for a robot apparatus according to claim 45, wherein:

[47] 47. In a moving device movable by moving means,

 Plane detecting means for detecting one or more planes included in the environment from the three-dimensional distance data and outputting as plane information;

 Stair recognition means for recognizing a stair having a movable plane from the plane information and outputting step information on the tread of the stair and information on kicking, and whether or not the stair can be raised or lowered based on the stair information , And when it is determined that the elevating operation is possible, it has stair ascending / descending control means for autonomously positioning with respect to the tread and controlling the stair ascending / descending operation.

 A mobile device characterized by that.