JP2007219645A - Data processing method, data processor, and program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a data processing method for automatically specifying a bias value for the moving amount of a traveling object with a processing load which is smaller than a conventional manner. <P>SOLUTION: A CPU 19 continuously stores a plurality of map data Mi assuming a plurality of different bias values bias_i, and when imaging data IM(S10L, S10R) are input, the plurality of stored map data Mi are each updated based on the imaging data. Then, the result is evaluated, so that the optimal bias value bias_i can be specified. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to a data processing method, a data processing apparatus, and a program for specifying a bias value with respect to a moving amount of a moving body.

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

Such robots generate and update map data based on image data obtained by a camera or the like in addition to map data designated in advance.
By the way, in such a robot, for example, if there is a slight difference in the lengths of a plurality of legs, even if the actuator is controlled by a CPU control signal, the actual movement amount of the robot is assumed to be between the assumed movement amount. There is a difference.
In such a case, there is a system that automatically specifies a bias value of the amount of movement of the robot and controls the movement of the robot based on the specified bias value.
D. Haehnel, W. Burgard, D. Fox, S. Thrun. “An efficient FastSLAM algorithm for Generating Maps of Large-Scale Cyclic Environments from Raw Laser Range Measurements”, Int. Conf. On Intelligent Robots and Systems (IROS), 2004 _ Particle filter for map-building

  However, the above-described conventional system has a problem that the amount of calculation is very large, the system becomes large-scale, and real-time control is difficult.

  The present invention provides a data processing method, data processing apparatus, and program capable of automatically specifying a bias value with respect to the amount of movement of a moving object with a smaller processing load than conventional methods in order to solve the above-described problems of the prior art. The purpose is to do.

  In order to solve the above-described problems of the prior art and achieve the above-described object, the data processing method according to the first aspect of the present invention provides a plurality of different bias values defined in advance with respect to the amount of movement of the moving object. A first step of generating position data indicating the position of the moving body based on the bias value and the measured value of the amount of movement, and a plurality of maps associated with each of the plurality of bias values A second step of updating the data on the assumption that imaging data is obtained at the position indicated by the position data generated in the first step with respect to the bias value corresponding to the map data; And a third step of performing a predetermined evaluation process based on the plurality of map data updated in the step and identifying one optimum bias value among the plurality of bias values.

  According to a second aspect of the present invention, there is provided the data processing device, wherein for each of a plurality of different bias values defined in advance with respect to the amount of movement of the moving body, the movement Position generating means for generating position data indicating the position of the body, and updating of the plurality of map data associated with each of the plurality of bias values by the position generating means for the bias value corresponding to the map data. A plurality of map updating means that performs imaging assuming that imaging data has been obtained at the position indicated by the generated position data, and a predetermined evaluation process based on the plurality of map data updated by the map updating means. Bias value specifying means for specifying an optimum one of the bias values.

  A program according to a third aspect of the invention is a program executed by a computer, and for each of a plurality of different bias values defined in advance with respect to the amount of movement of a moving object, the bias value and the measured value of the amount of movement. And updating the plurality of map data associated with each of the plurality of bias values based on the first procedure for generating position data indicating the position of the mobile object, and corresponding to the map data Based on the second procedure performed on the assumption that imaging data was obtained at the position indicated by the position data generated in the first procedure with respect to the bias value, and the plurality of map data updated in the second procedure A predetermined evaluation process is performed to cause the computer to execute a third procedure for specifying an optimum one of the plurality of bias values.

  According to the present invention, it is possible to provide a data processing method, a data processing apparatus, and a program that can automatically specify a bias value for the amount of movement of a moving object with a smaller processing load than in the past.

Hereinafter, a robot according to an embodiment of the present invention will be described.
<First Embodiment>
First, the correspondence between the components of the present embodiment and the components of the present invention will be described.
In the present embodiment, the robot 1 is an example of the moving body of the present invention. The bias value bias_i is an example of a plurality of bias values of the present invention, and the map data Mi is an example of map data of the present invention. Moreover, the 1st-3rd evaluation process of this embodiment is an example of the evaluation process of this invention.
Further, the program PRG shown in FIG. 1 is an example of the program of the present invention.

FIG. 1 is a functional block diagram of a robot 1 according to an embodiment of the present invention.
As shown in FIG. 1, the robot 1 includes, for example, CCD (Charge-Coupled Devices) cameras 10R and 10L, an actuator group 16, a sensor group 17, a memory 18, and a CPU 19.
[CCD camera 10R, 10L]
The CCD cameras 10R and 10L output the right image data S10R and the left image data S10L corresponding to the imaging results to the CPU 19, respectively.

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

