JP2007041657A - Moving body control method, and moving body - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a moving body control method capable of moving on a path by selecting a proper moving motion even when an available moving motion of the moving body is changed. <P>SOLUTION: A path decision section 36 identifies processing sections 35_1 to 35_6, etc. which can be used by a CPU 15 as functions, and dynamically identifies a plurality of paths to a target position, based on the above usable processing sections. The path decision section 36 selectively instructs the processing sections 35_1 to 35_6 in succession, so as to make a robot 1 perform a running motion. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to a moving body control method for controlling a moving body that moves based on a captured image, and the moving body.

  For example, unlike an industrial robot, an autonomous robot is a robot that can operate autonomously according to the surrounding external state or the internal state of the robot itself. For example, an autonomous robot must be able to plan an action path that detects an external obstacle and avoids the obstacle in walking motion. Among conventional robots, a robot that can create an obstacle map of its own environment and determine an action route based on the obstacle map has been proposed.

US Pat. No. 5,0069,884 JP 2003-269937 A

By the way, a conventional robot determines a path on which the robot moves and operates the robot on the assumption that the functions that the robot itself has, such as normal walking, stairs running, climbing movement, jumping movement, etc. are normal. ing.
By the way, some of the functions described above may fail, but in conventional robots, on the assumption that all functions operate normally, in order to sequentially determine the functions to be used in the movement process on the path, In some cases, the malfunctioning function is selected and the robot cannot move along the path.

  In order to solve the above-described problems of the prior art, the present invention provides a moving body control method and a moving body capable of selecting an appropriate moving action and moving on a path even when the moving action available to the moving body changes. The purpose is to provide.

  In order to solve the above-described problems of the prior art and achieve the above-described object, the mobile body control method of the first aspect of the invention is a mobile body control method for controlling the movement of the mobile body to the target position. A first step of detecting a state of a path from the current position of the moving body to the target position, a second step of identifying a plurality of moving operations that can be performed by the moving body, and the first step Based on the state of the path detected in the process, sequentially select a movement operation suitable for the position of the moving body from among a plurality of types of movement operations that can be executed by the moving body specified in the second step. And a third step of controlling the moving body to perform the selected moving operation.

  The moving body according to the second aspect of the invention is based on the imaging result generated by the imaging means, the traveling means, and the imaging result generated by the imaging means. The state of the path from the current position of the body to the target position is detected, a plurality of movement operations that can be executed by the moving body are specified, and the second step is specified based on the detected state of the path Among the plurality of types of moving operations that can be executed by the moving body, the moving means that sequentially selects a moving operation that matches the position state of the moving body, and the traveling unit performs the selected moving operation. Control means for controlling.

  According to the present invention, it is possible to provide a moving body control method and a moving body capable of selecting an appropriate moving action and moving on a path even when a moving action that can be used by the moving body changes.

Hereinafter, a robot according to an embodiment of the present invention will be described.
First, the correspondence between the components of the present embodiment and the components of the present invention will be described.
The process of the environment class classification unit 34 illustrated in FIG. 5 is an example of the first process of the present invention, and the process of step ST61 of the path determination unit 36 illustrated in FIG. 22 is an example of the second process of the present invention. Steps ST62 to ST64 and the processes of FIGS. 24 and 25 are examples of the third step of the present invention.
Moreover, the process of FIG. 23 is an example of the 4th process of this invention.

  Further, the CCD cameras 10R and 10L shown in FIG. 1 are examples of the image pickup means of the present invention, the legs shown in FIGS. 3 and 4 are examples of the travel means of the present invention, and the CPU 15 shown in FIG. It is an example of the control means of invention.

In the robot 1 of the present embodiment, as shown in FIG. 5 and the like, the robot 1 identifies the processing units 35_1 to 35_6 and the like that can be used as functions by the CPU 15 (step ST61 shown in FIG. 22), and the processing units that are used. Based on the above, a plurality of paths to the target position are dynamically specified (step ST62).
Thereby, for example, when a machine part such as the processing units 35_1 to 35_6 or the actuator group 16 controlled thereby fails, a path reaching the target position without using control by the processing unit corresponding to the failed function is obtained. You can choose.

FIG. 1 is a functional block diagram of a robot 1 according to an embodiment of the present invention.
As shown in FIG. 1, the robot 1 includes, for example, CCD cameras 10R and 10L, a stereo image processing device 12, a CPU 15, an actuator group 16, and a sensor group 17.
The CCD cameras 10R and 10L output the right image data S10R and the left image data S10L corresponding to the imaging results to the stereo image processing device 12, respectively.

The stereo image processing device 12 calculates disparity information (distance information) between the right image data S10R and the left image data S10L input from the CCD cameras 10R and 10L, and color image data (YUV: luminance Y, UV color difference). ) S12a and parallax image data (YDR: luminance Y, parallax D, reliability R) S12b are alternately calculated for each frame.
Here, the parallax indicates a difference between points where a certain point in the space is mapped to the left camera and the right camera, and changes according to the distance from the camera.
The color image data S12a and the parallax image data S12b are input to a CPU (control unit) 15.
The CPU 15 uses the color image data S12a and the parallax image data S12b input from the stereo image processing device 12 to perform image processing with a software configuration described later.

The actuator group 16 is various actuators that drive the movement of the robot 1.
The actuator group 16 is driven by a control signal S15 from the CPU 15.
The sensor group 17 is a non-contact type sensor such as an infrared sensor or a contact type sensor installed at a predetermined location of the robot 1.
The sensor group 17 outputs a detection signal S17 to the CPU 15.

Hereinafter, the configuration of the robot 1 shown in FIG. 1 will be described in more detail.
As shown in FIG. 2, the robot 1 is a biped walking type and is a practical robot that supports human activities in various scenes in daily life.
The robot 1 is an entertainment robot that can act in accordance with the internal state (anger, sadness, joy, enjoyment, etc.) and express basic actions performed by humans. The robot generates three-dimensional environment class data and determines an action route based on this.

  As shown in FIG. 2, the robot 1 includes 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 205R / L. Are connected.

A joint degree-of-freedom configuration of the robot 1 shown in FIG. 2 is schematically shown in FIG.
The neck joint that supports the head unit 203 has three degrees of freedom: a neck joint yaw axis 101, a neck joint pitch axis 102, and a neck joint roll axis 103.

Each arm unit 204R / L constituting the upper limb includes a shoulder joint pitch axis 107, a shoulder joint roll axis 108, an upper arm yaw axis 109, an elbow joint pitch axis 110, a forearm yaw axis 111, and a wrist. A joint pitch axis 112, a wrist joint roll axis 113, and a hand part 114 are included.
The hand portion 114 is actually a multi-joint / multi-degree-of-freedom structure including a plurality of fingers. However, since the movement of the hand portion 114 has little contribution or influence on the posture control or walking control of the robot 1, it is assumed in this specification that the degree of freedom is zero. Therefore, it is assumed that each arm portion has seven degrees of freedom.
The trunk unit 202 has three degrees of freedom: a trunk pitch axis 104, a trunk roll axis 105, and a trunk yaw axis 106.

