JP2003271975A - Method of extracting plane, extractor therefor, program therefor, recording medium therefor, and robot system mounted with plane extractor - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To extract a plane robust against a noise and high in reliability. <P>SOLUTION: A three-dimensional data is sampled at first at random from a three-dimensional data group, a plane parameter (θ, ϕ, d) is calculated, where (θ, ϕ) is a direction of a normal vector and d is a distance from the origin, and the plane parameter is balloted to a ballot space (θ, Ψ, d)=(θ, ϕcosθ, d), in order to extract the plane from the three-dimensional data such as a distance image. A vote value is weighted by reliability of the sampled data and a calculation method of the parameter. Then, the plane parameter of which the obtained vote value is estimated to get high is interlace-processed to the plane parameter. The plane parameter is output at last together with reliability of the sampling data, the number of sampling, and reliability obtained from an error or the like in the interlace. <P>COPYRIGHT: (C)2003,JPO

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a plane extraction method for extracting a plane from three-dimensional data such as parallax or range images, an apparatus therefor, a program therefor, a recording medium therefor, and a robot apparatus equipped with a plane extraction apparatus. Plane extraction method and apparatus for improving reliability and calculation speed in plane extraction,
The present invention relates to the program, a recording medium thereof, and a plane extraction device-mounted robot device.

[0002]

2. Description of the Related Art A mechanical device that makes a movement similar to that of a human being (organism) using electric or magnetic action is called a "robot". Robots started to spread in Japan from the end of the 1960s, but most of them are industrial robots such as manipulators and transfer robots (Industrial Robots) for the purpose of automating and unmanning production work in factories.
rialRobot).

Recently, practical robots have been developed to support life as a human partner, that is, to support human activities in various situations such as living environment and other daily life. Unlike an industrial robot, such a practical robot is used in various aspects of the human living environment.
It has the ability to self-learn how to adapt to individuals with different personalities or various environments. For example, it is designed by using a “pet type” robot that imitates the body mechanism and movement of a quadruped animal such as a dog or a cat, or a body mechanism and movement of a human walking two legs upright. Robot devices such as “humanoid” or “humanoid” robots (Humanoid Robots) are already in practical use.

These robot devices are sometimes called entertainment robots because they can perform various operations, etc., with emphasis on entertainment, as compared with industrial robots. Further, some of such robot devices operate autonomously in accordance with information from the outside and internal states.

By the way, in the autonomous robot apparatus,
Needless to say, the ability to recognize the environment around oneself, plan a route, and move accordingly. In order to recognize the environment, information on obstacles around the robot is required. Therefore, the robot device detects the plane that is the floor from the three-dimensional data such as the range image or the parallax image,
You need to recognize obstacles.

As a method of extracting a plane from three-dimensional data, there is a method of using Hough Transform. Forward Position, Backward Pos
There are several methods called ition and Feature Point Pairs. Hereinafter, these Hough transforms will be described as an example of straight line detection in a two-dimensional image plane for simplicity.
It is assumed that edges (feature points expected to lie on a straight line) have been detected from the original image by some image processing. Further, the number of feature points is M, and the size of the original image is L ×
L, the size of the voting destination parameter space (ballot box) is l
(L) x 1 (L).

In Forward Position, the characteristic points of the original image are sequentially traced, and votes are cast for each slot through which the curve when each point is mapped in the parameter space passes. The complexity of the algorithm is O (Ml).

In Backward Position, each slot in the parameter space is sequentially operated, and the number of characteristic points on the straight line obtained by inversely mapping each slot to the original image is used as the vote of that slot. The complexity of the algorithm is O (L
¹² ).

In Feature Point Pairs, every combination of two characteristic points is sequentially traced, and the slod to which the parameter calculated from the pair of two characteristic points belongs is voted. The complexity of the algorithm is O (M ² ).

However, these Hough transforms result in 3
The method of extracting the plane from the dimensional data requires an extremely large amount of calculation and memory. This is because the number of dimensions is one more than that in two dimensions, and all the data points have meaning as feature points. Therefore, neither of these Hough transforms is practical in terms of the amount of calculation and memory used in the task of plane detection from three-dimensional data.

In addition, Randomized Hough transform
There is a Hough transform called ough Transform (RHT).
This randomized Hough transform is based on the above Feature Point Pa
This is a derivation of irs, and in the above-described two-dimensional image plane, not all combinations of two feature points but only K combinations by random sampling are processed. The complexity of this algorithm is O
(K).

Conventionally, there has been proposed a method using Hough transform for detecting a plane from three-dimensional data. For example, Japanese Laid-Open Patent Publication No. 10-96607 discloses an object detection method and a plane estimation method for performing Hough transform processing on three-dimensional data to estimate the position of a plane object in a three-dimensional space (hereinafter, referred to as a plane estimation method). Conventional example 1.). In the technique described in Conventional Example 1, a Hough curve is plotted on a certain plane from a parallax image that is three-dimensional data, points on the plane through which these Huff curves most pass are determined, and based on this, a certain plane is obtained. The plane object is estimated by correcting the inclination of the perpendicular line of the plane straight line group assumed to be above and the distance to the Y axis.

Further, Japanese Patent Laid-Open No. 9-81755 discloses that
A plane estimation method has been disclosed for the purpose of accurately measuring distance data on a plane (hereinafter referred to as Conventional Example 2). In the technique described in the second conventional example, a stereo image is divided into rectangular small areas, distance data is obtained for each small area, and each small area is grouped in the horizontal direction into these areas. The plane in the imaging space is estimated by fitting a plane passing straight line from the included M pieces of distance data by Hough transform and performing this for all large areas.

Further, Japanese Unexamined Patent Publication No. 7-271978 discloses an image processing apparatus which has a reduced memory capacity and a reduced processing time (hereinafter referred to as Conventional Example 3). Is to input a straight line group from a three-dimensional space, vote a normal vector of this straight line group in the feature space, and extract a plane including a predetermined number of straight lines based on this.

[0015]

However, in the techniques described in the conventional examples 1 to 3, since the plane parameters are not directly obtained and voting is performed in the voting space, the process becomes complicated and the Hough There is a problem that the advantage of the probability estimation of the transformation cannot be fully utilized.

Further, when the Hough transform is used, there is a problem that the processing time becomes long and it is difficult to increase the speed.

As described above, a method for determining an unknown plane parameter from the noisy three-dimensional data, that is, for detecting the plane has not been established.

The present invention has been proposed in view of such a conventional situation, and is a plane extraction method capable of extracting a plane which is robust against noise and has high reliability, its apparatus, and An object of the present invention is to provide a program, a recording medium for the program, and a robot apparatus equipped with a plane extraction device.

[0019]

In order to achieve the above-mentioned object, the plane extraction method according to the present invention samples three-dimensional data of three or more points from a three-dimensional data group, and
A plane calculation step of calculating a plurality of plane parameters indicating one plane determined by the dimension data, and a plurality of plane parameters obtained from the plane calculation step are voted in a voting space to determine a plane based on the voting result. And a process.

In the present invention, the plane parameters are calculated from the three-dimensional data and the plane parameters are directly voted in the voting space, so that the plane can be stably extracted even from the noisy three-dimensional data.

The plane calculation step is a step of calculating the plane parameter (θ, φ, d) when the direction of the normal vector of the plane is (θ, φ) and the distance from the origin is d. Then, the voting space may be represented by the following formula (1-1) or formula (1-2).

[0022]

[Equation 3]

As a result, the plane parameter designating the plane forms polar coordinates with respect to the original three-dimensional Cartesian coordinate space, so that the voting slots (grids) occupy the same area in the original corresponding space. The voting space is defined by the above formula (1
-1) or equation (1-2),
Furthermore, robust and highly accurate data can be obtained.

The three-dimensional data group is converted into a three-dimensional data group based on the input image, and each of the three-dimensional parameters individually has a reliability parameter calculated based on the input image. However, it is possible to have a data selection step of selecting the three-dimensional data of the above-mentioned three-dimensional data group based on this reliability parameter.
Dimensional data can be discarded and the reliability and stability of the processing result can be improved.

Further, the sampling data is obtained by randomly sampling three points, or one randomly sampled reference point and two other randomly sampled points within a predetermined distance from the reference point. And the random sampling can be repeated a plurality of times. Furthermore, it is possible to have a step of calculating the plane parameter from the data of three points by a direct solution method, and it is possible to estimate the plane parameter with high accuracy by Hough transform for voting from the sampling data to the voting space.

The sampling data can be divided into a predetermined area in the three-dimensional space to be three-dimensional data in this area. Furthermore, the plane parameter is calculated by principal component analysis in which the covariance matrix is expanded from the three-dimensional data of three or more points to extract the vector corresponding to the minimum eigenvalue, or the plane does not pass through the origin. The plane parameters can be calculated using the least squares method assuming that the plane parameters are

Further, in the plane determining step, the voting is repeated, and when the total number of votes exceeds a predetermined threshold value, or the sum of the voting values at the position with the highest voting value determined based on the number of votes. Can have a step of cutting off the voting when a predetermined ratio of the total voting value, which is the sum of all voting values, is reached, so that the processing can be speeded up.

Furthermore, since the plane determination step has a weighted voting step of voting by setting different weights according to the reliability of the three-dimensional data and / or the plane calculation method, the reliability of the data is high and the accuracy is improved. Therefore, the total number of votes can be reduced and high-speed processing can be performed.

Further, the plane determining step can include a weighted averaging step of performing a weighted average by the number of votes in the vicinity of the peak of votes of the vote space, and the parameter can be obtained with a precision finer than the quantization size of the vote space. Therefore, the quantization size can be increased and the processing speed can be increased.

Further, there is a mapping step of obtaining a coordinate transformation matrix in which the three-dimensional data included in the plane extracted by the plane decision step becomes two-dimensional, and mapping the three-dimensional data by the coordinate transformation matrix to output. It is possible to facilitate the subsequent processing.

Furthermore, the plane determining step can include an optimum parameter calculating step of calculating an optimum parameter from the initial parameter by iteration using the plane parameter estimated by the voting result as an initial parameter. In the plane parameter estimation stage, the accuracy can be improved by the optimum parameter calculation if estimated with a certain degree of accuracy.
Further speedup is possible.

In the plane determining step, at least one selected from the group consisting of the sharpness of the voting peak in the voting, the number of data points used in the iteration, and the error residual calculated by the iteration is selected. Since it can have a reliability calculation step of calculating the reliability of the plane to be used, which is used as a reliability parameter,
The accuracy of the extracted plane becomes clear, and data with low accuracy can be discarded and the data can be selected.

The plane extraction device according to the present invention comprises plane calculation means for sampling three or more three-dimensional data from a three-dimensional data group and calculating a plurality of plane parameters indicating one plane determined by the three-dimensional data. A plurality of plane parameters obtained from the plane calculation step are voted in a voting space, and a plane determining means for determining a plane based on the voting result is provided.

The robot device according to the present invention is an autonomous robot device that operates based on the supplied input information. The robot device samples three-dimensional data of three or more points from a three-dimensional data group, and Plane calculating means for calculating a plurality of plane parameters indicating one plane determined by data, and plane determining means for voting a plurality of plane parameters obtained from the plane calculating step in a voting space and determining a plane based on the voting result. And having.

A program according to the present invention is a program for causing a computer to execute a predetermined operation,
A plane calculation step of sampling three or more three-dimensional data from a three-dimensional data group and calculating a plurality of plane parameters indicating one plane determined by the three-dimensional data, and a plurality of plane parameters obtained from this plane calculation step In a voting space and determining a plane based on the voting result.

A recording medium according to the present invention is a computer-readable recording medium in which a program for causing a computer to execute a predetermined operation is recorded, and three-dimensional data of three or more points are sampled from a three-dimensional data group. ,
A plane calculation step of calculating a plurality of plane parameters indicating one plane determined by the three-dimensional data, and a plurality of plane parameters obtained from this plane calculation step are voted in a voting space and a plane is determined based on this voting result. And a plane determining step.

[0037]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Specific embodiments to which the present invention is applied will be described below in detail with reference to the drawings. In the present embodiment, a bipedal robot device equipped with a plane extraction device will be described.

FIG. 1 is a block diagram showing an outline of a robot apparatus according to an embodiment of the present invention. As shown in FIG. 1, the head unit 250 of the robot apparatus 1 is provided with two CCD cameras 200R and 200L.
A stereo image processing device 210 is provided behind the CD cameras 200R and 200L. The right eye image 201R and the left eye image 201L captured by two CCD cameras (hereinafter, referred to as right eye 200R and left eye 200L) are
It is input to the stereo image processing device 210. The stereo image processing apparatus 210 calculates disparity information (disparity data) (distance information) of each of the images 201R and 201L, and a color image (YUV: luminance Y, UV color difference) 202 and a parallax image (YDR: luminance Y, parallax D). , Reliability R) 203 is calculated alternately for each frame. Here, the parallax indicates the difference between the points at which a certain point in space is mapped to the left eye and the right eye,
It changes according to the distance from the camera.