[Sensor group 17]
The sensor group 17 outputs a detection signal S17 to the CPU 19.
The sensor group 17 includes an encoder provided in the actuator of the actuator group 16.

[Memory 18]
The memory 18 stores a program PRG executed by the CPU 19 and data such as map data Mi.

[CPU 19]
The CPU 19 executes the following processing based on the program PRG read from the memory 18.
The CPU 19 calculates parallax information (disparity data) (distance information) between the right image data S10R and the left image data S10L input from the CCD cameras 10R and 10L, and performs color image data (YUV: luminance Y, UV color difference) and a parallax image. Data (YDR: luminance Y, parallax D, reliability R) is calculated alternately for each frame.
Here, the parallax indicates a difference between points where a certain point in the space is mapped to the left camera and the right camera, and changes according to the distance from the camera.
As will be described later, the CPU 19 specifies a bias value of the movement amount of the robot 1 using the above-described image data, and controls the actuator group 16 and the like using the bias value.

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

  As shown in FIG. 2, the robot 1 includes a head unit 203 connected to a predetermined position of the trunk unit 202, two left and right arm units 204R / L, and two left and right leg units 205R / L. Are connected.

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

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

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

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

Such a robot includes a control system that controls the operation of the entire robot, for example, in the trunk unit 202.
FIG. 4 is a schematic diagram illustrating a control system configuration of the robot 1 according to the present embodiment.
As shown in FIG. 4, the control system dynamically controls the whole body cooperative motion of the robot 1 such as driving the actuator 350 and the thought control module 200 that controls emotion judgment and emotion expression in response to user input and the like. And a control module 300.

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

The thought control module 200 determines the current emotion and intention of the robot 1 according to stimuli from the outside such as image data input from the image input device 251 and sound data input from the sound input device 252. That is, as described above, by recognizing the user's facial expression from the input image data and reflecting the information on the emotion and intention of the robot 201, it is possible to express an action according to the user's facial expression.
Here, the image input device 251 includes a plurality of CCD (Charge Coupled Device) cameras, for example, and can obtain a distance image from images captured by these cameras.
In the present embodiment, the image input device 251 corresponds to the CCD cameras 10R and 10L shown in FIG.
The voice input device 252 includes a plurality of microphones, for example.

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

The motion control module 300 includes a CPU 311 (CPU 19 shown in FIG. 1) that controls the whole body cooperative motion of the robot 1, a RAM 312, a ROM 313, an external storage device (hard disk drive, etc.) 314, and the like. This is an independent drive type information processing apparatus capable of performing a complete process. Also, the external storage device 314 can store, for example, walking patterns calculated offline, target ZMP trajectories, and other action plans.
The motion control module 300 includes an actuator 350 that realizes the degrees of freedom of joints distributed over the whole body of the robot 1 shown in FIG. 2, a distance measurement sensor (not shown) that measures the distance to the object, and the trunk A posture sensor 351 for measuring the posture and inclination of the unit 202, grounding confirmation sensors 352 and 353 for detecting the left or right foot floor getting off or landing, a load sensor provided on the sole 121 of the sole 121, a battery, etc. Various devices such as a power control device 354 to be managed are connected via a bus interface (I / F) 310.
Here, the attitude sensor 351 is configured by, for example, a combination of an acceleration sensor and a gyro sensor, and the grounding confirmation sensors 352 and 353 are configured by proximity sensors, micro switches, or the like.

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

The motion control module 300 controls the whole body cooperative motion by each actuator 350 in order to embody the action instructed from the thought control module 200. That is, the CPU 311 extracts an operation pattern corresponding to the action instructed from the thought control module 200 from the external storage device 314 or generates an operation pattern internally. Then, the CPU 311 sets a foot movement, a ZMP trajectory, a trunk movement, an upper limb movement, a horizontal waist position, a height, and the like according to a specified movement pattern, and instructs a movement according to these setting contents. The command value is transferred to each actuator 350.

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

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

The function of the CPU 19 (CPU 311) shown in FIGS. 1 and 5 will be described below.
FIG. 5 is a functional block diagram of some functions of the CPU 19 shown in FIGS. 1 and 4.
As illustrated in FIG. 5, the CPU 19 includes, for example, an image preprocessing unit 31, a map data generation unit 33, a system bias identification unit 35, a position / posture detection unit 37, and a motion control unit 39.
Each of the components shown in FIG. 5 may be realized by the CPU 19 executing the program PRG, or may be realized by dedicated hardware.
Hereinafter, each configuration shown in FIG. 5 will be described in detail.