Each leg unit 205R / L constituting the lower limb includes a hip joint yaw axis 115, a hip joint pitch axis 116, a hip joint roll axis 117, a knee joint pitch axis 118, an ankle joint pitch axis 119, and an ankle joint. A roll shaft 120 and a sole 121 are included.
In this 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 1. The human foot sole 121 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 1 has zero degrees of freedom. Accordingly, each leg is configured with 6 degrees of freedom.
In summary, the robot 1 as a whole has a total of 3 + 7 × 2 + 3 + 6 × 2 = 32 degrees of freedom. However, the robot 1 for entertainment is not necessarily limited to 32 degrees of freedom. Needless to say, the degree of freedom, that is, the number of joints, can be increased or decreased as appropriate in accordance with design / production constraints or required specifications.

Each degree of freedom of the robot 1 as described above is actually implemented using an actuator. It is preferable that the actuator be small and light in light of demands such as eliminating external bulges on the appearance and approximating the shape of a human body, and performing posture control on an unstable structure such as biped walking. .
The actuator group 16 shown in FIG. 1 is constituted by the actuators shown in FIGS. 3 and 4, and the sensor group 17 shown in FIG. 1 is constituted by the sensors shown in FIGS.

Such a robot includes a control system that controls the operation of the entire robot, for example, in the trunk unit 202.
3 and 4 are schematic views showing the control system configuration of the robot 1 of the present embodiment.
As shown in FIG. 3 and FIG. 4, the control system performs the whole body cooperative movement of the robot 1, such as driving the actuator 350 and the thinking control module 200 that controls emotion judgment and emotion expression dynamically in response to user input and the like. It is comprised with the exercise | movement control module 300 to control.

  The thought control module 200 includes a CPU (Central Processing Unit) 211, a RAM (Random Access Memory) 212, a ROM (Read Only Memory) 213, and an external storage device (hard disk / disk This is an independent drive type information processing apparatus that is configured with 214 and the like and can perform self-contained processing within the module.

The thought control module 200 determines the current emotion and intention of the robot 1 according to 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. 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 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.
In the present embodiment, the image input device 251 corresponds to the CCD cameras 10R and 10L shown in FIG.
The voice input device 252 includes a plurality of microphones, for example.

  The thought control module 200 issues a command to the movement control module 300 to execute an action or action sequence based on decision making, that is, movement of the limbs.

The motion control module 300 includes a CPU 311 (CPU 15 shown in FIG. 1), a RAM 312, a ROM 313, an external storage device (hard disk drive, etc.) 314, etc. that control the whole body cooperative motion of the robot 1. This is an independent drive type information processing apparatus capable of performing a complete process. Also, 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 degrees of freedom of joints distributed over the whole body of the robot 1 shown in FIG. 2, a distance measurement sensor (not shown) that measures the distance to the object, and the trunk A posture sensor 351 for measuring the posture and inclination of the unit 202, grounding confirmation sensors 352 and 353 for detecting the left or right foot floor getting off or landing, a load sensor provided on the sole 121 of the sole 121, a battery, etc. Various devices such as a power control device 354 to be managed are connected via a bus interface (I / F) 310.
Here, the attitude 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 proximity sensors, micro switches, or the like.

The thought control module 200 and the motion control module 300 are constructed on a common platform, and are interconnected via bus interfaces 210 and 310.

The motion control module 300 controls the whole body cooperative motion by each actuator 350 in order to embody the action instructed from 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 generates an operation pattern internally. Then, the CPU 311 sets a foot movement, a ZMP trajectory, a trunk movement, an upper limb movement, a horizontal waist position, a height, and the like according to a specified movement pattern, and instructs a movement according to these setting contents. The command value is transferred to each actuator 350.

  The CPU 311 detects the posture and inclination of the trunk unit 202 of the robot 1 based on the output signal of the posture sensor 351, and each leg unit 205R / L detects whether the leg unit 205R / L is a free leg or a stance based on the output signals of the ground contact confirmation sensors 352 and 353. By detecting which state it is, the whole body cooperative movement of the robot 1 can be adaptively controlled. Further, the CPU 311 controls the posture and operation of the robot 1 so that the ZMP position always moves toward the center of the ZMP stable region.

In addition, the motion control module 300 indicates how much the intended behavior determined in the thought control module 200 is expressed, that is, the processing status.
It returns to 0. In this way, the robot 1 can determine its own and surrounding conditions based on the control program and can act autonomously.

The function of the CPU 15 (CPU 311) shown in FIGS. 1 and 4 will be described below.
FIG. 5 is a functional block diagram of the CPU 15 shown in FIGS. 1 and 4.
As illustrated in FIG. 5, the CPU 15 includes, for example, a distance data generation unit 31, a plane detection unit 32, a grid map generation unit 33, an environment class classification unit 34, a normal travel processing unit 35_1, a staircase travel processing unit 35_2, and a caution travel process. A unit 35_3, a heel motion processing unit 35_4, a step over processing unit 35_5, a climbing / jump processing unit 35_6, a path determination unit 36, and a motion control unit 37.

[Distance data generator 31]
The distance data generation unit 31 indicates a three-dimensional distance from the robot 1 to an object in the image (environment) based on the color image data S12a and the parallax image data S12b input from the stereo image processing device 12 illustrated in FIG. Distance data (stereo data) S31 is generated and output to the plane detection unit 32 and the grid map generation unit 33.
The distance data generation unit 31 generates distance data S31 in an image (environment) as shown in FIG. 6, for example. In the real space S500 shown in FIG. 6, a staircase ST503 including planes PL501 and PL502 on which the robot 1 can climb, and obstacles OB504 and 505 on which the robot 1 cannot climb are shown. Has been.
FIG. 7 is a schematic diagram showing a state in which the robot 1 is photographing the outside world.
Here, when the floor (reference plane) is the xy plane and the height direction is the z axis, the robot 1 having an image input unit (stereo camera) in the head unit 203 as shown in FIG. The visual field range is a predetermined range in front of the robot 1. The robot 1 recognizes the environment in the visual field range and generates the distance data S31.

[Plane detection unit 32]
The plane detection unit 32 detects a plurality of planes existing in the environment from the distance data S31 input from the distance data generation unit 31, and outputs plane data S32 indicating them to the grid map generation unit 33.
As a plane detection method here, a known plane detection technique using a line segment expansion method, a Hough transform, or the like can be applied. As a representative example of detecting a plurality of planes like staircases from distance data including noise, it is possible to accurately detect planes by performing plane detection by a line segment expansion method.

The line segment expansion method can detect not only a dominant plane in the field of view but also a plurality of planes such as stairs, and can extract them when detecting a plane. In line segment extraction, a plane detection result that is robust against observation noise can be obtained by fitting a line segment adaptively according to the distribution of the points in the distance data.

As shown in FIG. 8, the plane detection unit 32 selects a distance data point group estimated to be in the same plane from the distance data constituting the image, and extracts a line segment for each distance data point group. An extraction unit 51 and a region expansion unit 52 that detects one or a plurality of planar regions present in the image from a line segment group that is included in the image and is composed of all line segments extracted by the line segment extraction unit 51.
The area expanding unit 52 selects arbitrary three line segments estimated to exist on the same plane from the line segment group, and obtains a reference plane therefrom. Then, it is determined whether or not the line segments adjacent to the selected three line segments belong to the same plane as the reference plane. If it is determined that they belong to the same plane, the line segment as the area expansion line segment is determined. To update the reference plane and expand the area of the reference plane.

  The line segment extraction unit 51 extracts a distance data point group 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 the distance from the distance data point group One or more line segments are generated according to the distribution of the data point group. That is, when 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 S51 is output to the area expansion unit 52.

  In this line segment S51, the area expanding unit 52 selects three line segments estimated to belong to the same plane, and obtains a plane serving as a reference plane from these. A range image is expanded to a plurality of planes by expanding a region of this plane (seeded region) by sequentially integrating line segments belonging to the same plane as the region type. Dividing and outputting plane data S32 of the plane group.

  The plane detection unit 32 not only detects a dominant plane included in the acquired image as in the Hough transform, but also enables detection of a plane even when a plurality of planes such as stairs are included. The plane is detected by the line segment expansion method. At this time, by generating a line segment according to the distribution of the distance data points, a detection result that is robust against measurement noise can be obtained.

FIG. 9 is a diagram for explaining a plane detection method based on a line segment expansion method.
In plane detection by the line segment expansion method, as shown in FIG. 9, first, processing is performed on a data string in the row direction or the column direction in the image 20 taken from the focal point F. For example, in a row of pixels in an image (image row), it is composed of distance data points that are 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. And in the obtained line segment group which consists of several line segments, it is a method of estimating and detecting a plane based on the line segment group which comprises the same plane.

FIG. 10 is a flowchart showing plane detection processing by the line segment expansion method.
As shown in FIG. 10, first, the plane detection unit 32 inputs distance data S31 (step ST1), and is estimated to belong to the same plane in each pixel column in the row direction (or column direction) of the distance image. A line segment is obtained from the point (step ST2).
Then, the plane detection unit 32 extracts line segments estimated to belong to the same plane from these line segment groups, and obtains a plane composed of these line segments (step ST3). In step S3, first, the plane detection unit 32 selects a region to be a plane seed (hereinafter referred to as a seed region), and selects a corresponding region type. This selection is made on condition that three line segments including one line in the row direction adjacent to the upper and lower sides (or the column direction adjacent to the left and right) are on the same plane. Here, the plane detection unit 32 uses the plane to which the selected region type consisting of the three line segments belong as a reference plane, and obtains a plane obtained by averaging from the three line segments. Further, an area composed of three line segments is set as a reference plane area.

  Then, the plane detection unit 32 compares the spatial distance to determine whether or not the straight line composed of the pixel columns in the row direction (or column direction) adjacent to the selected region type and the reference plane are the same plane. If it is the same plane, add the adjacent line segment to the reference plane area (area expansion process), update the reference plane as including the added line segment (plane update process), This is repeated until there is no line segment on the same plane in the data string adjacent to the plane area. 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, what constitutes the same plane is connected from the obtained plurality of region groups. In the present embodiment, 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 line segment group belonging to the obtained plane is further provided as step ST4. Although it is a plane, details will be described later.

  Here, the process of detecting a line segment from the three-dimensional distance data and combining the areas into the same plane as one plane is a plane detection process by the conventional line segment expansion method. Is different from the conventional line segment extraction method in step ST2. That is, 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, over-segmentation Or problems such as under-segmentation occur. Therefore, in the present embodiment, a method of adaptively changing the threshold value according to the accuracy of distance data and noise is analyzed by analyzing the distribution of distance data in this line segment extraction.

  The plane detected by the plane detection process described above will be described with reference to FIGS. The case where the plane detected by the plane detection unit 32 is, for example, the staircase STA shown in FIGS. 11 and 12 will be described. 11 (a) and 12 (a) are views of the stairs as seen from the front, and FIGS. 11 (b) and 12 (b) are views of the stairs as seen from the side. FIG. 11 (c) and FIG. (C) is the figure which looked at the stairs from diagonally.

  In this specification, a surface (surface on which a foot or a movable leg is placed) used by a human, a robot, etc. to move up and down stairs is called a tread surface, and the height (one step) from one tread surface to the next tread surface. The height of the stairs) is to be kicked up. The stairs are counted as the first and second steps from the side closer to the ground.

  The staircase STA1 shown in FIG. 11 is a staircase having three steps, the kicking height is 4 cm, the step size of the first and second steps is 30 cm wide, the depth is 10 cm, and only the third step tread is the width of the uppermost step. It is 30 cm and the depth is 21 cm. Further, the staircase STA2 shown in FIG. 12 is also a three-step staircase, the kicking height is 3 cm, the size of the tread surface of the first and second steps is 33 cm wide, the depth is 12 cm, and only the third step tread, which is the uppermost step, It is 33 cm wide and 32 cm deep.