This color image 202 and parallax image 203
Is a CPU built in the trunk 260 of the robot apparatus 1.
(Control unit) 220 is input. Also, the robot device 1
An actuator 230 is provided at each joint of
A control signal 221 serving as a command from the CPU 220 is supplied to drive the motor according to the command value. A potentiometer is attached to each joint (actuator), and the rotation angle of the motor at that time is sent to the CPU 220. Each sensor 240 such as a potentiometer attached to this actuator, a touch sensor attached to the sole of the foot, and a gyro sensor attached to the trunk
Measures the current state of the robot apparatus such as the current joint angle, installation information, and posture information, and outputs it as sensor data 241 to the CPU 220. The CPU 220 is input with the color image 202 and the parallax image 203 from the stereo image processing device 210 and the sensor data 241 such as all joint angles of each actuator 230, and realizes a software configuration described later.

The software of this embodiment is configured in units of objects, recognizes the position, movement amount, surrounding obstacles, environment map, etc. of the robot device, and takes action about the action that the robot device should finally take. It is for performing various recognition processes for outputting a column. As the coordinates indicating the position of the robot device, for example, a camera coordinate system of a world reference system (hereinafter also referred to as absolute coordinates) having a predetermined position based on a specific object such as a landmark described later as an origin of the coordinates. , And a robot center coordinate system (hereinafter, also referred to as relative coordinates) centered on the robot apparatus itself (origin of coordinates).

FIG. 2 is a schematic diagram showing the software configuration of the robot apparatus according to this embodiment. In the figure, the circles represent entities called objects or processes. The entire system operates by communicating objects asynchronously.
Each object transfers data and activates a program (Invoke) by an inter-object communication method using message communication and shared memory.

As shown in FIG. 2, software 300
Kinematics Odometr
y) KINE310, Plane Extractor P
LEX320, Obstacle grid calculator (Occupancy Gri
d) OG330, landmark position detector (Landmark Se
nsor) CLS340, absolute coordinate calculation unit (Localization)
LZ350 and Situated behavior Laye
r) It is composed of the SBL 360, and processing is performed for each object. The detailed configuration and operation of these software will be described later. The detailed configuration and operation of the robot device will also be described later.

Here, first, the plane extraction device mounted on the robot apparatus 1 described above, that is, the plane extraction unit P described above.
The plane extraction method of LEX320 is demonstrated.

In the plane extraction method of the present invention, plane parameters are calculated by randomly sampling data from three-dimensional data, and Hough transform (randomized Hough transform: Randomized) is performed.
Hough Transform), that is, the plane parameter is directly voted in the voting space to determine whether or not the plane is a plane. That is, the plane parameter (θ, φ, d), where the direction of the normal vector is (θ, φ) and the distance from the origin is d, from N-dimensional three-dimensional data shown in the following mathematical expression (2), that is, This is a method of obtaining the plane equation shown in the following mathematical expression (3).

[0045]

[Equation 4]

[0046]

[Equation 5]

Then, it is a probabilistic method for determining the parameters of the dominant plane included in the three-dimensional data from the three-dimensional data by voting, that is, estimating the probability density function based on the histogram.

The plane extraction method according to the embodiment of the present invention will be described in detail below. FIG. 3 is a flowchart showing steps of the plane extraction method according to the present embodiment.

For example, a parallax or range image obtained by a method such as stereoscopic vision (stereo camera) or an operation of a range sensor or the like can be easily converted into three-dimensional data by conversion based on appropriate calibration. Then, this array of three-dimensional data is input.

First, N pieces of data (three-dimensional data) {p
₁, P_Two, P_Three,,, p_N} To M subsets {p
₁ ¹, P_Two ¹, P_Three ¹,,, p_N ¹}, {P₁ ^Two, P_Two
^Two, P_Three ^Two,,, p_N ^Two}, ..., {p₁ ^M, P_Two ^M, P
_Three ^M,,, p_N ^M} Is taken out (step S1). this
The total number of votes at that time is M. Then each subset {p ₁
ⁱ, P_Two ⁱ, P_Three ⁱ,,, p_N ⁱ} From the parameters
(Θⁱ, Φⁱ, Dⁱ) Is calculated (step S2). Next
, Quantize the calculated parameters, and
To determine each and vote individually for each parameter
Is performed (step S3). And finally, the number of votes is high
The parameter is the solution. I.e. have this parameter
It is estimated that the plane that is
Is performed to determine the plane (step S4).

First, the processing performed before taking out the data in step S1 will be described. FIG. 4 shows the step S
It is a flowchart which shows the front process of 1. A method of processing a three-dimensional coordinate data group obtained in the form of, for example, a distance image or a parallax image will be described. As shown in FIG. 4, data from the stereoscopic view 401 or the range sensor 402 is input, and calibration or parameter conversion is performed to generate a three-dimensional data array (step S).
01). Here, the reliability parameter indicating the reliability r of the three-dimensional data is input from the stereoscopic view 401 or the range sensor 402 together with the three-dimensional data group converted into the three-dimensional coordinates (step S02). In the present embodiment, a distance image based on stereoscopic vision or the like is used as an input image,
The reliability parameter obtained in the process of stereo distance calculation can be effectively used. Further, when such a reliability parameter is not available, it can be directly applied to three-dimensional data not accompanied by the reliability parameter by treating it as a constant.

Although the reliability parameter can be calculated using various evaluation values, in the present embodiment, the reliability parameter can be calculated by the variance value of the template and the reliability parameter can be calculated by the matching score. Two methods will be described.

As described above, the three-dimensional data according to the present embodiment is input with the stereoscopic range image.
In stereo distance measurement, the correspondence between pixels in the left and right stereo images is searched for by template matching. For example, using the left stereo image as the reference image and the right stereo image as the matching destination image, the variance value of the luminance of the pixels in the matching template is obtained. Then, the variance value of all the templates to be searched can be calculated, and the magnitude of the variance value can be used as the reliability parameter. That is, the higher the variance value is, the higher the reliability is, and the lower the variance value is, the lower the reliability is.

As a method of obtaining the reliability image by the matching score, a template in the reference image and
A region selected along the epipolar line of the matching destination image is compared, and a matching score value is calculated from the difference in luminance of pixels in these regions. Then, the reliability parameter based on the matching score can be obtained from the matching score value, the peak width near the minimum matching score value, the steepness value of the graph, and the like. That is, the matching score value is low, the peak width near the minimum score value is small, and the steepness value of the graph is large,
Shows high reliability.

As described above, the reliability parameter is calculated individually for each three-dimensional parameter, and the input data is selected by truncating the data whose reliability is lower than the predetermined threshold value (step). S0
3). In any of the conventional examples described above, since no consideration is given to the reliability of the input three-dimensional data, there is a problem in that the data tends to be mixed with a lot of noise, and this noise deteriorates the performance. By selecting the three-dimensional data by the reliability parameter as in the embodiment, it is possible to improve reliability and stability and prevent performance deterioration.

Next, a method for extracting M subsets from the N three-dimensional data in step S1 will be described. FIG. 5 is a flowchart showing the steps S1 to S3. Step S03 described above
The three-dimensional data group (x, y, z) selected in step 3 and the reliability parameter (reliability r) associated with each data are output (step S11), and the plane parameters are estimated from these data groups. Sampling is performed to extract the sample. As a sampling method, three-dimensional data (x, y, z) can be used for 1. 1. Method of sampling at random A method of randomly sampling one point serving as a reference point from the entire three-dimensional data and sampling the remaining data from points near the reference point, for example, a data group having a sufficiently short distance in the three-dimensional space. There is a method in which data in each small neighborhood (for example, a data group having a sufficiently short distance in a three-dimensional space) is collected as a subset while scanning over a plurality of resolution levels.

The data group having a sufficiently short distance in the three-dimensional space is, for example, the distance from the reference point to the threshold values d _{min to} d.
It can be any point such that it is _max .

Here, when it is assumed that a dominant single plane exists in the input data, or when it can be known that a dominant plane exists by some additional information, When the plane area is large, it is effective to sample the entire screen in the high frequency range. In addition, sampling data in a small area is effective for an image including many small steps.

Next, the method of calculating the plane parameters in step S2 will be described. Step S shown in FIG.
21 has three types of plane parameter calculation methods, and an optimum calculation method can be taken as necessary. First, the first parameter calculation method is a method of determining parameters from a three-dimensional data of three points by using a direct solution method.

The plane α is determined by three parameters: the direction of the normal vector and the distance from the coordinate origin. That is, as shown in FIG. 6, when the direction of the normal vector is displayed in polar coordinates with (θ, φ) and the distance from the origin is d, the following mathematical expression (4) is an equation that determines the plane α.

[0061]

[Equation 6]

From another point of view, the above equation (4)
Can be replaced with the following formula (5).

[0063]

[Equation 7]

Therefore, it is extremely easy to obtain (θ, φ, d) satisfying the above equation (4) if the coordinates of three points that are not on the same straight line are given. If either one is obtained, the other is immediately obtained. It is determined.

For example, when three points {p ₀ , p ₁ , p ₂ } are given, the equation of the plane shown by the following equation (6) is the vector m = (p ₁ −p ₀ ) × (p ₂ -P ₀ ) is used to uniquely calculate by Equation (7) below.

[0066]

[Equation 8]

[0067]

[Equation 9]

Assuming that the generality is not impaired,
3 points are not on the same line (| vector m | ≠ 0), 3
One of the points, p _0, does not coincide with the origin (| p ₀ | ≠ 0)
I shall.

The second parameter calculation method is a plane parameter estimation method by principal component analysis. That is, the principal component analysis is performed on each data group obtained by scanning the entire screen with appropriate resolution granularity and resolution and dividing the input three-dimensional data into neighboring regions in the three-dimensional space. Is a method of estimating the plane parameters.

First, N pieces of three-dimensional data expected to be on the same plane are represented by the following formula (8).

[0071]

[Equation 10]

From the above equation (8), these covariance matrices C = PP ^T are (ideally) 3 × of rank (rank) 2.
There are 3 matrices. That is, if C is eigenvalue decomposed and the unique vector corresponding to the minimum eigenvalue is taken, this becomes the plane normal vector. Here, the first and second principal components give two vectors extending in a plane, and the vector perpendicular to these two vectors becomes the third principal component. Thus, from the principal component analysis, the following mathematical expression (9
The plane equation shown in -1) is calculated by the following mathematical expression (9-2).

[0073]

[Equation 11]

However, v ₃ is an eigenvector corresponding to the minimum eigenvalue λ ₃ . Thus, the plane parameters (θ, φ, d) can be calculated immediately by the second method as well.

A third parameter calculation method for obtaining the plane parameter is a method of fitting the plane parameter using the least square method. For example, in the same manner as described above, the entire screen is scanned with an appropriate division granularity and resolution, the obtained distance image or the like is divided into, for example, rectangular small areas, and the like.
This is a method of dividing a three-dimensional data group for each neighboring area in a dimensional space and calculating a plane parameter using the linear least squares method.

This is a method that is expertly specialized in the situation to which the embodiment of the present invention is applied, and is not a mathematically general method. In other words, it is a method of obtaining it assuming that the plane to be estimated does not pass through the coordinate origin. With this assumption, the simple solution method described below can be applied.

First, N pieces of three-dimensional data expected to be on the same plane are represented by the above equation (8), and the equation of this plane is represented by the following equation (10).

[0078]

[Equation 12]

The equation represented by the above equation (10) is an equation representing an arbitrary plane that does not pass through the coordinate origin. Here, the parameter Γ = (α, β, γ) has no constraint condition. At this time, the equation to be solved is the following equation (11).

[0080]

[Equation 13]

Therefore, if the generalized inverse matrix P ⁻¹ of P is calculated by a method such as singular value expansion, the parameter Γ shown in the following equation (12) becomes the error residual r shown in the following equation (13).
This gives an optimal solution that minimizes in the sense of least squares.

[0082]

[Equation 14]

[0083]

[Equation 15]

Here, the following mathematical expression (14) is established.

[0085]

[Equation 16]

Next, the voting in step S3 will be described. As a parameter that determines the plane (θ, φ,
The use of d) is quite natural. Here, it can be considered that the normal vector n of the plane exists on the surface of the unit sphere having a radius of 1 with the origin as the center, and the following equation (15)
The relationship shown in is established.

[0087]

[Equation 17]

However, considering a slight change in the normal vector, since the Jacobian J is given by the following mathematical expression (16), the parameters (θ, φ, d) that determine the plane are the voting destinations when the Hough transform is used. It is not appropriate to use it as a parameter that stretches the orthogonal axis of space.

[0089]

[Equation 18]