[Image Preprocessing Unit 31]
The image preprocessing unit 31 calculates disparity information (distance information) between the right image data S10R and the left image data S10L input from the CCD cameras 10R and 10L, and color image data (YUV: luminance Y, UV color difference). ) And parallax image data (YDR: luminance Y, parallax D, reliability R) are calculated alternately for each frame.
Here, the parallax indicates a difference between points where a certain point in the space is mapped to the left camera and the right camera, and changes according to the distance from the camera.

Based on the color image data and the parallax image data, the image preprocessing unit 31 generates distance data (stereo data) S31 indicating a three-dimensional distance from the robot 1 to the object in the image (environment). It outputs to the map data generation part 33 as imaging data IM.
The image preprocessing unit 31 detects a plurality of planes existing in the environment from the distance data, and outputs plane data S32 indicating the planes to the map data generation unit 33 as imaging data IM. As a plane detection method here, a known plane detection technique using a line segment expansion method, a Hough transform, or the like can be applied. As a representative example of detecting a plurality of planes like staircases from distance data including noise, it is possible to accurately detect planes by performing plane detection by a line segment expansion method.

[Map generator 33]
The map generation unit 33 generates map data MAP (three-dimensional grid map data) based on the imaging data IM (distance data S31 and plane detection data S32) input from the image preprocessing unit 31, and stores this in the memory 18. Write.
The map data generation unit 33 generates map data MAP using a technique such as SLAM (Simultaneous Localization And Mapping), for example.

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

The map generation unit 33 performs the following processing on all unit grids (hereinafter referred to as cells) p (x, y, z) in the real space represented by a three-dimensional grid.
The map generation unit 33 maps the map data MAP based on an algorithm that there is no obstacle on the straight line connecting the measurement point (three-dimensional grid) projected as an image in the captured image and the observation center (stereo camera center). Are created and / or updated.

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

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

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

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

In this specific example, the area represented as a three-dimensional grid is represented by a three-dimensional grid, for example, 4 meters square, with a horizontal resolution of 4 cm and a vertical resolution of 1 cm as one cell.
The map data MAP generated by the map generation unit 33 indicates, for example, the occupation probability OG (x, y, z) (= p (c)) for each cell of three-dimensional coordinates (x, y, z). Yes.

In the present embodiment, a plurality of map data Mi having different bias values bias_i (Δα, Δd, Δβ) defined in advance with respect to the movement amounts (α, d, β) shown in FIG. 6 of the robot 1 are prepared. Here, i is an integer of 2 or more. The bias value is defined in steps of 0.1 ° between -5 ° and + 5 °, for example, an angle such as Δα and Δβ.
In the present embodiment, the bias values bias_i (Δα, Δd, Δβ) are values for suppressing errors that always occur due to mechanical (system) factors of the robot 1.
Such an error is caused by, for example, an error in the length of the foot, a light error in the wheel when there is a wheel, or the like.
For each of a plurality of map data Mi having different bias values bias_i, the map data generation unit 33 includes position / posture data position (xi, yi, θi) of the corresponding bias value bias_i input from the position / posture detection unit 37, and The map data Mi of each bias value bias_i is updated based on the imaging data IM.
The map data generation unit 33 writes the map data Mi into the memory 18.