[Grid Map Generation Unit 33]
The grid map generation unit 33 generates the three-dimensional grid map data GM and the floor height data FH based on the distance data S31 input from the distance data generation unit 31 and the plane detection data S32 input from the plane detection unit 32. These are output to the environment class classification unit 34.

First, generation processing of the three-dimensional grid map data GM by the grid map generation unit 33 will be described.
The three-dimensional grid map data GM is obtained by dividing the surrounding environment into a grid (Grid) having a predetermined size in the horizontal direction and the vertical direction.
Based on the distance data S31 from the distance data generation unit 31 and the plane data S32 from the plane detection unit 32, the grid map generation unit 33 generates, for example, a space of 4 meters square and 1 meter in height with a horizontal resolution of 4 A rectangular parallelepiped space having a centimeter and a vertical resolution of 1 centimeter is defined as one grid unit (denoted as one cell), and three-dimensional grid map data GM represented by the set is generated.
The grid map generation unit 33 gives a probability of whether or not each cell is occupied by an object, and changes the occupation probability of each cell based on the observation data. In addition, the grid map generation unit 33 sets a threshold for the occupation probability, and expresses the state of the environment as a map depending on whether the occupation probability is larger than the threshold.
In this specific example, since the resolution in the height direction does not depend on the cell resolution, the vertical resolution can be coarser than 1 cm.

The grid map generation unit 33 performs the following processing on all unit grids (hereinafter referred to as cells) p (x, y, z) in the real space S500 represented by a three-dimensional grid.
The grid map generation unit 33 is based on an algorithm that there is no obstacle on a straight line connecting the measurement point (three-dimensional grid) projected as an image in the captured image and the observation center (stereo camera center). Grid map data GM is created and / or updated.
FIG. 13 schematically shows how the three-dimensional grid map data GM is updated according to the observation result as a side view from the y-axis direction. Cells that are not in the field of view of the robot 1 (hatched cells) represent cells that have not yet been measured. The cells measured by the robot 1 are shown in black, and the cells between the cells corresponding to the measurement points are shown in white. The hatched area indicates an unconfirmed area.

Based on the algorithm described above, the grid map generation unit 33 performs an empty process on the cells existing between the line segments connecting the cell p that is the measurement target point and the stereo cameras 11L and 11R of the robot. Do. Subsequently, an occupied process is performed on the cell corresponding to the measurement point p.
The three-dimensional grid map data GM holds an obstacle existence probability (probability occupied by an obstacle) p (c) with respect to the cell C, and the empty process and the occupy process are performed on each cell. On the other hand, it is a statistical process.
The empty process is a process for reducing the existence probability of an obstacle, and the occupy process is a process for increasing the existence probability of an obstacle. In this specific example, Bayesian update rules are used as an example of a method for calculating the existence probability of the empty process and the occupy process.