That is, when the polar coordinates of the parameters (φ, θ) are taken, in the voting space 500 shown in FIG. 7A, each grid 501 has the same area, whereas in FIG.
As shown in (b), the areas shown in the actual space are different.
That is, when the actual space is a sphere 510, for example, the area of the grid 511 on the sphere 510 in the direction perpendicular to the central axis from the center is a straight line of A passing through the center of the sphere and parallel to the paper surface and upward. Is S, for example, the area of the grid 512 on the sphere 510 in the direction of the central axis from the center is S cos θ, and the areas of the grid are different. Therefore, when voting in the voting space 500 shown in FIG. 7A, the voting actually takes place on grids of different areas, and it is not possible to obtain an appropriate voting value.

In this way, when the three-dimensional parameter space of the voting destination is simply designed and voted, the grids of different areas are voted in the original corresponding space, and the peak may not appear sharply. It is vulnerable to noise.

Here, the present inventors have found that the above-mentioned problem can be solved by defining the voting space by the following formula (1-1) or formula (1-2).

[0093]

[Formula 19]

FIG. 8 is a schematic diagram showing the voting space shown in the equation (1-1). As shown in FIG. 8A, by defining the voting space 600 by the above mathematical expression (1-1), each of the orthogonal grids 601 partitioned into the voting space 600 is the actual grid shown in FIG. 8B. Three-dimensional space 6
It can be considered that the small changes in the plane at 10 are uniformly associated. Assuming that the straight line passing through the center of the sphere in the direction parallel to the plane of the drawing is the central axis, the grid 611 in the direction parallel to the central axis and the vertical axis to the central axis are assumed by the above-mentioned mathematical expression (1-1). The grids 612 in both directions have the area S, and the grid sizes are the same. Further, even if the voting space is the space represented by the above mathematical expression (1-2), each grid occupies the same area in the original corresponding space. The voting space may be designed so that each grid occupies the same area in the corresponding space, and is not limited to the ones expressed by the mathematical formulas (1-1) and (1-2).

As described above, the plane parameters that specify the plane form polar coordinates with respect to the original three-dimensional rectangular coordinate space, so that the voting slots (grids) have the same size in the original corresponding space. The voting space in the above formula (1
By designing as -1) or (1-2), more robust and highly accurate data can be obtained. Therefore, the number of samplings can be reduced and the processing speed can be increased.

Next, the voting in step S3 will be described. In general, the weight of one vote when voting is often the same. That is, for example, for one vote, the vote value is set to one. On the other hand, in the present embodiment, the weight of one vote is determined in consideration of the nature of the error, and the robustness against noise is improved. That is, for one vote, the vote value is set to, for example, 0.5 or 2 by evaluating the error. As a method of evaluating the error in the number of votes, there is a method described below.

First, the first error evaluation method is a method of using the reliability (reliability parameter) of the original three-dimensional data used for calculating the voting destination (calculating the plane parameter). In this case, the voting values can be weighted by, for example, averaging the reliability associated with each of the three-dimensional data used to determine the plane parameter.

The second error evaluation method is a method using the standard deviation of the error of the original three-dimensional data used for calculating the voting destination. The error is, for example, an error of 0 in the first parameter calculation method for obtaining the plane parameter from the three points by the direct solution method, and is essentially 0 in the second parameter calculation method of obtaining the plane parameter by the principal component analysis. A value calculated from the power minimum eigenvalue λ ₃ or a value calculated from the error residual r can be used in the case of the third parameter calculation method for obtaining the plane parameter by the least square method.

As a third error method, when three-dimensional distance data is obtained from a stereoscopic device, a range sensor or the like, the error in the parallax direction is generally very small,
The error in the distance direction is proportional to the measured distance. As a result, the larger the solid angle when the distribution of the three-dimensional data of the calculation source is projected onto the spherical surface at the center of the viewpoint, the smaller the influence of the measurement error on the measurement result. Weight. Also, when the solid angles are the same, the greater the variance of the data distribution in real space,
In the second method based on principal component analysis, the larger the product λ ₁ λ _{2 of} the first principal component and the second principal component, the smaller the influence of the measurement error on the calculation result. The voting value can be weighted in consideration of this fact.

In this way, by differently weighting the voting value of one vote by the error evaluation, variations in input data, variations due to the characteristics of a device for capturing a range image, etc., and a sampling method. It is possible to reduce variations and the like. Further, from these, it is possible to obtain an index of the sensitivity to the error of the calculation result.

Based on the evaluation values of these errors, it is possible to obtain different voting values that are robust against noise by changing the weighting of the voting value of one vote at the time of voting (step S31).
Then, it is determined whether the total number of votes exceeds the threshold N _max , or whether the total number of votes exceeds the threshold N _max and the distribution of the vote values forms a sufficient peak (step S3).
2). Sufficient peak means that the data in the grid at the peak position has a certain ratio or more of the total number of votes (total votes), or the total number of votes in the grid at the peak position (the number of votes) or the vote value. Total (Voting value)
Can be determined by whether or not is greater than or equal to a predetermined threshold. Here, when it is determined that it is sufficient, the plane parameters in the grid at the peak position or its vicinity are averaged by the method described later. On the other hand, if it is determined that the plane parameter is not sufficient, the process returns to step S21 to extract the data subset from the three-dimensional data group to calculate the plane parameter.

Next, step S4 for determining the plane will be described. 9 and 10 are flowcharts showing the plane determining step. In the plane determination in step S4, as shown in FIG. 9, first, a weighted averaging process in the vicinity of the peak value is performed (step S41). This weighted averaging process allows the more accurate position of the peak to be estimated.

Let g be the grid set near the peak value of the voting space.
Then, the voting value (total of voting values) of each grid G _i included in this set g is v _i , and the representative parameter is (θ _i ,
If φ _i , d _i ), the estimated parameters (θ _k , φ _k ,
d _k ) is calculated as the following mathematical expression (17).

[0104]

[Equation 20]

As shown in the equation (17), parameter estimation can be performed with a finer granularity than the quantization size of the representative parameter of each grid. Here, the above formula (1
In 7), it is assumed that the probability distribution is almost normal in the vicinity of the peak value.

As shown in FIG. 9, initial values (initial parameters) (θ ₀ , φ ₀ , d ₀ ) of estimated parameters are output by the weighted averaging process in step S41. next,
An iteration is calculated from the initial parameters (θ ₀ , φ ₀ , d ₀ ) and the three-dimensional coordinate 700 (step S4).
2). With this iteration, more accurate plane parameters are estimated. Then, as shown in FIG. 10, the end determination is made by determining whether the variation of the parameter due to the iteration of step S42 has converged or repeated a predetermined number of times or more (step S43), and the reliability of the result is determined. (Step S44), the plane parameter (θ, φ, d) for which termination is determined and its reliability are output.

In this embodiment, the three-dimensional data is multiresolution-ized in real time by the filter bank (FBK), and the low-resolution three-dimensional data is used in the initial parameter estimation by the Hough transform. In this iteration, the precision is further improved by using high-resolution three-dimensional data. That is, in the iteration, points that are sufficiently close to the estimation plane estimated from the initial parameters are sampled.

The steps S42 and S43 will be described in more detail below. FIG. 11 is a flowchart showing steps S42 and S43. As shown in FIG. 11, when the initial parameters (θ ₀ , φ ₀ , d ₀ ) are input, an error tol is calculated from the input three-dimensional data set filtered by the reliability parameter in step S03 described above. A data point on the estimation plane, that is, a data subset P _i satisfying the condition of the following formula (18) is extracted (step S421). In the error tol, the initial value tol ₀ of the error allowable range is appropriately determined in advance.

[0109]

[Equation 21]

Next, the generalized inverse matrix P _i ^-1 of Pi shown in the following equation (19) is calculated, and the plane parameters Γ (α _i , β _i , which are optimally fitted to the data in the meaning of least squares are given.
γ _i ) = (− 1, −1, ..., −1) P _i ⁻¹ is calculated (step S422).

[0111]

[Equation 22]

Then, it is judged whether or not the change amount of the parameter is considered to be sufficiently small and converged, or whether the number of iterations reaches a preset maximum number of iterations Iter _max (step S431), and it is converged or the maximum number of iterations is reached. If the number Iter _max is reached, step S
Proceed to 432. It is also possible to reduce the error tol and accelerate the convergence. On the other hand, if it has not converged or has not reached the predetermined number of times, the error allowable value tol _i in the next iteration is determined from the error residual r _i , i is incremented, and the process returns to step S421.

From the data input from step S431, the plane parameters (θ, φ,
d) is determined (step S342).

[0114]

[Equation 23]

Returning to FIG. 10, in step S44, the plane parameters (θ, φ,
Calculate the reliability of d). In the calculation of the reliability, if there is prior knowledge, the plane parameter (θ, φ, d) calculated in step S432 is compared with the prior knowledge, and it is confirmed that it is within the expected range. If it is outside, reject this value. In addition, the number of votes in the vicinity of the peak value in step S3 described above, or the ratio of the votes to the total number of votes or votes, and the number of three-dimensional data used for parameter estimation in the iteration of step S42 are estimated results. Can be used as a confidence level. That is, the reliability can be calculated by each index shown in the following formulas (21-1) to (21-3). The reliability calculated here is output together with the plane parameters. If the prescribed reliability is not reached, the data can be rejected.

[0116]

[Equation 24]

When a plurality of peaks are detected in the voting space, the above-described parameter estimation process (steps S42 to S44) is repeated for each peak, and the parameter with the highest reliability is determined as the plane parameter. To do.

Furthermore, the determined plane parameters (θ,
For a plane having φ, d), a coordinate transformation matrix with z ′ = 0 is obtained, the input data group is mapped by the transformation matrix, and the transformed matrix is output, so that the later-described processing is convenient.

In the present embodiment, since the plane parameters can be directly estimated from the three-dimensional data by the Hough transform, the advantage of the Hough transform as the probability density estimation, that is, the data robust to the noise can be obtained. In addition, since the plane parameters can be randomly sampled, the overall statistical feature points can be grasped faster. In addition, the voting space of the three-dimensional parameter is defined by the above formula (1-1) or (1-2).
Therefore, more accurate voting results can be obtained. Furthermore, since the reliability parameter is used to sort the input three-dimensional data with a lot of noise and the reliability of the three-dimensional parameter itself obtained from the sorted three-dimensional data is also taken into consideration, the noise is reduced. It is extremely robust with respect to and can provide highly reliable and stable results. Furthermore, by cutting the voting at an appropriate number of times and improving the accuracy by iteration or the like, it is possible to speed up even if the plane parameters are obtained by the Hough transform. Since the input data is sampled by hardware at multiple resolutions, the speed can be increased in the phase of grasping the overall structure of the entire data.

In the Hough transform, there is a trade-off relationship between the quantization size of the voted three-dimensional parameter space and the accuracy. That is, if the quantization size is reduced, the accuracy is improved, but the number of samplings must be increased and it takes time to detect peaks. However, if the quantization size is increased, the speed is increased, but the granularity of the result is coarse and the accuracy is high. Is reduced. In the present embodiment, by performing weighted averaging around the peak of the voting result, it is possible to estimate parameters with a finer granularity than the quantization size of the three-dimensional parameter space of the voting destination. Furthermore, since the final result is generated by performing optimization by iteration using the Hough transform result as an initial parameter, sufficiently high accuracy can be obtained even if the quantization size of the voted three-dimensional parameter space is set to a certain size. .

Further, in the Hough transform, if the size of the three-dimensional parameter space of the vote destination becomes large, the processing speed becomes slow and the amount of data to be stored also increases. Therefore, the positional relationship between the camera and the plane is changed by another means. Further, the speed can be further increased by limiting the search space of the parameter by using the information obtained in step 1. As described above, in the present embodiment, it is possible to realize high speed while taking advantage of the Hough transform that is robust against noise and has high accuracy.

Furthermore, when the plane parameters are estimated from the three-dimensional data group, it is difficult to solve by direct release in one step because the relational expression includes a non-linear constraint condition, but in the present embodiment, the camera is used. With respect to the positional relationship between the plane and the plane, by making a special assumption, that is, the plane does not pass through the camera position (origin), it is reformulated into a simple linear problem, and by using this formulation,
The plane equation (plane parameter) can be calculated extremely easily.

Next, the plane extraction unit PLEX3 described above.
The software configuration and operation of the robot device 1 shown in FIG. Figure 1
2 is a flowchart showing the operation of the software 300 shown in FIG.

As described above, the image data 301 and the sensor data 302 are input to the kinematic odometry KINE 310 of the software 300 shown in FIG. This image data 301 is a color image and a parallax image from a stereo camera. Also, sensor data 3
02 is data such as the joint angle of the robot apparatus. The kinematic odometry KINE 310 receives these input data 301 and 302, and updates the image and sensor data stored so far in the memory (step S101).