[System bias specifying unit 35]
The system bias specifying unit 35 performs a predetermined evaluation process based on the plurality of map data Mi read from the memory 18 and having a plurality of different bias values bias_i, and specifies an optimum one of the plurality of bias values bias_i. (select.
For example, the system bias identification unit 35 executes any one of the following first to third evaluation processes.
(First evaluation process)
For example, as shown in FIG. 7, the system bias identification unit 35 calculates entropy E for each of the plurality of map data Mi based on the following equation (3).
In the following formula (3), “c” indicates a cell in the map data Mi.

[Equation 3]
E = −Σ _c (log (p (c | occ)) p (c | occ) + log (p (c | empt)) p (c | empt))
... (3)

The system bias specifying unit 35 specifies the bias value bias_i corresponding to the map data Mi having the sharpest peak in entropy, using the entropy E generated by the above equation (3) as the evaluation value fit.
The system bias identification unit 35 outputs the identified bias value bias_i to the position / posture detection unit 37 and the motion control unit 39.

(Second evaluation process)
The system bias specifying unit 35 extracts a straight line portion in the map data Mi, and determines the appropriateness of the bias value bias_i corresponding to the map data Mi based on the variance value.
For example, the system bias specifying unit 35 extracts a straight line portion from the map data Mi by the procedure shown in FIG.
In this embodiment, the following algorithm using RANSAC that is robust against noise was used as a straight line detection method.
First, the system bias specifying unit 35 randomly selects two occupied points in the map data Mi (step ST11). An occupied point is, for example, a point whose probability of being occupied is a predetermined value or more.
Next, the system bias specifying unit 35 calculates a straight line defined by the selected two occupied points (step ST12).
Next, the system bias specifying unit 35 specifies an occupancy point that is within a predetermined threshold with respect to the straight line calculated in step ST12 (step ST13).
Next, the system bias identification unit 35 uses the occupied points identified in step ST13 to recalculate the straight line that best matches those occupied points by the least square method or the like, and the dispersion of the distance from the straight line of the occupied point group The value is used as a score (step ST14).
The system bias specifying unit 35 returns to step ST16 if the condition (the number of iterations exceeds a certain number or the score obtained in step ST14 exceeds a certain threshold), otherwise returns to step ST16. Return to step ST13 (step ST15).
The system bias specifying unit 35 returns to step ST17 when the condition (the straight line score obtained at step ST15 is better than the straight line score obtained before) is satisfied, and otherwise, The process proceeds to step ST18 (step ST16).
The system bias specifying unit 35 stores the straight line determined in step ST16 (step ST17).
The system bias specifying unit 35 returns to step ST19 when the condition (the number of iterations exceeds a certain number or the best score stored in step ST17 exceeds a certain threshold) is satisfied, and so on. If not, the process returns to step ST11 (step ST18).
The system bias specifying unit 35 outputs the straight line stored in step ST17 as an output result (step ST19).
After that, the system bias specifying unit 35 calculates the variance value of the plurality of straight line groups extracted from the extracted map data Mi, and uses the variance value as the evaluation value fit, so that the map data having the minimum variance value is obtained. Mi is specified, and a bias value bias_i corresponding to the map data Mi is output to the position / posture detection unit 37 and the motion control unit 39.
The bias value bias_i of the map data Mi including the straight line portion having the smallest displacement variance value is used because (most of the environment is constituted by straight lines, and the map that fits the straight line is expected to be the most appropriate map. This is because of This hypothesis works most effectively, especially in homes and offices, where the environment is composed of straight lines such as corridors and walls.

(Third evaluation process)
First, the system bias specifying unit 35 compares the imaging data IM with the imaging data predicted from the current position / attitude / bias value of the moving body and the map data Mi, and the likelihood representing the likelihood of the imaging data IM. Calculate Next, the time-series integrated value (integrated likelihood weight) of the likelihood is calculated by integrating the likelihood immediately before each bias and the likelihood calculated this time, and the result is the highest. The bias value bias_i of the map data Mi is specified.
In addition, in the bias selection strategy, the system bias specifying unit 35 prioritizes observation of a certain fixed time region or moving region (window) immediately before, so that the likelihood increase rate is multiplied by the likelihood. Of the map data Mi having the highest increase ratio is specified by calculating the cumulative likelihood weight of the map data Mi and the cumulative likelihood weight after multiplying the likelihood.

[Position / Attitude Detection Unit 37]
The position / posture detection unit 37 adds the motion amount measurement values α1, d1, β1 input from the sensor group 17 and the bias value bias_i (Δα, Δd, Δβ) input from the system bias specifying unit 35, The actual motion amount (α, d, β) is calculated. Here, “α = α1 + Δα”, “d = d1 + Δd”, and “β = β1 + Δβ”.
The position / posture detection unit 37 generates position / posture data position (xi, yi, θi) of each bias value bias based on the actual motion amount (α, d, β) and generates map data. To the unit 33.
Here, xi and yi indicate the x coordinate and the y coordinate on the x and y plane of the robot 1 when the bias value is bias_i, respectively. Θi represents the direction of the robot 1 (the direction of the CCD cameras 10R and 10L) when the bias value is bias_i.

[Motion control unit 39]
The motion control unit 39 reads the map data Mi corresponding to the bias value bias_i input from the system bias specifying unit 35 from the memory 18.
Then, for example, the motion control unit 39 determines a route to move from the current location to a preset target location based on the map data Mi read from the memory 18.
Next, the motion control unit 39 controls the actuator group 16 so that the robot 1 moves on the determined rule while avoiding an obstacle based on the detection data of the sensor group 17. At this time, the motion control unit 39 controls the actuator group 16 so as to eliminate the system error of the robot 1 based on the bias values bias (Δα, Δd, Δβ) input from the system bias specifying unit 35.

Hereinafter, an operation example of the robot 1 will be described with reference to FIG.
[First operation example]
Hereinafter, an operation example for specifying the bias value bias_i in the robot 1 shown in FIG. 1 will be described.
FIG. 9 is a diagram for explaining an operation example for specifying the bias value bias_i of the robot 1 shown in FIG.
In the following operation example, hypothesis data hi is defined corresponding to each of the plurality of different bias values bias_i described above.
The hypothesis data hi is indicated by hi (xi, yi, θi, bias_i, Mi), and is defined by the position / posture data position (xi, yi, θi), the bias value bias_i, and the map data Mi described above.

Step ST31:
As an initial setting, the CPU 19 sets “0” to the bias value bias_i of all the plurality of hypothesis data hi (xi, yi, θi, bias_i, Mi).

Step ST32:
The CPU 19 sets different unique bias values for the bias values bias_i of the plurality of hypothesis data hi (xi, yi, θi, bias_i, Mi).

Step ST33:
The position / posture detection unit 37 determines whether or not the measured values (encoder values) α1, d1, and β1 of the movement amount are input from the sensor group 17 (that is, whether or not the robot 1 has moved). When it judges, it progresses to step ST34, and when that is not right, it progresses to step ST35.

Step ST34:
The position / posture detection unit 37 obtains position / posture data position (xi, yi, θi) of each hypothesis data hi from the sensor group 17 based on the measurement values α1, d1, β1 of the motion amount and the bias value bias_i. This is generated and output to the map data generation unit 33.

Step ST35:
The map data generation unit 33 determines whether or not the imaging data IM (distance data S31 and plane detection data S32) is input from the image preprocessing unit 31, and proceeds to step ST36 if it is determined that it has been input. Advances to step ST39.

Step ST36:
The map data generation unit 33 uses the position / posture data position (xi, yi, θi) to accumulate the input imaging data IM and update each of the map data Mi.

Step ST37:
As described above, the system bias specifying unit 35 calculates the evaluation value fit for each of the map data Mi.

Step ST38:
As described above, the system bias specifying unit 35 selects (specifies) the bias value bias_i, that is, the hypothesis data hi, based on the evaluation value fit generated in step ST37.

Step ST39:
The system bias identification unit 35 outputs the bias value bias_i selected in step ST38 to the position / posture detection unit 37 and the motion control unit 39.

[Second operation example]
Hereinafter, an operation example related to the motion control of the robot 1 by the motion control unit 39 will be described.
FIG. 10 is a flowchart for explaining a processing example of the motion control unit 39 shown in FIG.
Step ST51:
The motion control unit 39, for example, based on the input distance data S31 and plane detection data S32, the detection result of the sensor group 17, or the map data Mi corresponding to the selected bias value bias_i read from the memory 18, In the moving direction, it is determined whether there is an obstacle with a height that the robot 1 cannot move. If it is determined that there is an obstacle, the process proceeds to step ST12. If not, the process proceeds to step ST53.

Step ST52:
For example, the motion control unit 39 searches for a new direction in which the robot 1 can move based on the input distance data S31 and plane detection data S32, the detection result of the sensor group 17, or the map data MAP read from the memory 18. Then, the actuator group 16 is controlled to move in the searched direction.

Step ST53:
The motion control unit 39 determines whether there is an obstacle that can be avoided if the robot 1 moves to the end in the moving direction. If it is determined that there is an obstacle, the process proceeds to step ST54. If not, the process proceeds to step ST55. move on.

Step ST54:
The motion control unit 39 controls the actuator group 16 so as to move, for example, the end of the road so as to avoid an obstacle.

Step ST55:
The motion control unit 39 controls the actuator group 16 to move straight along a path that has already been determined.

As described above, according to the robot 1, a plurality of map data Mi assuming a plurality of different bias values bias_i are continuously held, and when the imaging data IM is input, the plurality of the held data is based on the map data Mi. Each of the map data Mi is updated and the result is evaluated to identify the optimum bias value bias_i.
Therefore, each time imaging data is input, the processing amount can be greatly reduced as compared with the case where the entire map data assuming a plurality of different bias values is generated from the beginning. Thereby, the processing load of the CPU 19 can be reduced, and an inexpensive one with low processing capability can be used. Moreover, real-time property can be improved.

Second Embodiment
The robot of this embodiment is the same as the robot 1 of the first embodiment except for a part of the processing of the system bias specifying unit 35a shown in FIG.
Hereinafter, processing of the system bias specifying unit 35a of the present embodiment will be described.
FIG. 11 is a flowchart for explaining the processing of the system bias specifying unit 35a shown in FIG.
In FIG. 11, the processes of steps ST61 to ST66 and ST68 to ST70 are the same as steps ST31 to ST36 and ST37 to ST39 shown in FIG. 9, respectively.
As shown in FIG. 11, the system bias identification unit 35a updates all map data Mi using a sufficient amount of imaging data IM (for example, imaging data IM of a predetermined number of frames) for performing the evaluation process. The process proceeds to step ST68 when it is determined whether the condition is satisfied.
Thereby, the reliability of the evaluation process for selecting the bias value bias_i can be improved.

<Third Embodiment>
The robot of the present embodiment is the same as the robot 1 of the first embodiment except for part of the processing of the map data generation unit 33b and the system bias identification unit 35b shown in FIG.
Hereinafter, processing of the map data generation unit 33b and the system bias identification unit 35b of the present embodiment will be described.
FIG. 12 is a flowchart for explaining processing of the map data generation unit 33b and the system bias identification unit 35b shown in FIG.
In FIG. 12, the processes of steps ST81 to ST84 and ST86 to ST91 are the same as steps ST61 to ST64 and ST65 to ST70 shown in FIG. 11, respectively.
As shown in FIG. 12, the position / posture detection unit 37 receives position / posture data position (xi) of each hypothesis data hi from the sensor group 17 on the basis of the measured motion amount values α1, d1, β1 and the bias value bias_i. , Yi, θi) (step ST84), the map data generation unit 33b performs a shift process so that the position of the robot 1 becomes the center (center) in the map data Mi and updates the map data Mi ( Step ST85).
At this time, the boundary portion generated in the map data Mi by the shift process is deleted.
According to the present embodiment, as described above, the accuracy of the evaluation process for selecting the bias value bias_i can be increased by positioning the robot 1 at the center in the map data Mi.

<Fourth embodiment>
The robot of this embodiment is the same as the robot 1 of the first embodiment except for part of the processing of the map data generation unit 33c and the system bias identification unit 35c shown in FIG.
Hereinafter, processing of the map data generation unit 33c and the system bias identification unit 35c of the present embodiment will be described.
FIG. 13 is a flowchart for explaining processing of the map data generation unit 33c and the system bias identification unit 35c shown in FIG.
In FIG. 13, the processes of steps ST101 to ST105 and ST110 are the same as steps ST31 to ST36 and ST70 shown in FIG. 9, respectively.
As shown in FIG. 13, when the map data generation unit 33c determines that the imaging data IM (distance data S31 and plane detection data S32) is input from the image preprocessing unit 31 (step ST105), the map data It is determined whether or not to update the data Mi (step ST106).
The determination in step ST106 is performed based on a rule such as, for example, once every time the determination is performed a predetermined number of times, the process proceeds to step ST109. Further, it may be determined at random whether to proceed to step ST107 or ST109.

The system bias identification unit 35c calculates a likelihood representing the likelihood of the imaging data IM by comparing the imaging data IM with the imaging data predicted from the current position / attitude / bias value of the moving body and the map data Mi. Then, by integrating the likelihood immediately before each bias and the likelihood calculated this time, a time-series integrated value (integrated likelihood weight) of the likelihood is calculated (step ST107).
Next, the system bias specifying unit 35c specifies (selects) a bias value bias_i of the map data Mi having the highest calculated likelihood weight (step ST108).

  Further, the system bias identification unit 35c updates each map data Mi based on the imaging data IM (step ST109).

<Fifth Embodiment>
The robot of this embodiment is the same as the robot 1 of the first embodiment except for part of the processing of the map data generation unit 33d and the system bias identification unit 35d shown in FIG.
Hereinafter, processing of the map data generation unit 33d and the system bias identification unit 35d of the present embodiment will be described.
FIG. 14 is a flowchart for explaining processing of the map data generation unit 33d and the system bias identification unit 35d shown in FIG.
In FIG. 14, the processes of steps ST121 to ST124 and ST133 are the same as steps ST31 to ST36 and ST70 shown in FIG. 9, respectively.

  As illustrated in FIG. 14, the position / posture detection unit 37 receives position / posture data position (xi) of each hypothesis data hi based on the motion amount measurement values α1, d1, β1 and the bias value bias_i from the sensor group 17. , Yi, θi) (step ST124), the map data generation unit 33d performs a shift process so that the position of the robot 1 becomes the center (center) in the map data Mi and updates the map data Mi ( Step ST125).