The grid map generation unit 33 decreases the occupation probability by the equation (1) in the empty process and increases the occupation probability by the equation (2) in the occupide process.
Equation (1) represents the probability of cell C under the condition that p (c) indicating the occupation probability of cell C is “occupied”, and cell C is occupied in equation (1) or equation (2). The probabilities p (occ |...) And p (empty |...) That indicate whether or not they are occupied are predetermined threshold values th.

[Equation 1]
p (c) ← p (c | empty) = p (empty | c) p (c) / {p (empty | c) p (c) + p (empty | c ~) p (c ~)}
... (1)

[Equation 2]
p (c) ← p (c | occ) = p (occ | c) p (c) / {p (occ | c) p (c) + p (occ | c ~) p (c ~)}
... (2)

  The contents indicated by the three-dimensional grid map data GM given the obstacle existence probability are shown in FIG. In this specific example, the area represented as a three-dimensional grid is represented by a three-dimensional grid, for example, 4 meters square, with a horizontal resolution of 4 cm and a vertical resolution of 1 cm as one cell.

  In FIG. 15, for convenience of explanation, the same area as the real space S500 shown in FIG. 6 is represented as a three-dimensional grid. However, the actual robot 1 has a surrounding area at predetermined intervals, for example, 30 times per second. In order to acquire the state, when the robot 1 is moving, the space S500 expressed as a three-dimensional grid changes every moment. The acquired cell is 1 when it is visible and 0.5 when it is not visible. The occupation probability is gradually updated while measuring 30 times per second.

  The three-dimensional grid map data GM generated by the grid map generation unit 33 is, for example, the occupancy probability OG (x, y, z) (= p (c)) for each cell of three-dimensional coordinates (x, y, z). ).

Hereinafter, generation processing of floor height data FH by the grid map generation unit 33 will be described.
The grid map generation unit 33 uses the cell occupancy probability data (obstacle presence probability data) obtained from the three-dimensional grid map data GM for the cells of the two-dimensional height map (2D Height Map). Data FH is verified.

In this specific example, the grid map generation unit 33 generates the floor height data FH using the three-dimensional grid map data GM.
The three-dimensional grid map data GM is created and / or updated based on an algorithm that there is no obstacle on a straight line that geometrically connects the measurement point and the observation center (stereo camera center).
The grid map generation unit 33 performs processing to update the occupation probability when the height information of the cell of the floor height data FH is associated with the cell of the three-dimensional grid with reference to the occupation probability of the cell of the three-dimensional grid. .

FIG. 16 shows the floor height data FH.
The floor height data FH holds height information h information and an invalid flag for each cell of a two-dimensional grid represented by a rectangular grid.
In FIG. 16, the height h = 30 is given to the cell corresponding to the plane PL501 in FIG. 6, and the height h = 60 is given to the cell corresponding to the plane PL502. If this cell has height information that the robot 1 cannot climb, an invalid flag (inv) is given.

  Similarly to the three-dimensional grid map data GM, the floor height data FH is expressed by, for example, a two-dimensional grid having one cell of 4 meters square and a horizontal resolution of 4 cm. In the floor height data FH, the vertical resolution is not considered, and height information is assigned to each cell. The invalid flag indicates that the robot 1 is an obstacle, but instead of preparing the invalid flag, an unrealistic value is given to express the space, for example, −10000. May be invalid, that is, an obstacle to the robot 1.

FIG. 17 is a flowchart for describing generation processing of floor height data FH by the grid map generation unit 33.
The grid map generation unit 33 acquires the height information in the floor height data FH as the reference surface height map, and associates the height information with the height coordinates of the three-dimensional grid map data GM to correspond to the corresponding 3 Compare whether or not the occupation probability of a cell in the three-dimensional grid is larger than a threshold value that determines whether or not the object is recognized as an obstacle in the three-dimensional grid. If it is larger, this cell is determined to be an obstacle, and the height information of the corresponding height map for the reference plane is updated.

The grid map generation unit 33 refers to the floor height data FH for all cells (x, y) and acquires the height of the grid (step ST11).
If the invalid flag is set in the height information h, the process is terminated (step ST12; yes).

  On the other hand, when the height information h is valid, that is, when the height information h is applied, the height information h is made to correspond to the three-dimensional grid to obtain an index k in the three-dimensional grid (step ST13). Subsequently, whether or not the obstacle existence probability p (Cxyz) of the cell (x, y, z) in the three-dimensional grid is larger than a threshold th that determines whether or not the obstacle is recognized in the three-dimensional grid. Are compared (step ST14).

If it is smaller than the threshold (step ST14; no), it is determined that this cell is not an obstacle.
At this time, the grid map generation unit 33 updates the cell (x, y) based on an algorithm for creating the three-dimensional grid map data GM. If it is larger than the threshold (step ST14; yes), it is determined that this cell is an obstacle, and the process is terminated. That is, in this case, the height information h of the floor height data FH is not updated.

As shown in FIG. 13, the amount of calculation can be reduced by applying the above process only to the cell region corresponding to the viewing angle region in the grid. In FIG. 13, the cell region corresponding to this viewing angle region is described as a bounding box.

The grid map generation unit 33 updates the floor height data FH from the plane data S32 by the plane detection unit 32 and the three-dimensional grid map data GM.
For all measurement points (cells) p included in the measurement space in the visual field region of the robot 1, the plane to which the measurement point p belongs is a horizontal plane, and the three-dimensional grid map data GM corresponding to the measurement point p When the obstacle existence probability of the cell is higher than the threshold th, the height information corresponding to the measurement point p is updated.

The update processing of the floor height data FH by the grid map generation unit 33 is shown in FIG.
First, the grid map generation unit 33 acquires plane information to which the measurement point p belongs for all measurement points (cells) p in the visual field region (step ST21).
As the plane information, plane data S32 that is a plane detection result by the plane detector 32 is used. Specifically, a plane ID (p_id) representing which plane the measurement point p belongs to is acquired, and when this p_id is valid (step ST22; yes), this pid is used as a key. The plane information to which the measurement point p belongs is searched from the detected plane group (step ST23).