Next, the image data 301 and the sensor data 302 are temporally associated (step S102).
-1). That is, the joint angle of the sensor data 302 at the time when the image of the image data 301 is captured is calculated. Next, using this joint angle data, the robot central coordinate system in which the robot apparatus 1 is fixed to the center is converted into the coordinate system of the camera provided in the head unit (step S10).
2-2). In this case, in the present embodiment, the simultaneous conversion matrix and the like of the camera coordinate system are derived from the robot center coordinate system, and this simultaneous conversion matrix 311 and the image data corresponding thereto are transmitted to the object that performs image recognition. That is,
Simultaneous conversion matrix 311 and corresponding parallax image 312
To the plane extraction unit PLEX320, and the simultaneous conversion matrix 3
11 and the color image 313 in the landmark sensor section CL
Output to S340.

Further, the movement amount of the robot apparatus 1 is calculated from the walking parameters obtained from the sensor data 302 and the number of steps counted using the sole sensor, and the movement amount of the robot apparatus 1 in the robot apparatus central coordinate system is calculated. calculate. Hereinafter, the amount of movement of the robot device central coordinate system is also referred to as odometry. The odometry 314 is output to the obstacle grid calculation unit OG330 and the absolute coordinate calculation unit LZ350.

The plane extraction unit PLEX 320 receives the simultaneous conversion matrix 311 calculated by the kinematic odometry KINE 310 and the parallax image 312 obtained from the stereo camera corresponding to the simultaneous conversion matrix 311, and the parallax image 312 is stored in the memory until then. Also, these data are updated (step S103). Then, three-dimensional position data (range data) is calculated from the parallax image 312 using the calibration parameters of the stereo camera (step S).
104-1). Then, using the Hough transform or the like, the planes other than the planes such as the wall and the table are extracted as the planes from the range data. Further, by taking a correspondence from the coordinate transformation matrix 311 with a plane where the sole of the robot apparatus 1 is in contact with the ground, selecting a floor surface, a point that is not on the floor surface, for example, a position higher than a predetermined threshold value, etc. Is used as an obstacle to calculate the distance from the floor surface, and the obstacle information (obstacle) 321 is output to the obstacle grid calculation unit 330 (step S1).
04-2).

In the obstacle grid calculation unit OG330, as described above, the kinematic odometry KINE31.
Odometry 314 calculated at 0 and plane extraction unit PL
When the obstacle observation information (obstacle information) 321 calculated by the EX 320 is input, the data stored up to that point stored in the memory is updated (step S105). Then, the obstacle grid holding the probability of whether or not there is an obstacle on the floor is updated by the stochastic method (step S
106).

This obstacle grid calculation unit OG330
For example, information about an obstacle around the robot device 1 around 4 m, that is, the environment map described above, and posture information indicating the direction in which the robot device 1 faces are held, and the environment map is updated by the method described above. By outputting the updated recognition result (obstacle information 331), the upper layer, that is, the route plan determination unit SBL3 in the present embodiment.
At 60, a plan to avoid obstacles can be created.

When the simultaneous conversion matrix 311 and the color image 313 are input from the kinematic odometry KINE 310, the landmark sensor CLS 340 updates these data stored in advance in the memory (step S107). Then, the color image 313 is subjected to image processing to detect a color landmark recognized in advance. The position and size of this color landmark on the color image 313 are converted into the position on the camera coordinate system.
Further, using the simultaneous conversion matrix 311, the position of the color landmark in the camera coordinate system is converted into the position in the robot center position coordinate system, and the information of the color landmark position in the robot center position coordinate system (color landmark relative position The information) 341 is output to the absolute coordinate calculation unit LZ350 (step S108).

The absolute coordinate calculation unit LZ350 uses the odometry 314 from the kinematic odometry KINE310.
And the color landmark relative position information 341 from the landmark sensor unit CLS340 are input, these data stored in advance in the memory are updated (step S109). Then, the absolute coordinate calculation unit LZ3
50 uses the absolute coordinates (position in the world coordinate system) of the color landmarks, the relative position information 341 of the color landmarks, and the odometry 314 that the 50 recognizes in advance, and uses the probabilistic method to determine the absolute coordinates (world coordinate system) of the robot device. Position) is calculated. And this absolute coordinate position 35
1 is output to the route plan determination unit SBL360.

The route plan determination unit SBL360 receives the obstacle grid information 331 from the obstacle grid calculation unit OG330.
Is input and the absolute coordinate position 351 is input from the absolute coordinate calculation unit LZ350, these data stored in advance in the memory are updated (step S111). Then, the recognition result regarding the obstacle existing around the robot apparatus 1 is acquired from the obstacle information 331 from the route plan determination unit SBL360 obstacle grid calculation unit OG330, and the current coordinate of the robot apparatus 1 is acquired from the absolute coordinate calculation unit LZ350. By acquiring the absolute coordinates, a path that can be walked without colliding with an obstacle is generated for the target point given in the absolute coordinate system or the robot center coordinate system centered on the robot device, and the route is set according to the route. Issue an action command to perform. That is, from the input data, the robot device 1 can be used according to the situation.
Determines the action to be taken by the user and outputs the action sequence (step S112).

In the case of human navigation, the obstacle grid calculation unit OG330 gives the recognition result regarding the obstacles existing around the robot apparatus and the absolute coordinates of the current position of the robot apparatus from the absolute coordinate calculation unit LZ350. It is provided to the user and the operation command is issued in response to the input from the user.

FIG. 13 is a diagram schematically showing the flow of data input to the above software. Note that FIG.
2, the same components as those shown in FIGS. 1 and 2 are designated by the same reference numerals, and detailed description thereof will be omitted.

Face Detection Unit FDT (FaceDetecto)
r) 371 is an object that detects a face area in the image frame, receives the color image 202 from an image input device such as a camera, and reduces and converts the color image 202 into, for example, a nine-step scale image. A rectangular area corresponding to the face is searched from all the images. The face identification unit F outputs the information 372 such as the position, size, and feature amount regarding the area finally determined to be the face by reducing the overlapping candidate areas.
I (Face Identification) 377.

The face identifying section FI377 is an object for identifying the detected face image, receives information 372 consisting of a rectangular area image indicating the area of the face from the face detecting section FDT371, and stores this face image in the memory. The person is identified by comparing which person in the existing person dictionary corresponds to. Then, the ID information 378 of the person is output to the DIL 379 together with the position and size information of the face image area of the face image received from the face detection unit FDT371.

Color recognition unit MCT (MultiColorT)
The racker) 373 is an object for performing color recognition, receives the color image 202 from an image input device such as a camera, extracts a color area based on a plurality of color model information held in advance, and forms a continuous area. To divide.
The color recognition unit MCT 373 provides information 374 such as the position, size, and feature amount of each divided area to the distance information addition unit DIL.
(DistanceInformationLinke
r) Output to 379.

Motion Detection Unit MDT (MotionDete)
(ctor) 375 is an object for detecting a moving portion in the image, and information 37 of the detected moving area.
6 is output to the distance information adding unit DIL379.

The distance information adding unit DIL379 is an object which adds distance information to the input two-dimensional information and outputs three-dimensional information. The ID information 378 from the face detecting unit FI377 and the color recognizing unit MCT373. The distance information is added to the information 374 such as the position, size, and feature amount of each of the divided regions of FIG.
The data is output to the M (Short Term Memory) 381.

The short-term storage unit STM381 is an object that holds information about the external environment of the robot apparatus 1 for a relatively short time, and ArthurDecoder
The voice recognition result (word, sound source direction, certainty factor) is received from (not shown), the position and size of the flesh color area and the position and size of the face area are received from the color recognition unit MCT373, and the face recognition unit FI377 receives the person. Receives ID information and the like. Further, the direction (joint angle) of the neck of the robot apparatus is received from each sensor on the body of the robot apparatus 1. Then, by using these recognition results and sensor outputs in an integrated manner, it is possible to obtain information about which person is currently in which person, the spoken word belongs to which person, and what kind of dialogue has been performed with that person so far. save. The physical information about such an object, that is, the target, and the event (history) viewed in the time direction are output, and the route plan determination unit (situation-dependent action hierarchy) (SB
L) Pass it to a higher-level module such as 360.

The route plan determining unit SBL360 uses the information from the above-mentioned short-term storage unit STM381 as the basis for the robot apparatus 1.
It is an object that determines the behavior of (the behavior depending on the situation). Multiple actions can be evaluated and performed simultaneously. In addition, the action can be switched to put the aircraft in a sleep state, and another action can be activated.

The above objects will be described in more detail below. First, the plane extraction unit PLEX320 described in the above embodiment will be described. The plane extraction unit PLEX 320 receives the simultaneous conversion matrix 311, and the parallax image 312 obtained from the left-eye image 201L and the right-eye image 201R shown in FIG. 14 corresponding to the simultaneous conversion matrix 311, and removes an obstacle according to the processing procedure shown in FIG. 15 below. recognize.

First, the plane extraction unit PLEX320 receives the simultaneous conversion matrix 311 and the parallax image 312 corresponding thereto (step S61). Then, three-dimensional position data (x, y, z) viewed from the camera coordinates for each pixel is generated as a distance image using a calibration parameter that absorbs lens distortion and stereo mounting error from the parallax image 312. (Step S62).

Next, the plane extraction unit PLEX320 detects the parameters of the dominant plane in the image from the range image generated in step S62 by using Hough transform or the like (step S63). The parameters of this plane are detected by the histogram of the parameter space (θ, φ) (voting space) shown in FIG. A small parameter vote can be regarded as an obstacle, and a large parameter vote can be regarded as on a plane. Using the parameters of this plane, it is possible to know how far the distance measurement point originally obtained from the image is on the plane. The above is as described in the above embodiment.

Next, the simultaneous conversion matrix 311 of the camera coordinate system
To conversion of the robot to the ground contact surface of the sole of the foot as shown in FIG. 17 (step S64). As a result, the parameters of the ground plane expressed in the camera coordinate system are calculated. Then, based on the detection of the plane in the image in step S63 and the collation of the sole grounding plane in step S64, the one corresponding to the floor surface is selected from the plane parameters in the image (step S65).

Next, using the plane parameters selected in step S65, points on the plane are selected from the original range image (step S66). This is determined by using the following formulas (22) and (23) by using the fact that the distance d from the plane is smaller than the threshold value _Dth .

[0147]

[Equation 25]

[0148]

[Equation 26]

FIG. 18 shows the measurement points (marked with X) selected within the range where the threshold value D _th is 1 cm. In FIG. 18, the black dots are points that were not determined to be flat.

Therefore, points other than the points lying on the plane (floor surface) in step S66 (points not on the floor surface) can be recognized as obstacles in step S67. These judgment results are expressed by a point (x, y) on the floor plane and its height z. For example, the height z <0 indicates a point that is recessed from the plane.

As a result, an obstacle point having a height higher than that of the robot can pass through the obstacle point, so that it can be determined that the obstacle point is not an obstacle.

The robot view ((a) in FIG. 19)
The height z of the floor surface extraction image ((b) of FIG. 19) obtained from FIG.
If the coordinates are converted so that (z = 0), it is possible to represent a floor or an obstacle at a two-dimensional position on a plane as shown in (c) of FIG.

As described above, the obstacle recognizing device can detect a stable plane because the plane is detected using a large number of measurement points. In addition, the correct plane can be selected by comparing the plane candidates obtained from the image with the floor surface parameters obtained from the posture of the robot. Moreover, since the floor surface is substantially recognized instead of recognizing the obstacle, it is possible to recognize the obstacle irrespective of the shape or size of the obstacle. Further, since the obstacle is represented by the distance from the floor surface, it is possible to detect even a small step or a recess. In addition, it is easy to determine whether to cross over or pass through considering the size of the robot. Furthermore, since it represents an obstacle on a two-dimensional floor surface, the method used by existing mobile robots can be applied to route planning and the like.
It can be operated faster than dimensional representation.

The obstacle grid calculation unit OG330 divides the grid into a grid of a predetermined size and is an environment map which is the map information of the robot center coordinate system, and a predetermined x-axis direction or y-axis direction on the environment map. And orientation information indicating the direction in which the robot device faces from the direction. In addition, the environment map includes the plane extraction unit PLEX3 whose obstacle information is described above.
It has a grid (obstacle occupied area) input from 20 and recognized as an obstacle. This obstacle grid calculation unit OG330 is used when the robot apparatus moves, that is,
When the odometry 314 is input, the environment map and the posture information stored in advance in the memory are recognized in advance along with the change in the posture (difference movement angle) and the movement distance (difference movement amount) of the robot apparatus. Update the information on the environment map and posture direction. Here, if the difference movement amount is smaller than the grid size, the environment map is not updated, and if the movement amount exceeds the grid size, the environment map is updated. Further, by appropriately changing the size of the environment map and the grid thereof as necessary, it is possible to reduce the calculation amount and the memory copy cost.