As illustrated in FIG. 14, when the map data generation unit 33d determines that the imaging data IM (distance data S31 and plane detection data S32) is input from the image preprocessing unit 31 (step ST126), the map data generation unit 33d then It is determined whether or not to update the data Mi (step ST127).
The determination in step ST127 is performed based on a rule such as, for example, once every time the determination is performed a predetermined number of times, the process proceeds to step ST132. Further, it may be determined at random whether to proceed to step ST128 or ST132.

The system bias specifying unit 35d calculates a likelihood representing the likelihood of the imaging data IM by comparing the imaging data IM with the imaging data predicted from the current position / posture / bias value of the moving body and the map data Mi. Then, by integrating the likelihood immediately before each bias and the likelihood calculated this time, a time-series integrated value (integrated likelihood weight) of the likelihood is calculated (step ST128). Store (save) (step ST129).
Next, the system bias specifying unit 35d calculates a ratio ratio between the integrated likelihood weight w generated in step ST128 and the previously generated integrated likelihood weight w for each hypothesis (step ST130).
Next, the system bias identification unit 35d selects a hypothesis (bias value bias_i) based on the ratio such as the largest based on the ratio ratio generated in step ST130 (step ST131).

  Further, the system bias specifying unit 35c updates each map data Mi based on the imaging data IM (step ST132).