Next, the grid map generation unit 33 verifies whether or not the plane to which the measurement point p belongs is a horizontal plane, and ends the process if it is not a horizontal plane (step ST24; no). On the other hand, when it is a horizontal plane (step ST24; yes), the obstacle existence probability of the cell of the three-dimensional grid corresponding to the measurement point p is referred (step ST25).
Specifically, the grid map generation unit 33 obtains an index (i, j, k) as position information of the three-dimensional grid from position information (x, y, z) of the measurement point p in the floor height data FH. Based on this index, a cell of the three-dimensional grid is referred to.
In the processing for obtaining the index (i, j, k), in this embodiment, in order to reduce the influence of noise at each measurement point, the height information h is not the actual measurement height of the measurement point p but the measurement point p. The height of the plane to which the file belongs (averaged height; plane.m_z) is used. Of course, the height of the measurement point p may be used as it is.

  Subsequently, the grid map generation unit 33 determines whether or not to update the height information h corresponding to the measurement point p (step ST26). Here, when the occupation probability of the three-dimensional grid corresponding to the measurement point p is larger than the threshold th indicating that an obstacle exists (step ST26; yes), the corresponding cell (i, j) of the floor height data FH. ) Is updated with the height information h (step ST27).

  There are several methods for updating the cell (i, j) in the floor height data FH. For example, when overwriting is updated, the measured maximum value is held, and the like. In the case of overwriting update, the height of the cell (i, j) is always overwritten by the acquired h. When the maximum value is held, only when the height of the cell (i, j) is lower than the height information h so far, or when the invalid flag is set in the cell (i, j). Update with new value h. In addition, the average of the height information h so far and the new value may be taken.

  FIG. 19 shows an environment map created by this series of processing. As described above, the floor height data FH is complemented by the three-dimensional grid map data GM. In the environment map, whether or not the height information h or an obstacle (obs) is applied to the cells of the two-dimensional grid.

  The floor height data FH generated by the grid map generation unit 33 indicates, for example, the floor height Floor (x, y) for each of the two-dimensional coordinates (x, y).

[Environmental Class Classification Unit 34]
The environment class classification unit 34 inputs the three-dimensional grid map data GM and the floor height data FH from the grid map generation unit 33, and based on these, the position corresponding to the two-dimensional cell (x, y), for example, Variable env_type (x, y), which is environmental class classification data classified into any one of an obstacle, bumpy, tunnel, unknown, floor, stairs, and boundary, is generated and output to the path determination unit 35.

The environment class classification unit 34 first acquires the floor height Floor (x, y) of the cell (x, y) of the two-dimensional coordinates (x, y) based on the floor height data FH, and uses this to obtain hf Assign to.
Note that when the floor height Floor (x, y) = − ∞ (inv), there is no floor or no floor.
Next, the environment class classification unit 34 determines the occupation probability OG (x, y, z) of the three-dimensional coordinates (x, y, z) satisfying “z> hf” on the two-dimensional coordinates (x, y), Obtained from the three-dimensional grid map data GM.
Then, the environmental class classification unit 34 identifies the cell (x, y) and the acquired occupancy probability OG (x, y, z) that exceed the threshold Pocc, and determines the highest z among the obstacles. Obstacle (x, y, hf) indicating the height of.
When the occupation probability OG (x, y, z) does not exceed a predetermined threshold value Pocc (for example, 0.8) in all z of “z> hf” of the cell (x, y). , Obstable (x, y, hf) = − ∞ (not a failure).

In addition, the environment class classification unit 34 identifies the acquired occupancy probability OG (x, y, z) of the cell (x, y) that exceeds the threshold value Pocc, and sets the lowest z as the ceiling. Let Ceiling (x, y, hf) indicate the height of.
When the occupation probability OG (x, y, z) does not exceed a predetermined threshold value Pocc (for example, 0.8) in all z of “z> hf” of the cell (x, y). , Ceiling (x, y, hf) = − ∞ (not the ceiling).

  The environment class classification unit 34 also includes the floor height Floor (x, y) of the cell (x, y) indicated by the floor height data FH and the two-dimensional coordinates (x ′, y ′) adjacent to the cell. The maximum absolute value of the difference from the floor height Floor (x ′, y ′) of the adjacent cell (x ′, y ′) is calculated as Δh (x, y).

FIG. 20 is a flowchart for explaining the environment class classification process of each cell (x, y) by the environment class classification unit 34.
First, the environment class classification unit 34, as described above, based on the three-dimensional grid map data GM and the floor height data FH input from the grid map generation unit 33, the floor height of each cell (x, y). Floor (x, y), Obstable (x, y, hf), and Ceiling (x, y, hf) are acquired or generated, respectively, and substituted for variables f, o, and c (step ST41).

Next, the environment class classification unit 34 determines whether or not the condition of “variable o> f” is satisfied. If it is determined that the condition is satisfied, the process proceeds to step ST43. If not, the process proceeds to step ST51 (step ST42).
Here, when the condition “variable o> f” is not satisfied (when the process proceeds to step ST51), for example, “Obscale (x, y, hf) = − ∞ (not a failure)” is set. .

Next, the environment class classification unit 34 determines whether or not the condition “the variable f is not inv (invalid)” and “c> f + d_tunnel” is satisfied (step ST43). , Y) is determined to be a tunnel (step ST44), and if not, the process proceeds to step ST45.
Here, “d_tunnel” indicates the minimum height at which the robot 1 can travel while scooping under the ceiling. That is, it shows the minimum height in the tunnel.

Then, the environment class classification unit 34 determines whether or not the condition that “the variable o does not exceed a predetermined threshold” is satisfied, and if it is determined that the condition is satisfied, the cell (x, y) is rough. It is determined that the surface is a clutter (step ST47).
Here, the condition that the variable o does not exceed a predetermined threshold is satisfied, for example, when the height of the obstacle indicated by the variable o is small enough to get over the robot 1.
On the other hand, if the environment class classification unit 34 determines that the condition “the variable o does not exceed the predetermined threshold” is not satisfied, the cell (x, y) is determined to be an obstacle (obstacle) ( Step ST46).
The rough surface means a surface having only an obstacle that the robot 1 can easily get over.
The obstacle means an obstacle that is large enough that the robot 1 cannot travel.

  In step ST51, the environment class classification unit 34 determines whether or not the variable f is invalid (inv) (step ST51). If it is determined that the variable f is invalid, the cell (x, y) is unknown (unknown). (Step ST52), and if it is not invalid, the process proceeds to step ST53.

In step ST53, as described above, the environmental class classification unit 34 calculates the floor height Floor (x, y) of the cell (x, y) and the two-dimensional coordinates (x ′, y ′) adjacent to the cell. The maximum absolute value of the difference from the floor height Floor (x ′, y ′) of the adjacent cell (x ′, y ′) is calculated as Δh (x, y). Then, the environment class classification unit 34 substitutes Δh (x, y) calculated above for the variable d (step ST52).
Next, the environment class classification unit 34 determines whether or not the condition “d <d_step” is satisfied, and if it is determined that the condition is satisfied, the cell (x, y) is determined to be a floor (step ST55). If not, the process proceeds to step ST56.
Here, “d_step” indicates the maximum height of the step where the robot 1 can normally walk, and is set in advance.
The floor is a plane on which the robot 1 can easily walk normally.

In step ST54, the environmental class classification unit 34 determines whether or not the condition “d <d_stays” is satisfied. If the environmental class classification unit 34 determines that the condition “d <d_stays” is satisfied, it determines that the cell (x, y) is a stairs (step ST57). Otherwise, it is determined that the cell (x, y) is a border (step ST58).
Here, “d_stays” indicates the maximum value of the height of the step where the robot 1 can walk up the stairs, and is set in advance.