The landmark position detector CLS340 is
As shown in FIG. 20, for example, the green portion 1001,
In an environment including an artificial color landmark 1004 having a color such as a pink portion 1002 or a blue portion 1003, the robot device 1's self-position (position) is determined by the sensor information of the robot device 1 and the motion information performed by the robot device 1. And posture).

For example, in a two-dimensional work space, grids (x, y) are provided at substantially equal intervals, and the probability p that a robot device exists for each position l (localization) of each grid.
Manage (l). The existence probability p (l) is updated in response to the movement of the robot apparatus, that is, the internal observation information a, or the observation of the landmark, that is, the external observation information s.

The existence probability p (l) is the existence probability p (l ') at the previous state of the robot apparatus, that is, the self position l'.
And the transition probability p (l | a, l ′) that the state becomes l when the movement a is performed in the previous state l ′. That is, the probability p (l ') of each state l'
And adding (or integrating) the products of transition probabilities p (l | a, l ′) that the state 1 is obtained when the movement a is performed in the state l ′. It converges to the existence probability p (l) that the position is 1. Therefore, when the movement a of the robot device as the result of the external world observation is observed, the existence probability p (l) of the robot device can be updated in each grid according to the following mathematical expression (24).

[0158]

[Equation 27]

Further, the existence probability p (l) that the robot device exists at the state, that is, at the self position l is the existence probability p.
(L) and the transition probability p (s | l) of observing a landmark in this state l. Therefore, in the state 1, the landmark observation, that is, the external observation information s
When is input, the existence probability p (l) of the robot device can be updated according to the following equation. However, as shown in the following mathematical expression (25), the right side is normalized for normalization by dividing by the probability p (s) of observing the landmark.

[0160]

[Equation 28]

FIG. 21 shows a landmark sensor CLS (self-localization system) 3 according to the present embodiment, which uses both Markov localization and extended Kalman filter.
The functional configuration of 40 is schematically shown. As shown in the figure, the landmark sensor CLS340 includes a Markov localization unit (ML) 342, an extended Kalman filter unit (EKL) 343, and an EKL control unit 344 that controls the operations of the extended Kalman filter unit 343. It consists of and.

Markov Localization Section 342
Holds its own position in the work space as a self-position probability density distribution on a discrete grid, and inputs external observation information s regarding the observation of landmarks and internal observation information a regarding the movement of the robot device itself. Then, the self-position probability density distribution is updated. Then, at each time point, the grid having the highest value of the self-position probability density distribution is output to the EKL control unit 344 as the self-position estimation result.

FIG. 22 shows the self-position probability density distribution on each grid obtained by the Markov localization unit 342. In the figure, the probability density in each grid is represented by shading. Darkest color,
That is, the grid having the highest value of the self-position probability density distribution becomes the self-position estimation result.

The self-position identification by the Markov localization is robust against the noise of the sensor, and is characterized mainly in that the accuracy of the identification solution is coarse but the convergence speed of the solution is fast.

On the other hand, the extended Kalman filter unit 343 shown in FIG. 21 holds the self position as a state variable [x, y, θ] as an actually measured value, and observes the color landmark 1004 installed in the environment, The self-position is estimated based on the relative position from the landmark. Further, when observing its own motion information, the state quantity is estimated based on the motion information.

The extended Kalman filter unit 343 shows the relationship between the motion information a of the robot apparatus itself and the state, that is, the state model defining the relationship between the self position l and the self position l and the observation information s of the landmark. It consists of a specified observation model.

The state model has a transition function F (l, a) that gives a theoretical state 1 when the robot apparatus performs the action a in the state (self position) l. In reality, since the noise component w is superimposed on the theoretical state 1, the state 1 of the robot apparatus converges according to the following equation (26) according to the state model.

[0168]

[Equation 29]

Further, the observation model is such that the robot device is in a state, that is, in the self position i, in a known environment Env.
It is provided with an observation function H (Env, l) that gives a theoretical value s of observation (for example, the position of a landmark).
Since the noise component v is actually superimposed on the theoretical value of observation, the observed value s is calculated by the following mathematical formula (2
It converges like 7).

[0170]

[Equation 30]

Note that the noises w and v superimposed on the state 1 and the observation s are assumed to be Gaussian distributions whose center value is zero.

An extended Kalman filter unit 343 having a state model defining the relationship between the motion information a of the robot apparatus itself and the self position l and an observation model defining the relationship between the self position l and the observation information s of the landmark. In the above, the operation information a is the observation information s of the landmark as the internal observation result.
Are known as external observation results. Therefore, the self-position identification of the robot device can be reduced to the problem of estimating the state 1 of the robot device from the motion information a and the observation information s of the robot device. Here, the operation a, the state l, and the observation s of the robot device can be expressed as a Gaussian distribution shown below.

[0173]

[Equation 31]

It is estimated that the state 1 of the robot apparatus at a certain time point is a Gaussian distribution having a certain median value and covariance. Then, when the motion a of the robot apparatus is observed, the median value and the covariance regarding the estimated value of the state 1 can be updated by the following mathematical expressions (28-1) and (28-2).

[0175]

[Equation 32]

Here, ∇F _l and ∇F _a are as follows.

[0177]

[Expression 33]

Similarly, the state 1 of the robot apparatus at a certain point of time is estimated to have a Gaussian distribution having a certain median and covariance. And the landmark observation information s
When is observed, the median and the covariance regarding the estimated value of the state 1 can be updated by the following mathematical expressions (29-1) and (29-2).

[0179]

[Equation 34]

Here, each parameter is represented by the following formula (30-
1) to (30-4).

[0181]

[Equation 35]

Since the extended Kalman filter 343 is excellent in robustness to sensor information, the estimation result of the extended Kalman filter unit 343 is the output of the entire landmark sensor CLS340.

The EKL control section 344 controls the operation of the extended Kalman filter section 344 according to the output result of the Markov localization section 342. More specifically, the validity of the landmark observation information s is verified based on the self-position estimation result of the Markov localization unit 342. The validity of the observation information s is that the probability p (s | mlp) of observing a landmark at the grid position mlp, which is the maximum existence probability in the Markov localization unit 342, is a predetermined threshold parameter th.
It can be judged by whether or not it exceeds the resh _obs .

The probability p (s | mlp) of observing a landmark at the grid position mlp is the threshold parameter th.
When it is less than resh _obs , it is estimated that even in the Markov localization unit 342 that is robust against sensor noise, the identification solution does not sufficiently converge due to sensor noise. In such a case, even if the self-position is estimated in the extended Kalman filter unit 343, which has low robustness against sensor noise, a highly accurate solution cannot be obtained, and rather the calculation time is wasted. Therefore, when it is determined that the observation information s is not appropriate, the input of the external observation information s, that is, the observation information of the landmark, to the extended Kalman filter unit 343 is blocked using the switch 345, and the extension is performed. Kalman filter unit 34
The update of the self-position estimation value in 3 is stopped.

The EKL control unit 344 also verifies the validity of the self-position estimation result of the extended Kalman filter unit 343. The validity of the self-position estimation result can be determined by a distribution comparison test with the existence probability p (l) output from the Markov localization unit 342, using the estimated median and covariance of the state l. . An example of the distribution comparison test is the chi-square test chi-square.
-Test (ml, ekf).

By the distribution comparison test, the Markov localization unit 342 and the extended Kalman filter unit 343.
If the probability distributions of and are not similar, it may be determined that the self-position estimation value in the extended Kalman filter unit 343, which has low robustness to sensor noise, is not valid due to the influence of sensor noise. it can. In such a case, the EKL control unit 344 causes the extended Kalman filter unit 343 to be reinitialized. This is because the extended Kalman filter requires a great deal of time to restore again.

Next, the operation of the landmark sensor CLS340 will be described. FIG. 23 is a flowchart showing the operation of the landmark sensor CLS340. Figure 2
As shown in FIG. 3, when the inner-field observation information a regarding the movement of the robot apparatus 1 is input to the landmark sensor CLS340, first, in the Markov localization unit 342, the self-position estimation value is updated using the mathematical expression (24). Processing is performed (step S201). Then, in the extended Kalman filter unit 343,
-1) and (28-2) are used to update the self-position estimation value (step S202).

When the outside world observation information s regarding the observation of the landmark is input to the landmark sensor CLS340, the Markov localization unit 342 first updates the self-position estimation value using the above equation (25). It is performed (step S211).

The output result of the Markov localization unit 342 is input to the EKL control unit 344, and the validity of the observation information s is verified (step S212). The validity of the observation information s is based on the Markov localization section 3
The grid position mlp having the maximum existence probability at 42
Probability of observing landmarks at p (s | mlp)
Can exceed the predetermined threshold parameter thresh _obs .

The probability p (s | mlp) of observing a landmark at the grid position mlp is the threshold parameter th.
When it is less than resh _obs , it is estimated that even in the Markov localization unit 342 that is robust against sensor noise, the identification solution does not sufficiently converge due to sensor noise. In such a case, even if the extended Kalman filter unit 343, which has low robustness to sensor noise, estimates the self-position, an accurate solution cannot be obtained, but rather the calculation time is wasted. Therefore, when it is determined that the observation information s is not appropriate, the input of the external observation information s, that is, the observation information of the landmark, to the extended Kalman filter unit 343 is blocked using the switch 345, and the extension is performed. Kalman filter unit 34
The update of the self-position estimation value in 3 is stopped.

On the other hand, as a result of verifying the observation information s, the probability p (s | mlp) that satisfies the validity, that is, the landmark is observed at the grid position mlp is the threshold parameter t.
When it exceeds hresh _obs , the extended Kalman filter unit 343 further calculates the above mathematical expression (29-1),
The update process is performed using (29-2) (step S
213).

The result of self-position estimation by the extended Kalman filter unit 343 is input to the EKL control unit 344 and its validity is verified (step S214). The validity of the self-position estimation result by the extended Kalman filter unit 343 is determined by comparing the distribution with the existence probability p (l) output from the Markov localization unit 342, using the estimated median and covariance of the state l. Can be judged by the test. An example of a distribution comparison test is a chi-square test chi-square-test (ml, ek
f).

By the distribution comparison test, the Markov localization unit 342 and the extended Kalman filter unit 343.
When the respective probability distributions are not similar, the self-position estimation value in the extended Kalman filter unit 343, which has low robustness to sensor noise, is the sensor position.
It can be judged to be invalid due to the influence of noise. In such a case, the EKL control unit 344 causes the extended Kalman filter unit 343 to be re-initialized (step S215). This is because the extended Kalman filter requires a great deal of time to restore again.

In this way, the landmark sensor CL
In S340, it is possible to perform high-accuracy, high-speed, and robust self-localization by using both the global search, which performs a search in a relatively short search time in a wide range, and the local search, which has a high accuracy but requires a search time. .

The path plan determination unit SBL360 acquires the recognition result regarding the obstacle existing around the robot apparatus 1 from the obstacle information 331 from the obstacle grid calculation unit OG330, and the current robot is calculated from the absolute coordinate calculation unit LZ350. By acquiring the absolute coordinates of the device 1, a path that can be walked without colliding with an obstacle to a target point given in the absolute coordinate system or the robot center coordinate system of the robot device center is generated, Issue an operation command to carry out the route. That is, the action to be taken by the robot apparatus 1 is determined from the input data according to the situation, and the action sequence is output. Here, in the obstacle grid calculation unit OG330,
The processing based on the obstacle information 331 will be described later.

The points on the obstacle map generated by the obstacle information 331 from the obstacle grid calculation unit OG330 are
As shown in FIG. 24, they are classified into the following three types.

The first point is a point where an obstacle exists (point shown in black in the figure). The second point is a point (point represented by white in the figure) on the free space (space where no obstacle exists). Then, the third point is a point on the unobserved region (point indicated by the diagonal line in the figure).

Next, the route planning algorithm adopted by the route plan determining unit SBL360 is shown in the flowchart of FIG. 25, and the details will be described below.

First, the line of sight is directed toward the destination so that an obstacle map around a straight route connecting the current position to the target position is created (step S71). Then, the distance image is observed, the distance is measured, and the obstacle map is created (updated) (step S72).

Next, in the generated obstacle map, the unobserved area and the free space area are regarded as the movable area and the route is planned (step S73).

[0201] As the path planning, for example, A ^* search (A ^* search) that minimizes the cost of the entire route using the method referred to. In this A ^* search, f is used as the evaluation function, and h
It is a best-first search in which the function is admissible. We use the point of efficiency optimization for any heuristic function.

In step S73, it is checked in step S74 whether or not the route generated by applying the A ^* search is movable, and the route that can avoid the obstacle cannot be planned. In the case of (NO), there is no possibility that a movable route can be obtained even if the observation is continued any more, so that is notified and the route planning ends (step S75).

In step S73, if the movable route can be planned by applying, for example, the A ^* search (YES), the process proceeds to step S76, and whether or not an unobserved region is included in the output route. To search. This step S76
If no unobserved area is included in the route (NO)
, A movable route is output as a route plan to the destination in step S77. If the unobserved region is included in step S76 (YES), the process proceeds to step S78, the step count from the current position to the unobserved region is calculated, and it is checked whether the step count exceeds a threshold value.

If the step count exceeds the threshold value in step S78 (YES), the movable route to the unknown area is output in step S79, and the process returns to step S71. On the other hand, when the number of steps to the unknown observation region is less than the threshold value in step S78 (NO), the line-of-sight direction is controlled so that the unobserved region is observed by distance (step S80), and the obstacle is detected again. Update the physical map.

The route plan determination unit SBL360 which adopts the route planning algorithm as described above, considers the unobserved region and the free space region as a movable region, performs the route planning, and outputs the unobserved region included in the route. By re-observing only a part, it is possible to generate a moving route plan efficiently and in a short time without performing unnecessary observation and distance image calculation processing when moving to a destination.

Hereinafter, a bipedal walking type robot device equipped with the above-mentioned plane extracting device according to the embodiment of the present invention will be described in detail. This humanoid robot device is a practical robot that supports human activities in various situations in the living environment and other daily life, and can act according to internal conditions (anger, sadness, joy, enjoyment, etc.), It is an entertainment robot that can express the basic actions that humans perform.

As shown in FIG. 26, the robot apparatus 1 has
The head unit 3 is connected to a predetermined position of the trunk unit 2, and two left and right arm units 4R / L and two left and right leg units 5R / L are connected (however, provided. Each of R and L is a suffix indicating each of right and left. The same applies below.).

FIG. 27 schematically shows the joint degree-of-freedom configuration of the robot apparatus 1. The neck joint supporting the head unit 3 includes a neck joint yaw axis 101 and a neck joint pitch axis 1.
02 and the neck joint roll shaft 103 have three degrees of freedom.

Each arm unit 4R / L constituting the upper limb has a shoulder joint pitch axis 107, a shoulder joint roll axis 108, an upper arm yaw axis 109, and an elbow joint pitch axis 110.
, Forearm yaw axis 111, wrist joint pitch axis 112,
It is composed of a wrist joint roll shaft 113 and a hand portion 114. The hand portion 114 is actually a multi-joint / multi-degree-of-freedom structure including a plurality of fingers. However, since the motion of the hand portion 114 has little contribution or influence to the posture control and the walking control of the robot apparatus 1, it is assumed that the degree of freedom is zero in this specification. Therefore, each arm has seven degrees of freedom.

The trunk unit 2 has three degrees of freedom: a trunk pitch axis 104, a trunk roll axis 105, and a trunk yaw axis 106.

Further, each leg unit 5R / L constituting the lower limb has a hip joint yaw axis 115 and a hip joint pitch axis 1
16, a hip joint roll shaft 117, and a knee joint pitch shaft 11
8, ankle joint pitch axis 119, and ankle joint roll axis 1
20 and a foot 121. 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 apparatus 1. The foot 121 of the human body is actually a structure including a multi-joint, multi-degree-of-freedom foot, but the foot of the robot apparatus 1 has zero degrees of freedom. Therefore, each leg has 6 degrees of freedom.

In summary, the robot apparatus 1 as a whole has 3 + 7 × 2 + 3 + 6 × 2 = 32 degrees of freedom. However, the robot device 1 for entertainment is not necessarily limited to 32 degrees of freedom. It is needless to say that the degree of freedom, that is, the number of joints, can be appropriately increased or decreased in accordance with design / production constraints, required specifications, and the like.

Each degree of freedom of the robot apparatus 1 as described above is actually implemented by using an actuator.
It is preferable that the actuator be small and lightweight in view of demands such as eliminating extra bulges in appearance and approximating the shape of a natural human body, and performing posture control for an unstable structure such as bipedal walking. .

FIG. 28 schematically shows the control system configuration of the robot apparatus 1. As shown in the figure, the robot device 1 performs a coordinated operation between the trunk unit 2, the head unit 3, the arm unit 4R / L, the leg unit 5R / L, which represent human limbs, and the respective units. And a control unit 10 that performs adaptive control for realizing the above.

The overall operation of the robot apparatus 1 is controlled by the control unit 10. Control unit 10
Is a CPU (Central Processing Unit) or DRA
An interface (not shown) for exchanging data and commands with the main control unit 11 including main circuit components (not shown) such as M and flash ROM, and the power supply circuit and each component of the robot apparatus 1. And the peripheral circuit 12 including.

In implementing the present invention, the installation place of the control unit 10 is not particularly limited. Although it is mounted on the trunk unit 2 in FIG. 28, it may be mounted on the head unit 3. Alternatively, the control unit 10 may be provided outside the robot apparatus 1 to communicate with the body of the robot apparatus 1 in a wired or wireless manner.

The degree of freedom of each joint in the robot apparatus 1 shown in FIG. 27 is realized by an actuator corresponding to each joint. That is, the head unit 3 includes a neck joint yaw axis 101, a neck joint pitch axis 102, and a neck joint roll axis 103.
, A neck joint yaw axis actuator A ₂ , a neck joint pitch axis actuator A ₃ , and a neck joint roll axis actuator A ₄ are provided.

Further, the head unit 3 is provided with a CCD (Charge Coupled Device) camera for picking up an image of an external situation, and a distance sensor for measuring a distance to an object located in front of the head unit 3. A microphone for collecting external sound, a speaker for outputting voice, a touch sensor for detecting the pressure received by the physical action of the user such as "stroking" or "striking" are provided. ing.

Further, the trunk unit 2 has a trunk pitch axis actuator A ₅ , a trunk roll axis actuator A ₆ , which expresses the trunk pitch axis 104, trunk roll axis 105, and trunk yaw axis 106, respectively. A trunk yaw axis actuator A ₇ is provided. Also, in the trunk unit 2,
The robot apparatus 1 is provided with a battery as a power source. This battery is composed of a chargeable / dischargeable battery.

The arm unit 4R / L is subdivided into an upper arm unit 4 ₁ R / L, an elbow joint unit 4 ₂ R / L, and a forearm unit 4 ₃ R / L. 107, shoulder joint roll axis 108, upper arm yaw axis 109, elbow joint pitch axis 110, forearm yaw axis 111, wrist joint pitch axis 112, wrist joint roll axis 113, respectively, shoulder joint pitch axis actuator A ₈ , shoulder joint roll An axis actuator A ₉ , an upper arm yaw axis actuator A ₁₀ , an elbow joint pitch axis actuator A ₁₁ , an elbow joint roll axis actuator A ₁₂ , a wrist joint pitch axis actuator A ₁₃ , and a wrist joint roll axis actuator A ₁₄ are provided.

Also, the leg unit 5R / L is used for the thigh unit.
Knit 5₁R / L and knee unit 5 _TwoR / L and shin
Knit 5_ThreeAlthough it is subdivided into R / L, the hip joint yaw axis 11
5, hip pitch axis 116, hip roll axis 117, knee
Joint pitch axis 118, ankle joint pitch axis 119, ankle function
Hip joint yaw axis actuation expressing each of the knot roll axes 120
Player A₁₆, Hip joint pitch axis actuator
A₁₇, Hip joint roll axis actuator A₁₈,Knee joint
Pitch axis actuator A₁₉, Ankle joint pitch axis
Cheetah A₂₀, Ankle joint roll axis actuator A
₂₁Has been deployed. Actuator used for each joint
Data A_Two, A_ThreeIs more preferably a direct gear type
Motor unit with integrated servo control system in one chip
Small type AC servo actuator mounted inside
Can be composed of

For each mechanical unit such as the trunk unit 2, head unit 3, each arm unit 4R / L, each leg unit 5R / L, etc., the sub-control units 20, 21, 22R / of the actuator drive control unit are provided. L, 23R / L are deployed. Furthermore, a ground contact confirmation sensor 30R / L that detects whether or not the sole of each leg unit 5R / L has landed is attached, and a posture sensor 31 that measures a posture is provided in the trunk unit 2. is doing.

The ground contact confirmation sensor 30R / L is composed of, for example, a proximity sensor or a micro switch installed on the sole of the foot. Further, the posture sensor 31 is composed of, for example, a combination of an acceleration sensor and a gyro sensor.

By the output of the ground contact confirmation sensor 30R / L, it is possible to determine whether each of the left and right legs is currently standing or swinging during an operation such as walking or running. Further, the output of the posture sensor 31 can detect the inclination and posture of the trunk.

The main control section 11 uses the sensors 30R / L, 3
The control target can be dynamically corrected in response to the output of 1. More specifically, the sub control units 20, 21,
By performing adaptive control on each of 22R / L and 23R / L, it is possible to realize a whole-body movement pattern in which the upper limbs, the trunk, and the lower limbs of the robot apparatus 1 are cooperatively driven.

The whole body motion of the robot apparatus 1 on the body is
Foot control, ZMP (Zero Moment Point) trajectory, trunk movement, upper limb movement, waist height, etc. are set, and commands for instructing movements according to these setting contents are issued to the sub-control units 20, 21, 22R. / L, 23R / L. The sub-control units 20, 21, ... Interpret the received commands from the main control unit 11 and output drive control signals to the actuators A ₂ , A ₃ ,. . The “ZMP” mentioned here is a point on the floor surface where the moment due to the floor reaction force during walking becomes zero, and the “ZMP trajectory” is, for example, the robot device 1
Means a locus of movement of the ZMP during the walking motion period. Regarding the concept of ZMP and the application of ZMP to the stability criterion of walking robots, see Miomir Vukobratovi.
c "LEGGED LOCOMOTION ROBOTS" (Ichiro Kato, "Walking Robot and Artificial Feet" (Nikkan Kogyo Shimbun)).

As described above, in the robot apparatus 1, each of the sub-control units 20, 21, ... Interprets the command received from the main control unit 11 and each actuator A ₂ , A ₃
A drive control signal is output to ... to control the drive of each unit. As a result, the robot apparatus 1 makes a stable transition to the target posture and can walk in a stable posture.

Further, in the control unit 10 of the robot apparatus 1, in addition to the posture control as described above, various sensors such as an acceleration sensor, a touch sensor, a ground contact confirmation sensor, image information from a CCD camera, and sound from a microphone. Information is handled in a centralized manner. In the control unit 10, although not shown, various sensors such as an acceleration sensor, a gyro sensor, a touch sensor, a distance sensor, a microphone, a speaker, actuators, CCD cameras, and batteries are connected to the main control unit 11 via corresponding hubs. Has been done.

The main controller 11 sequentially takes in sensor data, image data, and audio data supplied from the above-mentioned sensors, and sequentially stores them in a predetermined position in the DRAM through the internal interface. Further, the main control unit 11 sequentially takes in the battery remaining amount data representing the battery remaining amount supplied from the battery, and stores this in the DRAM.
It is stored in a predetermined position inside. Each sensor data, image data, voice data, and battery remaining amount data stored in the DRAM are used when the main control unit 11 controls the operation of the robot apparatus 1.

The main controller 11 reads out the control program and stores it in the DRAM at the initial stage when the power of the robot apparatus 1 is turned on. In addition, the main control unit 11 uses the sensor data, the image data, the audio data, and the battery remaining amount data that are sequentially stored in the DRAM from the main control unit 11 as described above, based on the self and surrounding conditions and the user. Judging whether or not there are instructions and how to work.

Further, the main control section 11 determines an action according to its own situation based on this determination result and the control program stored in the DRAM, and drives a necessary actuator based on the determination result. The robot apparatus 1 is caused to take actions such as so-called “gesture” and “hand gesture”.

In this way, the robot apparatus 1 can judge its own and surroundings based on the control program and can act autonomously in accordance with instructions and actions from the user.

By the way, the robot apparatus 1 can act autonomously according to the internal state. Therefore, a software configuration example of the control program in the robot apparatus 1 will be described with reference to FIGS. 29 to 34.

In FIG. 29, the device driver layer 40 is located at the lowest layer of the control program and is composed of a device driver set 41 consisting of a plurality of device drivers. In this case, each device
The driver is an object that is allowed to directly access hardware used in a normal computer such as a CCD camera and a timer, and receives an interrupt from the corresponding hardware to perform processing.

The robotic server object 42 is located in the lowest layer of the device driver layer 40, and is, for example, the above-mentioned various sensors and actuators 2.
The virtual robot 43, which is a software group that provides an interface for accessing hardware such as 8 _{1 to} 28 _n , the power manager 44 that is a software group that manages power supply switching, and other various devices. A device driver manager 45 that is a software group that manages a driver and a designed driver that is a software group that manages the mechanism of the robot apparatus 1.
It is composed of a robot 46.

The manager object 47 includes an object manager 48 and a service manager 49.
It consists of Object manager 48
Is a software group that manages activation and termination of each software group included in the robotic server object 42, the middleware layer 50, and the application layer 51, and the service manager 49.
Is a software group that manages the connection of each object based on the connection information between each object described in the connection file stored in the memory card.

The middle wear layer 50 is located in the upper layer of the robotic server object 42 and is composed of a software group that provides basic functions of the robot apparatus 1 such as image processing and voice processing. There is. Further, the application layer 51 is located above the middleware layer 50, and the middleware layer 50
It is composed of a software group for determining the action of the robot apparatus 1 based on the processing result processed by each software group forming the layer 50.

FIG. 30 shows the specific software configurations of the middleware layer 50 and the application layer 51, respectively.

The middleware layer 50, as shown in FIG. 30, is for noise detection, temperature detection, brightness detection,
Each signal processing module 6 for scale recognition, distance detection, posture detection, touch sensor, motion detection, and color recognition
A recognition system 70 having 0 to 68 and an input semantics converter module 69, an output semantics converter module 78, and posture management, tracking, motion reproduction, walking, fall recovery, LE
Signal processing modules 71 to 7 for D lighting and sound reproduction
And an output system 79 having 7 or the like.

[0240] Each signal processing module 60 of the recognition system 70-
68 captures corresponding data of each sensor data, image data, and audio data read from the DRAM by the virtual robot 43 of the robotic server object 42, performs a predetermined process based on the data, The processing result is given to the input semantics converter module 69. Here, for example, the virtual robot 43 is configured as a portion that exchanges or converts a signal according to a predetermined communication protocol.

The input semantics converter module 69 detects "noisy", "hot", "bright", "ball detected", "fall" based on the processing results given from the respective signal processing modules 60 to 68. The user and surroundings, such as "Yes", "Stabbed", "Struck", "I heard Domiso scale", "A moving object was detected", or "An obstacle was detected", and the user. It recognizes the command and the action from, and outputs the recognition result to the application layer 41.

The application layer 51 is shown in FIG.
As shown in FIG. 5, the action model library 80, the action switching module 81, the learning module 82, the emotion model 83, and the instinct model 84 are composed of five modules.

In the behavior model library 80, as shown in FIG. 32, "when the battery level is low",
Independently corresponding to some preselected condition items such as "returning from a fall", "avoiding obstacles", "expressing emotions", "detecting a ball", etc. A behavior model is provided.

Each of these behavior models will be described later as necessary when a recognition result is given from the input semantics converter module 69, or when a fixed time has passed since the last recognition result was given. As described above, each subsequent action is determined with reference to the corresponding emotional parameter value held in the emotion model 83 and the corresponding desire parameter value held in the instinct model 84, and the decision result is determined by the action switching module 81. Output to.

[0245] In the case of this embodiment, each behavior model, as a method to determine the next action, one node _(state) as shown in FIG. 33 NODE 0 ~NODE _n which the other nodes NODE ₀ ˜NODE _n , a finite probability that determines probabilistically based on the transition probabilities P _{1 to} P _n respectively set for the arcs ARC ₁ to ARC _n1 connecting the nodes NODE _{0 to} NODE _n. An algorithm called an automaton is used.

Specifically, each behavior model has its own
NODE that forms the behavior model of the child₀~ NODE_n
To correspond to each of these nodes NODE₀~ NO
DE _nEach has a state transition table 90 as shown in FIG.
There is.

In this state transition table 90, the node NO.
Input events (recognition results) that are transition conditions in DE _{0 to} NODE _n are listed in order of priority in the column of “input event name”, and further conditions regarding the transition conditions are listed in the columns of “data name” and “data range”. It is described in the corresponding line.

Therefore, in the node NODE ₁₀₀ shown in the state transition table 90 of FIG. 34, "ball is detected (BAL
L) ”given the recognition result, the“ SIZE ”of the ball given with the recognition result.
Is in the range of 0 to 1000, and if the recognition result of "obstacle detection (OBSTACLE)" is given, the "distance (DISTANCE)" to the obstacle given with the recognition result is The condition for transitioning to another node is that it is in the range of "0 to 100".