Hereinafter, the effects and actions of the present embodiment will be described by exemplifying the bias of the robot 1 and the actual movement trajectory.
Consider the case where the robot 1 moves in a straight corridor 100 as shown in FIG.
When the actual bias value of the robot 1 is Δβ = −3.5 °, if the correction process is not performed, the robot 1 moves with a miracle as shown in FIG. 16 even if the linear movement control is performed.
On the other hand, when the assumed bias value bias_i is Δβ = −2.84 °, the above-described entropy changes as shown in FIG. 17B by the evaluation process, and the bias value bias_i is selected. The movement locus of the robot 1 is as shown in FIG. In FIG. 17B, the horizontal axis represents time, and the vertical axis represents (bias value [degree]).
When the assumed bias value bias_i is Δβ = −3.45 °, the movement locus of the robot 1 is as shown in FIG. 18A by the evaluation process (line extraction).
When the assumed bias value bias_i is Δβ = −3.23 °, the movement locus of the robot 1 is as shown in FIG. 19A by the third evaluation process.
Further, when the actual bias value is Δβ = −3.5 ° in the first half and Δβ = 3.5 ° in the second half, if the correction process is not performed, the movement locus of the robot 1 is as shown in FIG. become. On the other hand, when correction processing is performed, the movement locus of the robot 1 is as shown in FIG.