By the above-described processing by the environment class classification unit 34, the cell state defined by the three-dimensional coordinates (x, y, z) is classified (determined) as shown in FIG. 21, for example.
As described above, the environment class classification unit 34 stores the result of the state determined for all cells (x, y) in the variable env_type (x, y), and outputs this to the path determination unit 36.
The environment class classification unit 34 writes the variable env_type (x, y) to a predetermined memory (for example, the RAR 1312 or the external storage device 314 shown in FIG. 4) and reads it before outputting it to the path determination unit 36. To the path determination unit 36.

  In the example described above, the state of the cell (x, y) is exemplified as floor, obstable, unknown, stays, border, tunnel, and clutter. However, in addition to the above, the environment class classification unit 34 includes an obstacle. Narrow, which means an approaching narrow area, ramp, which means an inclined surface, human, which means human, etc., ladder, which means stairs, door, which means ordinary door, elevator, which means elevator, escalator, which means escalator, etc. Other states may be determined.

[Operation function section]
For example, the CPU 15 sets, as a default, a normal travel processing unit 35_1, a staircase travel processing unit 35_2, a caution travel processing unit 35_3, a saddle motion processing unit 35_4, a step over processing unit 35_5, a climbing / jump processing unit 35_6, and a program, for example. Run to achieve.
For example, attribute data (name, info, types, cost, sensors, excuter) is assigned to each of the processing units 35_1 to 35_6.
“name” indicates the name of the processing unit (module).
info indicates a description of the function of the processing unit, for example, a program code.
“types” indicates a state of a cell that can be handled by the processing unit.
“cost” indicates the cost assigned to the processing of the processing unit when determining the optimum path (route) for the robot 1 to move. The cost has a value proportional to the time required to pass the position when the robot 1 performs an operation under the control of the processing unit.
Sensors indicates a sensor necessary as an input for processing of the processing unit.
Execute indicates a system (hardware) that executes the processing of the processing unit.

For example, the normal travel processing unit 35_1 performs a process of instructing the motion control unit 37 to perform a series of operations in which the robot 1 walks two legs on a flat floor.
The staircase traveling processing unit 35_2 performs, for example, processing for instructing the motion control unit 37 to perform a series of operations in which the robot 1 walks two legs on the stairs.
The attention travel processing unit 35_3 is a process for instructing the motion control unit 37, for example, a series of slowly walking motions while the robot 1 confirms the surface state based on the detection signals from the sensor group 17. I do.
For example, when there is a high obstacle, the heel motion processing unit 35_4 controls a series of heel motions in which the robot 1 bends at the waist, walks around the head, walks with the knees, and crawls with four legs. .
The step over processing unit 35_5 instructs the motion control unit 37 to perform a series of operations in which the robot 1 moves across a small obstacle.
The climbing / jump processing unit 35_6 instructs the motion control unit 37 to perform a series of operations in which the robot 1 climbs an obstacle using a hand or jumps a boundary portion.

  As the processing unit realized by the CPU 15, in addition to the processing unit described above, a processing unit that prescribes a slowly walking operation (sideways), a processing unit that prescribes an operation (dialog) for talking with a human, and moving an obstacle Stipulates a processing unit that defines the operation to be performed (manupulate), a processing unit that defines the operation to open the door (open door), a processing unit that defines the operation to move the inclined surface (slide), and an operation that uses the elevator (use elevator) And a processing unit that defines an operation using a escalator (use escalator).

The CPU 15 includes any of the processing units described above as defaults, and these can be deleted by an instruction from the user.
Further, the CPU 15 can add a new processing unit in addition to the processing unit provided as a default.

[Path determination unit 36]
The path determination unit 36 receives the variable env_type (x, y) that is input from the environment class classification unit 34 and indicates the result of the determination for all the cells (x, y), and the list of available operation function units described above. Based on the above, the optimal path for the robot 1 to move from the current location to the destination is determined.
The path determination unit 36 constantly updates the list of available operation function units.
In addition, the path determination unit 36 sequentially selects appropriate processing units based on the determined path and the movement status of the robot 1 to control the operation of the robot 1.

First, the optimum path determination process by the path determination unit 36 will be described.
FIG. 22 is a flowchart for explaining optimum path determination processing by the path determination unit 36 shown in FIG.
The path determination unit 36 refers to the available function list and identifies an available processing unit (step ST61).
In this embodiment, as an example, as illustrated in FIG. 5, usable processing units are a normal traveling processing unit 35_1, a staircase traveling processing unit 35_2, a caution traveling processing unit 35_3, a saddle movement processing unit 35_4, and a step over processing unit. The case of 35_5 and the climbing / jump processing unit 35_6 will be described.

  Next, the path determination unit 36 specifies a plurality of paths (routes) reaching the target position from the current position of the robot 1 based on the variable env_type (x, y) input from the environment class classification unit 34 (step ST62). ).

Next, the path determination unit 36 moves the state indicated by the coordinates (x, y) on the path, the variable env_type (x, y), and the state of each of the plurality of paths specified in step ST62. The cost is calculated based on the cost cost assigned to the function unit of the robot 1 (step ST63).
Next, the path determination unit 36 determines the path with the lowest cost calculated in step ST63 among the plurality of paths identified in step ST62 (step ST64).
Specifically, the path determination unit 36 calculates, for each of the plurality of paths, the sum of the cost costs corresponding to the coordinates (x, y) on the path, and determines the path with the minimum sum. .

Hereinafter, the update process of the available function list by the path determination unit 36 will be described.
FIG. 23 is a flowchart for explaining an available function list update process by the path determination unit 36 shown in FIG.
The path determination unit 36 determines whether or not a new processing unit (function) that defines the operation of the robot 1 has been added (step ST61). If it is determined that it has been added, the process proceeds to step ST62. Proceed to ST63.
The path determination unit 36 adds an entry for the newly added function (processing unit) to the available function list (step ST62).
Here, the addition of the above functions to the CPU 15 of the robot 1 is realized by the addition of a program module or the addition of dedicated hardware.

The path determination unit 36 determines whether the function that is an entry in the available function list has been deleted or has failed (step ST63). If it is determined to be, the process proceeds to step ST64. If it judges, it will return to step ST61.
The path determination unit 36 deletes the entry of the deleted or failed function from the available function list (step ST64).

Hereinafter, processing unit selection processing by the path determination unit 36 will be described.
24 and 25 are flowcharts for explaining processing unit selection processing by the path determination unit 36 shown in FIG.
The path determination unit 36 determines a cell (to which the robot 1 moves next) based on at least one of the control progress of the motion control unit 37, the imaging results of the CCD cameras 10R and 10L, and the detection signal from the sensor group 27. x, y) is specified (step ST71).
Next, the path determination unit 36 substitutes the variable env_type (x, y) of the cell (x, y) specified in step ST71 for the variable t (step ST72).

Next, the path determination unit 36 determines whether or not the variable t indicates floor (step ST73). If the variable t indicates floor, the path determination unit 36 outputs an operation instruction to the normal travel processing unit 35_1 (step ST74). If not, the process proceeds to step ST75.
In step ST74, the motion control unit 37 controls the actuator group 16 to cause the robot 1 to perform a normal traveling operation based on an instruction from the normal traveling processing unit 35_1.