Further, in this node NODE ₁₀₀ , even when there is no recognition result input, the parameter values of the emotions and desires held in the emotion model 83 and the instinct model 84 which the behavior model periodically refers to are stored. Among them, "Joy" held by emotion model 83,
When the parameter value of either "Surprise" or "Sadness" is in the range of "50 to 100", it is possible to transit to another node.

Further, in the state transition table 90, the node names that can transit from the nodes NODE _{0 to} NODE _n are listed in the row of “transition destination node” in the column of “probability of transition to another node”, and “ Input event name ",
Each other node NOD that can transit when all the conditions described in the columns of "data name" and "data range" are met
The transition probabilities from E _{0 to} NODE _n are respectively described in the corresponding locations in the “transition probabilities to other nodes” section, and the action to be output when transitioning to the nodes NODE _{0 to} NODE _n is “other It is described in the row of “output action” in the column of “transition probability to node”. In addition, the sum of the probabilities of each row in the column of "probability of transition to other node" is 100.
It is [%].

Therefore, in the node NODE ₁₀₀ represented by the state transition table 90 in FIG. 34, for example, "ball is detected (BALL)" and the "SIZE" of the ball is in the range of "0 to 1000". If the recognition result that is, is given, there is a probability of "30 [%]"
ODE ₁₂₀ (node 120) ", at that time" A
The action of “CATION 1” will be output.