The present invention is not limited to the embodiment described above.
That is, those skilled in the art may make various modifications, combinations, subcombinations, and alternatives regarding the components of the above-described embodiments within the technical scope of the present invention or an equivalent scope thereof.
For example, in the above-described embodiment, the biped walking robot 1 is illustrated as the communication device of the present invention. However, for example, a moving body using wheels, or a robot with one or three feet or more may be used as the communication device. Good. Further, the communication device of the present invention may be a fixed camera device other than the robot as long as it has a communication function.

In the above-described embodiment, the map generation unit 33 exemplifies the case of generating the map data MAP indicating the occupation probability of each of the three-dimensional grids. However, the graph including the node node and the edge edge is constructed, Map data indicating a graph may be generated. In this case, each node node represents some feature or submap. Each edge edge indicates a path between node nodes.
Further, as shown in FIG. 23, the map generation unit 33 generates map data MAP that defines landmarks LM such as a television, a chair, and a table using a Kalman filter based on SLAM (Simultaneous Localization and Map Building). May be.

FIG. 1 is a functional block diagram of a robot according to an embodiment of the present invention. FIG. 2 is a diagram for explaining an example of the robot detection unit illustrated in FIG. 1. FIG. 3 is an external view of the robot according to the embodiment of the present invention. FIG. 4 is a diagram for explaining the drive system of the robot according to the embodiment of the present invention. FIG. 5 is a functional block diagram of the CPU shown in FIG. FIG. 6 is a diagram for explaining the motion amount and the bias value in the embodiment of the present invention. FIG. 7 is a diagram for describing a first evaluation process performed by the system bias identification unit illustrated in FIG. 5. FIG. 8 is a diagram for explaining the second evaluation process performed by the system bias identification unit shown in FIG. FIG. 9 is a flowchart for explaining an operation example for specifying the bias value bias_i of the robot shown in FIG. FIG. 10 is a flowchart for explaining a processing example of the motion control unit shown in FIG. FIG. 11 is a flowchart for explaining the bias value determination processing of the robot according to the second embodiment of the present invention. FIG. 12 is a flowchart for explaining the bias value determination process of the robot according to the third embodiment of the present invention. FIG. 13 is a flowchart for explaining the bias value determination process of the robot according to the fourth embodiment of the present invention. FIG. 14 is a flowchart for explaining the bias value determination process of the robot according to the fifth embodiment of the present invention. FIG. 15 is a diagram for explaining an example of a robot bias and an actual movement trajectory for the effects and operations of the embodiment of the present invention. FIG. 16 is a diagram for explaining an example of a robot bias and an actual movement trajectory for the effects and operations of the embodiment of the present invention. FIG. 17 is a diagram for explaining an example of the bias of the robot and the actual movement trajectory for the effects and operations of the embodiment of the present invention. FIG. 18 is a diagram for explaining an example of the effect of the embodiment of the present invention and the action of the robot bias and the actual movement trajectory. FIG. 19 is a diagram for explaining an example of a robot bias and an actual movement trajectory for the effects and operations of the embodiment of the present invention. FIG. 20 is a diagram for explaining an example of a robot bias and an actual movement trajectory for the effects and operations of the embodiment of the present invention. FIG. 21 is a diagram for explaining an example of the bias of the robot and the actual movement trajectory for the effects and operations of the embodiment of the present invention.