Next, the path determination unit 36 determines whether or not the variable t indicates stairs (step ST75), and when it indicates stairs, outputs an operation instruction to the staircase travel processing unit 35_2 (step ST76). If not, the process proceeds to step ST77.
In step ST76, the motion control unit 37 controls the actuator group 16 to cause the robot 1 to perform the stair travel operation based on the instruction from the stair travel processing unit 35_2.

Next, the path determination unit 36 determines whether or not the variable t indicates “unknown” (step ST77). If the variable t indicates “unknown”, the path determination unit 36 outputs an operation instruction to the attention traveling processing unit 35_3 (step ST78). If not, the process proceeds to step ST79.
In step ST78, the motion control unit 37 controls the actuator group 16 to cause the robot 1 to perform a caution driving operation based on an instruction from the caution driving processing unit 35_3.

Next, the path determination unit 36 determines whether or not the variable t indicates tunnel (step ST79). If the variable t indicates tunnel, the path determination unit 36 outputs an operation instruction to the heel motion processing unit 35_4 (step ST80). If not, the process proceeds to step ST81.
In step ST80, the motion control unit 37 controls the actuator group 16 to cause the robot 1 to perform the heel motion based on an instruction from the heel motion processing portion 35_4.

Next, the path determination unit 36 determines whether or not the variable t indicates “clutter” (step ST81), and outputs an operation instruction to the step over processing unit 35_5 when it indicates “clutter” (step ST82). If not, the process proceeds to step ST83.
In step ST82, the motion control unit 37 controls the actuator group 16 to cause the robot 1 to perform a step over operation based on an instruction from the step over processing unit 35_5.

Next, the path determination unit 36 determines whether or not the variable t indicates “border” (step ST83), and outputs an operation instruction to the climb / jump processing unit 35_6 when “border” is indicated (step ST84). Otherwise, the process proceeds to step ST85.
In step ST84, the motion control unit 37 controls the actuator group 16 so that the robot 1 performs a climbing operation or a jumping operation based on an instruction from the climbing / jump processing unit 35_6.

  Next, whether the path determination unit 36 has reached the destination based on the control progress of the motion control unit 37, the imaging results of the CCD cameras 10R and 10L, and the detection signal from the sensor group 27. It is determined whether or not (step ST85), and if it is determined that the destination has been reached, the process ends. If not, the process returns to step ST71.

For example, as shown in FIG. 26, the path determination unit 36 has the areas AR1, AR3, and AR5 floor and controls the normal travel processing unit 35_1, and the area AR2 is stails and controls the staircase travel processing unit 35_2. The area AR4 is a boundary and the climbing / jump processing unit 35_6 performs control, and the area AR6 is unknown and the attention traveling processing unit 35_3 performs control.
In addition, for example, as illustrated in FIG. 27, the path determination unit 36 determines an optimal path from the current position (Robot) of the robot 1 to the destination (Goal).

[Motion control unit 37]
The movement control unit 37 is based on instructions from the normal traveling processing unit 35_1, the staircase traveling processing unit 35_2, the caution traveling processing unit 35_3, the saddle movement processing unit 35_4, the step over processing unit 35_5, the climbing / jump processing unit 35_6, and the like. The actuator group 16 is driven to cause the robot 1 to execute a desired operation.

[Whole movement of robot 1]
Hereinafter, an example of the overall operation of the robot 1 shown in FIG. 1 will be described.
The CCD cameras 10R and 10L output the right image data S10R and the left image data S10L corresponding to the imaging results to the stereo image processing device 12, respectively.
Then, the stereo image processing device 12 calculates disparity information (distance information) between the right image data S10R and the left image data S10L input from the CCD cameras 10R and 10L, and color image data (YUV: luminance Y, UV color difference) S12a and parallax image data (YDR: luminance Y, parallax D, reliability R) S12b are calculated alternately on the left and right for each frame and output to the CPU 15 (distance data generating unit 31 shown in FIG. 5).

Next, the distance data generation unit 31 uses the color image data S12a and the parallax image data S12b input from the stereo image processing device 12 to provide three-dimensional distance data (stereo data) for the object in the image (environment). S31 is generated and output to the plane detection unit 32 and the grid map generation unit 33.
Next, the plane detection unit 32 detects a plurality of planes existing in the environment from the distance data S31 input from the distance data generation unit 31, and outputs the plane data S32 indicating them to the grid map generation unit 33.
Next, based on the distance data S31 input from the distance data generation unit 31 and the plane detection data S32 input from the plane detection unit 32, the grid map generation unit 33 performs 3D grid map data GM and floor height data. FHs are generated and output to the environment class classification unit 34.

Next, the environment class classification unit 34 inputs the three-dimensional grid map data GM and the floor height data FH from the grid map generation unit 33, and based on these, the position corresponding to the two-dimensional cell (x, y) is determined. For example, a variable env_type (x, y), which is environment class classification data classified into any one of an obstacle, bumpy, tunnel, unknown, floor, stairs, and boundary, is generated and output to the path determination unit 35. .
Next, the variable env_type (x, y) indicating the result of the determination for all the cells (x, y) input from the environment class classification unit 34 by the path determination unit 36 and the above-described usable operation function units And the robot 1 determines an optimal path for the robot 1 to move from the current location to the destination.
The path determination unit 36 constantly updates the list of available operation function units.
Further, the path determination unit 36 sequentially selects appropriate processing units 35_1 to 35_6 based on the determined path and the movement status of the robot 1.
Then, the selected processing units 35_1 to 35_6 issue instructions to the motion control unit 37.
Next, the motion control unit 37 drives the actuator group 16 based on the instruction from the selected processing units 35_1 to 35_6, and causes the robot 1 to execute a desired operation.

As described above, in the robot 1, as shown in FIG. 5 and the like, the processing units 35_1 to 35_6 and the like that can be used as functions by the CPU 15 are identified (step ST61 shown in FIG. 22). In addition, a plurality of paths to the target position are dynamically specified (step ST62).
Thereby, for example, when a machine part such as the processing units 35_1 to 35_6 or the actuator group 16 controlled thereby fails, a path reaching the target position without using control by the processing unit corresponding to the failed function is obtained. You can choose.
Further, when a new processing unit is added and made available, a path that can be traveled by the robot 1 can be put in the selection frame by using the function of the added processing unit.
In addition, the robot 1 can determine the optimum path by dynamically changing the available processing unit according to the environment in which the robot 1 is located.
Further, in the robot 1, for each of the plurality of paths, a sum of costs is calculated based on a variable env_type (x, y) of a two-dimensional coordinate on the path and a cost (time cost) associated with the variable env_type (x, y). Since the path to be minimized is selected, a path that can be reached in the shortest time can be selected with high reliability.

In the robot 1, the environment class classification unit 34 uses the variable env_type (x, y) that classifies the state of each of the two-dimensional coordinates (x, y) based on the three-dimensional grid map data GM and the floor height data FH. y) is generated.
Then, the path determination unit 36 identifies the state corresponding to the two-dimensional position (x, y) of the robot 1 during the movement process of the robot 1, and the processing unit 35_1 associated with the state in advance. ˜35_6 is selected to drive the operation of the robot 1.
As described above, the robot 1 generates a variable env_type (x, y) in which the state of the two-dimensional coordinates (x, y) is classified in advance, and stores this in the RAM 312 shown in FIG.
As a result, during the operation of the robot 1, the path determination unit 36 can acquire the state of the position at which the robot 1 will move next by simply reading the variable env_type (x, y) from the environment class classification unit 34. In addition, the next operation of the robot 1 can be determined in a short time, and the processing load can be reduced as compared with the prior art.
In the robot 1, it is sufficient to store the variable env_type (x, y) in which the state of the dimensional coordinates (x, y) is classified in a memory such as the RAM 312. After the variable env_type (x, y) is generated, Further, it is not necessary to hold the grid map data GM and the floor height data FH, and the memory can be used efficiently.