Each behavior model has nodes NODE ₀ to NO described as such a state transition table 90.
DE _n are configured to be connected to each other, and when a recognition result is given from the input semantics converter module 69, the corresponding nodes NODE ₀ to
The next action is stochastically determined using the state transition table of NODE _n , and the determination result is output to the action switching module 81.

The action switching module 81 shown in FIG.
Among the actions output from the action models of the action model library 80, the action output from the action model having a predetermined high priority is selected, and a command to execute the action (hereinafter, action command That.)
To the output semantics converter module 78 of the middleware layer 50. Note that in this embodiment, the lower the action model shown in FIG. 32, the higher the priority is set.

Further, the action switching module 81 notifies the learning module 82, the emotion model 83, and the instinct model 84 that the action is completed based on the action completion information given from the output semantics converter module 78 after the action is completed. .

On the other hand, the learning module 82 inputs the recognition result of the teaching received as an action from the user such as “struck” or “stroked” among the recognition results given from the input semantics converter module 69. To do.

Then, based on the recognition result and the notification from the action switching module 71, the learning module 82 lowers the probability of the action being expressed when "struck (scored)" and "stroked ( Praised) ”, the corresponding transition probability of the corresponding behavior model in the behavior model library 70 is changed so as to increase the occurrence probability of that behavior.

On the other hand, the emotion model 83 is "joy (Jo
y) ”,“ Sadness ”,“ Anger ”,
With respect to a total of 6 emotions of “Surprise”, “Disgust”, and “Fear”, a parameter indicating the strength of the emotion is held for each emotion. Then, the emotion model 83 gives specific recognition results such as “struck” and “stabbed” given from the input semantics converter module 69 to the parameter values of these emotions, the elapsed time and the action switching module 81. It is updated periodically based on notifications from etc.

Specifically, the emotion model 83 is determined based on the recognition result given from the input semantics converter module 69, the action of the robot apparatus 1 at that time, the elapsed time from the last update, and the like. The amount of change in emotion at that time calculated by the arithmetic expression is ΔE
[T], E [t] of the current parameter value of the emotion, the coefficient representing the sensitivity of the emotion as k _e, the following equation (3
The parameter value E [t + 1] of the emotion in the next cycle is calculated by 1), and the parameter value of the emotion is updated by replacing this with the current parameter value E [t] of the emotion. Further, the emotion model 83 updates the parameter values of all emotions in the same manner as above.

[0259]

[Equation 36]

It is to be noted that it is predetermined how much each recognition result or the notification from the output semantics converter module 78 affects the variation amount ΔE [t] of the parameter value of each emotion. The recognition result such as “ta” is the variation amount ΔE of the parameter value of the emotion of “anger”
[T] has a great influence, and the recognition result such as “struck” is the variation amount ΔE of the parameter value of the emotion of “joy”.
It has a great influence on [t].

Here, the notification from the output semantics converter module 78 is so-called action feedback information (action completion information) and information on the appearance result of the action, and the emotion model 83 is also based on such information. Change emotions. This is, for example, that the behavior level of anger is lowered by the action of "screaming". The notification from the output semantics converter module 78 is sent to the learning module 8 described above.
2 is also input, and the learning module 82 changes the corresponding transition probability of the behavior model based on the notification.

The feedback of the action result may be performed by the output of the action switching module 81 (action added with emotion).

On the other hand, the instinct model 84 is "exercise desire (exer
cise), “affection”, “appetite”
e) ”and“ curiosity ”independent of each other 4
For each desire, a parameter indicating the strength of the desire is held for each of these desires. And the instinct model 84
Updates the parameter values of these desires periodically based on the recognition result provided from the input semantics converter module 69, the elapsed time, the notification from the action switching module 81, and the like.

Specifically, the instinct model 84 determines a predetermined "movement desire", "love desire", and "curiosity" based on the recognition result, the elapsed time, the notification from the output semantics converter module 78, and the like. The fluctuation amount of the desire at that time calculated by the arithmetic expression is ΔI [k],
Let I [k] be the current parameter value of the desire, and let the coefficient k _i representing the sensitivity of the desire be the following formula (3) at a predetermined cycle.
2) is used to calculate the parameter value I [k + 1] of the desire in the next cycle, and the calculation result is replaced with the current parameter value I [k] of the desire to update the parameter value of the desire. . Also, the instinct model 84
Updates the parameter values of each desire except "appetite" in the same manner.

[0265]

[Equation 37]

The degree of influence of the recognition result and the notification from the output semantics converter module 78 on the variation amount ΔI [k] of the parameter value of each desire is predetermined, and for example, the output semantics converter is used. The notification from the module 78 has a great influence on the fluctuation amount ΔI [k] of the “tiredness” parameter value.

In this embodiment, each affect
And each desire (instinct) parameter value is 0 to 10
It is regulated to fluctuate within the range of 0, and
A few k _e, K_iThe value of is also set individually for each emotion and each desire.
Has been.

On the other hand, the output semantics converter module 78 of the middleware layer 50, as shown in FIG. 30, is "forward" and "happy" given from the action switching module 81 of the application layer 51 as described above. , An abstract action command such as “squeal” or “tracking (chasing the ball)” is given to the corresponding signal processing modules 71 to 77 of the output system 79.

Then, these signal processing modules 71 to 7
When an action command is given, 7 generates a servo command value to be given to a corresponding actuator to take the action, sound data of sound output from a speaker, and / or drive data given to the LED, based on the action command. Then, these data are sequentially transmitted to the corresponding actuator or speaker or LED via the virtual robot 43 of the robotic server object 42 and the signal processing circuit.

In this way, the robot apparatus 1 can perform an autonomous action according to its own (internal) and surrounding (external) conditions, and instructions and actions from the user, based on the above-mentioned control program.

Such a control program is provided via a recording medium recorded in a format readable by the robot apparatus. As a recording medium for recording the control program,
Recording medium of magnetic reading system (for example, magnetic tape, flexible disk, magnetic card), recording medium of optical reading system (for example, CD-ROM, MO, CD-R, DVD)
Etc. are possible. The recording medium also includes a storage medium such as a semiconductor memory (so-called memory card (rectangular type, square type, or any shape), IC card) or the like. Also,
The control program may be provided via the so-called Internet or the like.

These control programs are reproduced via a dedicated read driver device, a personal computer or the like, and transmitted to the robot device 1 by a wired or wireless connection to be read. Further, when the robot device 1 includes a drive device for a miniaturized storage medium such as a semiconductor memory or an IC card, the control program can be directly read from the storage medium.

In this embodiment, the CCD cameras 200R and 200L of the head unit and the image processing circuit 21 are used.
Since the correct correction distance data obtained by using the reliability image from the stereo distance measuring device consisting of 0 is input to the information processing means, the robot device recognizes the position and the object using the stereo distance measuring data. The recognition accuracy of the latter stage of the robot can be improved.

Further, the above parameters include the parameters of the CCD camera and the parameters of the robot apparatus, and by controlling the camera parameters and the robot parameters based on the reliability of the reliability image, the imaging condition and the imaging of the CCD image are obtained. In addition to controlling camera parameters such as position, the robot head unit, left and right two arm units 4R / L and left and right two leg units 5R / L
By changing various robot parameters that operate etc., it is possible to obtain a more reliable range image,
The performance of the robot device is improved.

[0275]