Explanation of symbols

  10L, 10R ... CCD camera, 16 ... actuator group, 17 ... sensor group, 18 ... memory, 19 ... CPU, 31 ... image pre-processing unit, 33 ... map data generation unit, 35 ... system bias specifying unit, 37 ... position / Posture detection unit, 39 ... motion control unit

Claims (11)

For each of a plurality of different bias values defined in advance with respect to the amount of movement of the moving object, first data for generating position data indicating the position of the moving object is generated based on the bias value and the measured value of the amount of movement. And the process of
The update of the plurality of map data associated with each of the plurality of bias values is obtained at the position indicated by the position data generated in the first step for the bias value corresponding to the map data. A second step performed assuming that
A third step of performing a predetermined evaluation process based on the plurality of map data updated in the second step, and specifying an optimum one of the plurality of bias values. Method. The data processing method according to claim 1, further comprising: a fourth step of controlling a drive system of the moving body based on the bias value specified in the third step. The first step further generates posture data indicating an imaging direction of an imaging unit included in the moving body based on the bias value and the measured value of the amount of movement.
2. The data processing method according to claim 1, wherein in the second step, the map data is updated in the course of obtaining the imaging data in the imaging direction indicated by the posture data generated in the first step. When the map data is composed of a plurality of three-dimensionally defined cells each indicating the probability of being occupied by an obstacle that obstructs the movement of the moving object,
The third step includes
The data processing method according to claim 1, wherein entropy is calculated for the plurality of cells constituting the map data, and the bias value corresponding to the map data in which a high peak region is formed by the entropy is specified. When the map data is composed of a plurality of three-dimensionally defined cells each indicating the probability of being occupied by an obstacle that obstructs the movement of the moving object,
The third step includes
The data processing according to claim 1, wherein a variance value of a straight line portion in the map data is detected based on the probability of the cells constituting the map data, and the bias value is specified based on the detected variance value. Method. When the map data is composed of a plurality of three-dimensionally defined cells each indicating the probability of being occupied by an obstacle that obstructs the movement of the moving object,
The third step includes
The likelihood representing the likelihood of the imaging data is calculated by comparing the imaging data with the imaging data predicted from the current position, posture, bias value and map data of the moving object, and the likelihood for each bias The data processing method according to claim 1, wherein the bias value is specified based on a series of integrated values (integrated likelihood weights). The third process performs the evaluation process on the condition that it is determined that the map data has been updated in the second process using imaging data sufficient to perform the evaluation process. The data processing method described. Based on the position data generated in the first step, further includes a fourth step of changing the map data so that the mobile body is located near the center in the map indicated by the map data,
The data processing method according to claim 1, wherein the second step updates the map data changed in the fourth step. In order to give priority to the observation of a certain fixed time region or moving region (window) immediately before, the likelihood increase ratio is integrated with the integrated likelihood weight before multiplying the likelihood and the integrated after multiplying the likelihood. The data processing method according to claim 6, wherein the bias value is calculated based on a likelihood weight and the bias value is specified based on the ratio. Position generation for generating position data indicating the position of the moving body for each of a plurality of different bias values defined in advance with respect to the moving amount of the moving body, based on the bias value and the measured value of the moving amount Means,
When the imaging data is obtained at the position indicated by the position data generated by the position generation unit for the update of the plurality of map data associated with each of the plurality of bias values for the bias value corresponding to the map data. Assumed map update means,
A bias value specifying means for performing a predetermined evaluation process based on the plurality of map data updated by the map update means and specifying an optimal one of the bias values; . A program executed by a computer,
For each of a plurality of different bias values defined in advance with respect to the amount of movement of the moving object, first data for generating position data indicating the position of the moving object is generated based on the bias value and the measured value of the amount of movement. And the steps
Imaging data is obtained at the position indicated by the position data generated in the first procedure with respect to the bias value corresponding to the map data by updating the plurality of map data associated with each of the plurality of bias values. A second procedure performed assuming that
A third procedure for performing a predetermined evaluation process on the basis of the plurality of map data updated in the second procedure and identifying the optimum one of the plurality of bias values; The program to be executed.

Images