The present invention is not limited to the embodiment described above.
That is, those skilled in the art may make various modifications, combinations, subcombinations, and alternatives regarding the components of the above-described embodiments within the technical scope of the present invention or an equivalent scope thereof.
For example, in the above-described embodiment, the bipedal walking robot 1 is illustrated as the moving body of the present invention. However, for example, a moving body using wheels or a robot having one or three legs or more may be used as the moving body. Good.

FIG. 1 is a functional block diagram of a robot according to an embodiment of the present invention. FIG. 2 is an external view of the robot according to the embodiment of the present invention. FIG. 3 is a diagram schematically illustrating a joint degree-of-freedom configuration included in the robot according to the embodiment of the present invention. FIG. 4 is a schematic diagram showing a configuration of a robot control system according to the embodiment of the present invention. FIG. 5 is a functional block diagram of the CPU shown in FIG. FIG. 6 is a schematic diagram for explaining a real space recognized by the robot according to the embodiment of the present invention. FIG. 7 is a schematic diagram showing a state in which the robot according to the embodiment of the present invention is photographing the outside world. FIG. 8 is a functional block diagram of the plane detection unit shown in FIG. FIG. 9 is a diagram for explaining a plane detection method by a line segment expansion method performed by the plane detection unit shown in FIG. FIG. 10 is a flowchart showing plane detection processing by the line segment expansion method. 11A and 11B are schematic diagrams showing the stairs, where FIG. 11A is a diagram of the stairs viewed from the front, FIG. 11B is a diagram of the stairs viewed from the side, and FIG. 11C is a diagram of the stairs viewed obliquely. 12A and 12B are schematic views showing other examples of stairs, where FIG. 12A is a view of the stairs viewed from the front, FIG. 12B is a view of the stairs viewed from the side, and FIG. is there. FIG. 13 is a side view of a schematic view of the state in which the three-dimensional grid is updated according to the observation result as seen from the y-axis direction. FIG. 14 is a diagram for explaining a robot having means for applying a texture. FIG. 15 is a schematic diagram illustrating a three-dimensional grid that represents the same area as the real space illustrated in FIG. 6. FIG. 16 is a schematic diagram showing a height map of the space shown in FIG. FIG. 17 is a flowchart for explaining the generation processing of the floor height data FH by the grid map generation unit shown in FIG. FIG. 18 is a diagram for explaining the process of updating the floor height data FH by the grid map generation unit shown in FIG. FIG. 19 is a diagram for explaining data generated by the environment class classification unit illustrated in FIG. 5. FIG. 20 is a flowchart for explaining environment class classification processing of each cell (x, y) by the environment class classification unit shown in FIG. FIG. 21 is a diagram for explaining the processing of the environment class classification unit shown in FIG. FIG. 22 is a flowchart for explaining optimum path determination processing by the path determination unit shown in FIG. FIG. 23 is a flowchart for explaining the available function list update processing by the path determination unit shown in FIG. FIG. 24 is a flowchart for explaining processing unit selection processing by the path determination unit shown in FIG. FIG. 25 is a flowchart continued from FIG. 24 for explaining processing unit selection processing by the path determination unit shown in FIG. FIG. 26 is a diagram for explaining an example of processing of the path determination unit illustrated in FIG. FIG. 27 is a diagram for explaining an example of processing of the path determination unit illustrated in FIG.

Explanation of symbols

10L, 10R ... CCD camera, 12 ... stereo image processing device, 15 ... CPU, 16 ... actuator group, 17 ... sensor group, 31 ... distance data generation unit, 32 ... plane detection unit, 33 ... grid map generation unit, 34 ... Environmental class classification unit, 35_1 ... normal travel processing unit, 35_2 ... stair travel processing unit, 35_3 ... caution travel processing unit, 35_4 ... saddle motion processing unit, 35_5 ... step over processing unit, 35_6 ... climbing / jump processing unit, 36 ... Path determination unit, 37 ... motion control unit

Claims (11)

A moving body control method for controlling movement of a moving body to a target position,
A first step of detecting a state of a path from the current position of the moving body to the target position;
A second step of identifying a plurality of movement operations that the movable body can execute;
Based on the state of the path detected in the first step, the movement suitable for the state of the position of the moving body among a plurality of types of moving operations that can be executed by the moving body specified in the second step And a third step of controlling the selected moving operation so that the moving body performs the selected moving operation. A fourth step of adding or deleting the moving operation executable by the moving body,
The moving body control method according to claim 1, wherein the second step specifies the executable movement operation after being added or deleted in the fourth step. The third step includes
For each of a plurality of paths from the current position of the mobile body to the target position, the total of travel costs when traveling the mobile body on the path is calculated, and the path that minimizes the total is transferred to the path. The moving body control method according to claim 1, wherein the moving body is selected as a path along which the body moves. The first step determines the state of the position on the path based on the imaging results of the imaging target regions from different imaging positions,
The moving body control method according to claim 1, wherein the third step selects a moving action previously associated with the determined state with respect to the position of the moving body. The first step includes
Generating 3D grid data indicating the occupation state of each 3D grid object defined in the imaging target area;
Based on the generated three-dimensional grid data, for each of the two-dimensional positions of the three-dimensional grid, it is determined which of a plurality of predetermined states,
The moving body control method according to claim 1, wherein the state of the path is detected according to the determined state. The movement according to claim 5, wherein the first step determines whether each of the two-dimensional positions is a floor or a staircase on which the movable body can move and an obstacle on which the movable body cannot move. Body control method. The moving body is a humanoid robot having two legs,
The moving body control method according to claim 1, wherein the plurality of types of movement operations are any one of normal travel, stair travel, caution travel, dredging operation, step over, climbing operation, and jump operation. Imaging means for generating imaging results of imaging target regions from different imaging positions;
Traveling means;
Based on the imaging result generated by the imaging means, the state of the path from the current position of the moving body to the target position is detected, and a plurality of moving operations that can be executed by the moving body are identified and detected. Based on the state of the path, from among a plurality of types of moving operations that can be executed by the moving body specified in the second step, sequentially select a moving operation that matches the position state of the moving body, and perform the selection. And a control means for controlling the traveling means so that the moving body performs the moving operation. The moving body according to claim 8, wherein the control unit adds or deletes the moving operation that can be executed by the moving body. The control means calculates, for each of a plurality of paths from the current position of the moving body to the target position, a sum of traveling costs when traveling on the path along the path, and minimizes the sum. The moving body according to claim 8, wherein a path is selected as a path along which the moving body moves. The traveling means is two legs,
The moving body according to claim 8, which is a humanoid robot.

Images