As described above in detail, in the plane extraction method according to the present invention, three-dimensional data of three or more points are sampled from a three-dimensional data group, and a plane parameter indicating one plane determined by the three-dimensional data is sampled. Since there is a plane calculating step for calculating a plurality of plane parameters and a plane determining step for voting a plurality of plane parameters obtained from the plane calculating step in a voting space and determining a plane based on the voting result, You can vote directly for
It is robust against noise and highly accurate, and therefore the number of samplings can be reduced to speed up the processing.

The robot device according to the present invention is an autonomous robot device which operates based on the supplied input information. The robot device samples three-dimensional data of three or more points from a three-dimensional data group, and Plane calculating means for calculating a plurality of plane parameters indicating one plane determined by data, and plane determining means for voting a plurality of plane parameters obtained from the plane calculating step in a voting space and determining a plane based on the voting result. With the above, it is possible to extract a plane with high accuracy, and it is possible to recognize the environment of the robot device such as an obstacle from the detection result of the plane.

[Brief description of drawings]

FIG. 1 is a block diagram showing an outline of a robot apparatus according to an embodiment of the present invention.

FIG. 2 is a schematic diagram showing a software configuration of the robot apparatus according to the embodiment of the present invention.

FIG. 3 is a flowchart showing a plane extraction method according to the embodiment of the present invention.

FIG. 4 is a flowchart illustrating a process of step S1 shown in FIG.

5 is a flowchart illustrating steps S2 and S3 shown in FIG. 3. FIG.

FIG. 6 is a schematic diagram showing plane parameters in the embodiment of the present invention.

FIG. 7 is a schematic diagram showing a three-dimensional parameter voting space.

FIG. 8 is a schematic diagram showing a three-dimensional parameter voting space according to the embodiment of the present invention.

9 is a flowchart illustrating a process of step S4 shown in FIG.

10 is a flow chart showing a process of step S4 shown in FIG.

11 is a flowchart showing steps of steps S42 and S43 shown in FIG.

FIG. 12 is a flowchart showing an operation of software of the robot apparatus according to the embodiment of the present invention.

FIG. 13 is a schematic diagram showing the flow of data input to the software.

[Fig. 14] Fig. 14 is a diagram for describing generation of a parallax image input to the plane extraction unit PLEX.

FIG. 15 is a flowchart showing a processing procedure in which the plane extraction unit PLEX recognizes an obstacle.

FIG. 16 is a diagram showing plane parameters detected by a plane detector PLEX.

FIG. 17 is a diagram for explaining a conversion process from a camera coordinate system to a plantar ground contact plane coordinate system.

FIG. 18 is a diagram showing points on a plane extracted by a plane extraction unit PLEX.

FIG. 19 is a diagram showing that a floor surface is extracted from the robot view, coordinates are further converted, and an obstacle is expressed in a two-dimensional (floor plane) square.

FIG. 20 is a schematic diagram showing color landmarks in a landmark sensor CLS.

FIG. 21 is a block diagram schematically showing the configuration of a landmark sensor CLS.

FIG. 22 is a schematic diagram showing a self-position probability density distribution on each grid, which is obtained by the Markov localization unit.

FIG. 23 is a flowchart showing the operation of the landmark sensor CLS340.

FIG. 24 is a schematic diagram showing a route from a current position to a destination position on an obstacle map generated by obstacle information.

FIG. 25 is a flowchart showing a route planning algorithm.

FIG. 26 is a perspective view showing an external configuration of a robot device according to an embodiment of the present invention.

FIG. 27 is a diagram schematically showing a degree-of-freedom configuration model of the robot apparatus.

FIG. 28 is a block diagram showing a circuit configuration of the robot apparatus.

FIG. 29 is a block diagram showing a software configuration of the robot apparatus.

FIG. 30 is a block diagram showing a configuration of a middle wear layer in the software configuration of the robot apparatus.

FIG. 31 is a block diagram showing a configuration of an application layer in the software configuration of the robot apparatus.

FIG. 32 is a block diagram showing the configuration of an application layer behavior model library.

FIG. 33 is a diagram illustrating a finite probability automaton that is information for determining the action of the robot apparatus.

FIG. 34 is a diagram showing a state transition table prepared for each node of the finite probability automaton.

[Explanation of symbols]

1 robot device, 250 head unit, 200R,
200L CCD camera, 201R right eye image, 201
L left eye image, 202 color image, 203 parallax image,
210 stereo image processing device, 220 CPU, 23
0 actuator, 231 control signal, 240 sensor, 241 sensor data, 260 trunk, 300 software, 310 kinematic odometry KIN
E310, 320 Plane extraction unit PLEX, 330 Obstacle grid calculation unit OG, 340 Landmark position detection unit CLS, 350 Absolute coordinate calculation unit LZ, 360 Action determination unit SBL, 400, 500, 510, 600, 61
0,700

   ─────────────────────────────────────────────────── ─── Continued front page    (72) Inventor Takeshi Ohashi             6-735 Kita-Shinagawa, Shinagawa-ku, Tokyo Soni             -Inside the corporation (72) Inventor Masaki Fukuchi             6-735 Kita-Shinagawa, Shinagawa-ku, Tokyo Soni             -Inside the corporation (72) Inventor Atsushi Okubo             6-735 Kita-Shinagawa, Shinagawa-ku, Tokyo Soni             -Inside the corporation F-term (reference) 3C007 AS36 CS08 KS12 KT03 KT11                       KT15 MT04 MT08 WA03 WA13                       WB21                 5B057 AA05 BA02 CA01 CA13 CA16                       DA08 DB03 DC02 DC13                 5L096 AA02 AA09 BA05 CA05 FA24

Claims (28)

[Claims] 1. A plane calculation step of sampling three-dimensional data of three or more points from a three-dimensional data group and calculating a plurality of plane parameters indicating one plane determined by the three-dimensional data; and a plane calculation step obtained from this plane calculation step. And a plane determining step of determining a plane based on the voting result of a plurality of plane parameters in a voting space. 2. The plane calculation step is a step of calculating the plane parameter (θ, φ, d) when the direction of the normal vector of the plane is (θ, φ) and the distance from the origin is d. And the voting space is defined by the following formula (1-1) or formula (1-
2. The plane extraction method according to claim 1, which is represented by 2). [Equation 1] 3. The three-dimensional data group is converted into a three-dimensional data group based on an input image, and each of the three-dimensional data groups
The dimension parameter individually has a reliability parameter calculated based on the input image, and has a data selection step of selecting three-dimensional data of the three-dimensional data group based on the reliability parameter. Item 2. The plane extraction method according to item 1. 4. The sampling data is data obtained by randomly sampling three points, and the random sampling is repeated a plurality of times.
The plane extraction method described. 5. The sampling data comprises one randomly sampled reference point and two other randomly sampled points within a predetermined distance from the reference point. The plane extraction method described. 6. The three-dimensional space having the three-dimensional data group is divided into predetermined regions, and the sampling data is three-dimensional data included in the divided regions. Plane extraction method. 7. The plane extracting method according to claim 1, wherein the plane calculating step includes a step of calculating the plane parameters from the three-dimensional data of three points by a direct solution method. 8. The plane calculation step comprises three or more points,
2. The plane extraction method according to claim 1, further comprising a step of calculating the plane parameter by a principal component analysis of expanding a covariance matrix from dimensional data to extract a vector corresponding to a minimum eigenvalue. 9. The plane calculation step comprises three or more points of three or more.
2. The plane extraction method according to claim 1, further comprising the step of calculating the plane parameter using the least squares method by assuming that the plane is an arbitrary plane that does not pass through the origin from the dimensional data. 10. The plane determining step repeats the voting, and when the total number of votes exceeds a predetermined threshold value, or the sum of voting values at a position where the voting value determined based on the number of votes is the highest. 2. The plane extraction method according to claim 1, further comprising the step of stopping voting when a predetermined percentage of the total voting value, which is the sum of all voting values, is reached. 11. The plane extracting step according to claim 1, wherein the plane determining step includes a weighting voting step of voting by setting different weights according to the reliability of the three-dimensional data and / or the plane calculating method. Method. 12. The plane extracting method according to claim 1, wherein the plane determining step includes a weighted averaging step of performing a weighted average according to the number of votes in the vicinity of a peak of votes in the voting space. 13. A mapping step of obtaining a coordinate transformation matrix in which three-dimensional data included in the plane extracted by the plane decision step becomes two-dimensional, and mapping the three-dimensional data by the coordinate transformation matrix to output. The plane extraction method according to claim 1. 14. The plane extracting method according to claim 1, wherein the plane determining step includes an optimum parameter calculating step of calculating an optimum parameter from the initial parameter by iteration using a plane parameter estimated by the voting result as an initial parameter. . 15. The plane determining step includes selecting one or more selected from the group consisting of a sharpness of a voting peak in the voting, the number of data points used in the iteration, and an error residual calculated by the iteration. The plane extraction method according to claim 14, further comprising a reliability calculation step of calculating a reliability of a plane which is used as a reliability parameter and is determined. 16. The iteration is performed in the parameter calculating step by using three-dimensional data having a higher resolution than the three-dimensional data in the plane calculating step of estimating the initial parameter. Plane extraction method. 17. A plane calculation means for sampling three-dimensional data of three or more points from a three-dimensional data group and calculating a plurality of plane parameters indicating one plane determined by the three-dimensional data, and a plane calculation step obtained from this plane calculation step. And a plane determining means for determining a plane based on the voting result of a plurality of plane parameters voted in a voting space. 18. The plane calculation means calculates the plane parameter (θ, φ, d) when the direction of the normal vector of the plane is (θ, φ) and the distance from the origin is d. And the voting space is defined by the following formula (1-1) or formula (1-
The method according to claim 17, which is represented by 2).
The plane extraction device described. [Equation 2] 19. The three-dimensional data group is converted into a three-dimensional data group based on an input image, and each of the three-dimensional parameters individually has a reliability parameter calculated based on the input image. The plane extraction device according to claim 17, further comprising a data selection unit that selects three-dimensional data of the three-dimensional data group based on the reliability parameter. 20. The first plane calculation means samples first three points from the three-dimensional data and calculates the second plane parameter by a direct solution method; and the third step calculates three or more points from the three-dimensional data to obtain a covariance. Second parameter means for calculating the plane parameter by principal component analysis for expanding the matrix to extract the vector corresponding to the smallest eigenvalue, and sampling three or more points from the three-dimensional data so that the plane does not pass the origin. 18. The method according to claim 17, further comprising one or more calculation means selected from the group consisting of third parameter calculation means for calculating the plane parameters by using the least squares method assuming that the plane is an arbitrary plane. Plane extraction device. 21. The plane determining means repeats the votes, and when the total number of votes exceeds a predetermined threshold value, or the sum of the votes at the position with the highest votes determined based on the votes. 18. The plane extracting apparatus according to claim 17, further comprising a voting stopping unit that stops voting when a predetermined percentage of total voting values, which is the sum of all voting values, is reached. 22. The plane extraction according to claim 17, wherein the plane determining means has a weighted voting step of voting by setting different weights according to the reliability of the three-dimensional data and / or the plane calculation method. apparatus. 23. The plane deciding means comprises a weighted averaging means for performing a weighted averaging according to the number of votes in the vicinity of a voting peak in the voting space.
The plane extraction device described. 24. The plane extracting device according to claim 17, wherein the plane determining means has an optimum parameter calculating means for calculating an optimum parameter from the initial parameter by iteration using the plane parameter estimated by the voting result as an initial parameter. . 25. The plane determining means selects one or more selected from the group consisting of a sharpness of a voting peak in the voting, the number of data points used in the iteration, and an error residual calculated by the iteration. 25. The plane extraction apparatus according to claim 24, further comprising a reliability calculation means for calculating a reliability of a plane which is used as a reliability parameter and which is determined. 26. An autonomous robot apparatus that operates based on supplied input information, wherein three-dimensional data of three or more points are sampled from a three-dimensional data group, and one plane determined by the three-dimensional data. And a plane determining means for voting a plurality of plane parameters obtained from the plane calculating step in a voting space and determining a plane based on the voting result. Robot device. 27. A program for causing a computer to execute a predetermined operation, wherein three-dimensional data of three or more points are sampled from a three-dimensional data group, and a plane parameter indicating one plane determined by the three-dimensional data is set. A program comprising: a plane calculating step of calculating a plurality of planes; and a plane determining step of voting a plurality of plane parameters obtained from the plane calculating step in a voting space and determining a plane based on the voting result. 28. A computer-readable recording medium recording a program for causing a computer to execute a predetermined operation, wherein three-dimensional data of three or more points is sampled from a three-dimensional data group, and the three-dimensional data is used. A plane calculation step of calculating a plurality of plane parameters indicating one determined plane, and a plane determination step of voting a plurality of plane parameters obtained from the plane calculation step in a voting space and determining a plane based on the voting result. A recording medium having a program recorded therein.