JP2003266345A - Path planning device, path planning method, path planning program, and moving robot device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To prepare a travel path plan efficiently in a short time without performing unnecessary observation and distance image calculation processing when moving a robot device to destination. <P>SOLUTION: An obstacle map updating part 223 of a path planning device 221 receives obstacle information 331, a current position, and destination information from an obstacle grid calculation part OG to prepare (update) an obstacle map. At this time, a line of sight control part 222 controls a line of sight so that a line of sight of the robot device 1 is directed in the direction of destination. A path planning part 224 plans a path by regarding an unobserved region and a free space region as a travel possible region based on the obstacle map prepared in the obstacle map updating part 223. A path evaluation part 225 evaluates the path planned in the path planning part 224, outputs the path from the path planning part 224 based on the results of evaluation, outputs the results indicating that preparation is impossible, and adjusts line of sight control by the line of sight control part 222. <P>COPYRIGHT: (C)2003,JPO

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention searches a route on an environment map while controlling the line-of-sight direction in a visual recognition means such as a CCD camera provided in a head unit of a mobile robot apparatus. The present invention relates to a route planning device for planning a route, a route planning method, a route planning program, and a mobile 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.

Many mobile robot devices having a conventional distance observing device and map generating device have a line-of-sight control algorithm for observing the line of sight in all directions in order to create a map of the entire environment. The method of creating a map of the entire environment is suitable when the destination to be moved is unknown or when it is desired to move the entire environment like a cleaning robot, but the destination to move and the direction to which it is aimed If is already given, it is inefficient because it contains many unnecessary observations.

[0007]

The present invention can efficiently generate a travel route plan in a short time by not performing unnecessary observation and distance image calculation processing when moving to a destination. A path planning device, a path planning method, a path planning program, and a mobile robot apparatus.

[0008]

In order to solve the above-mentioned problems, a route planning apparatus according to the present invention is designed such that a mobile robot apparatus avoids obstacles from a current position to a target position in an environment with obstacles. In a route planning device for planning a route for moving, a line-of-sight control means for controlling the line-of-sight of a mobile robot device, and an obstacle consisting of an obstacle region, a free space region and an unobserved region based on the information on the obstacle. Obstacle map updating means for creating or updating a map, and a path of the mobile robot apparatus assuming that the free space area and the unobserved area on the obstacle map updated by the obstacle map updating means are movable spaces. And a route evaluation unit for evaluating the route plan planned by the route planning unit, the route evaluation unit planning by the route planning unit. Adjusting the path planning means and said eye control means depending on whether the unobserved region route planning that is included.

In order to solve the above problems, the route planning method according to the present invention provides a route for a mobile robot apparatus to move from a current position to a target position while avoiding obstacles in an environment with obstacles. In the route planning method to plan,
A line-of-sight control process for controlling the line-of-sight of the mobile robot device,
An obstacle map updating step of creating or updating an obstacle map composed of an obstacle area, a free space area and an unobserved area based on the information on the obstacle, and an obstacle map updated by the obstacle map updating step. A route planning step of planning a route of the mobile robot device as the movable space of the free space region and the unobserved region, and a route evaluation process of evaluating the route plan planned in the route planning process. The route evaluation process adjusts the route planning process and the line-of-sight control process depending on whether or not the unobserved region is included in the route plan planned in the route planning process.

In order to solve the above-mentioned problems, a route planning program according to the present invention provides a route for moving a mobile robot apparatus from an existing position to a target position while avoiding obstacles in an environment with obstacles. In the planned route planning program, a line-of-sight control process for controlling the line-of-sight of the mobile robot device and an obstacle map including an obstacle region, a free space region, and an unobserved region is created or updated based on the information about the obstacle. Obstacle map updating step, and a path planning step of planning a path of the mobile robot device by assuming that the free space area and the unobserved area on the obstacle map updated by the obstacle map updating step are movable spaces. And a route evaluation process for evaluating the route plan planned in the route planning process, wherein the route evaluation process is planned in the route planning process. Wherein the path planning to move while avoiding obstacles the mobile robot device from the current position by adjusting the path planning step and the eye control process depending on whether it contains unobserved region to the target position.

In order to solve the above problems, a mobile robot apparatus according to the present invention comprises a head unit and at least one head unit.
A mobile type comprising the above movable leg unit and a body unit having information processing means, and moving from the current position to a target position while avoiding obstacles using the movable leg unit in an environment with obstacles. In the robot device, a line-of-sight control unit that controls the line of sight by the visual recognition unit disposed in the head unit, and an obstacle map including an obstacle region, a free space region, and an unobserved region based on the information about the obstacle is displayed. Obstacle map updating means for creating or updating, and route planning means for planning a movement route assuming that the free space area and the unobserved area on the obstacle map updated by the obstacle map updating means are movable spaces. And adjusting the route planning unit and the line-of-sight control unit depending on whether or not the unobserved region is included in the route plan planned by the route planning unit. The path planning apparatus comprising a route evaluation unit becomes provided in the body unit.

As described above, according to the present invention, when searching for a moving route from an obstacle map, it is assumed that not only the free space region but also the unobserved region is a movable space, and the unrecovered region on the route is output. Re-observe the observation area.

[0013]

BEST MODE FOR CARRYING OUT THE INVENTION Embodiments of the present invention will be described below with reference to the drawings. In this embodiment, the route planning device according to the present invention is applied to a bipedal robot device.

FIG. 1 is a block diagram showing the outline of a bipedal robot apparatus. As shown in FIG. 1, the head unit 250 of the robot apparatus 1 includes two CCD cameras 2
00R, 200L are provided, and this CCD camera 200
A stereo image processing device 210 is provided after the R and 200L.
Is provided. Two CCD cameras (hereinafter, right eye 2
00R, left eye 200L. ), The right-eye image 201R and the left-eye image 201L are input to the stereo image processing device 210. Stereo image processing device 210
Is the disparity information (disparity) of each of the images 201R and 201L.
data) (distance information) and a color image (YUV: luminance Y, UV color difference) 202 and a parallax image (YDR: luminance Y, parallax D, reliability R) 203 are alternately calculated 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, and 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 231 serving as a command from the CPU 220 is supplied, and the motor is driven 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. Potentiometer attached to this actuator,
Each sensor 240 such as a touch sensor attached to the sole of the foot and a gyro sensor attached to the torso measures the current state of the robot device such as the current joint angle, installation information, and posture information, and the sensor data CP as 241
Output to U220. The CPU 220 includes the color image 202 and the parallax image 20 from the stereo image processing device 210.
3 and the sensor data 241 such as all joint angles of the actuator are input to realize the 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. Note that, as the coordinates indicating the position of the robot device, for example, a camera coordinate system (hereinafter also referred to as absolute coordinates) of a world reference system in which a specific object such as a landmark described later is the origin of the coordinates, and the robot device itself. Two coordinates are used, which is a robot center coordinate system (hereinafter also referred to as relative coordinates) with the center being the center (the origin of the coordinates).

FIG. 2 is a schematic diagram showing a 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 exchanges data and invokes 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 path planning decision unit (Situated behavior La
yer) 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.

First, the route planning apparatus of the present invention mounted on the above-mentioned robot apparatus 1 will be described. The route planning device uses the Situated behavior
yer) It is configured in the CPU 220 that executes the SBL 360. FIG. 3 shows a functional block diagram of the route planning device 221. The route planning device 221 includes a line-of-sight control unit 222 that controls the line of sight of the robot device 1, an obstacle map updating unit 223 that creates (updates) an obstacle map, and a route planning unit 2
24, and a route evaluation unit 225 that evaluates the route planned by the route planning unit 224.

Route planning device 221 (route plan determination unit SB
The obstacle map update unit 223 of (L360) receives the obstacle information 331 and the current position and destination information from the obstacle grid calculation unit OG330, and creates (updates) the obstacle map. At this time, the line-of-sight control unit 222 controls the line-of-sight so that the line-of-sight of the robot apparatus 1 is directed toward the destination. The route planning unit 224 uses the obstacle map updating unit 22.
Based on the obstacle map created in 3, the unobserved area and the free space area are considered as movable areas and the route is planned. The route evaluation unit 225 evaluates the route planned by the route planning unit 224, and determines the route based on the evaluation result.
The output is made from 24, the result that it cannot be generated is output, and the line-of-sight control by the line-of-sight control unit 222 is adjusted.

The points on the obstacle map generated by the obstacle map updating section 223 based on the obstacle information 331 from the obstacle grid calculating section OG330 are the following three points as shown in FIG.
It is classified into two types.

The first point is a point where an obstacle exists (point represented by 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. 5, and the details will be described below.

First, the line of sight is directed toward the destination so that an obstacle map around the straight line path 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).

[0026] 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 the A ^* search (YES), the process proceeds to step S76, and it is determined 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 the 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.

Next, the robot apparatus 1 shown in FIG. 2 described above.
The configuration and operation of the software will be described in detail. FIG. 6 is a flowchart showing the operation of the software 300 shown in FIG.

The image data 30 is stored in the kinematic odometry KINE 310 of the software 300 shown in FIG.
1 and the sensor data 302 are input. This image data 301 is a color image and a parallax image from a stereo camera. The sensor data 302 is data such as the joint angle of the robot device. The kinematic odometry KINE 310 is used to input these input data 301, 30.
2 is received, and the image and sensor data up to that time stored in the memory are updated (step S101).

Next, the image data 301 and the sensor data 302 are temporally correlated (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 corresponding to the simultaneous conversion matrix 311 and 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 calculator OG330, the odometry 314 calculated by the kinematic odometry KINE310 and the obstacle observation information (obstacle information) 321 calculated by the plane extractor PLEX320 are input. , The data that has been stored in the memory up to that point is updated (step S105). Then, the obstacle grid that holds the probability of whether or not there is an obstacle on the floor is updated by a probabilistic method (step S106).

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 route plan for avoiding 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 the memory in advance (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 route plan determination unit SBL360 acquires the recognition result regarding the obstacle existing around the robot device 1 from the obstacle information 331 from the obstacle grid calculation unit OG330, and the current robot device from the absolute coordinate calculation unit LZ350. By acquiring the absolute coordinates of 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, and the generated route is generated. A motion command for performing walking is issued according to. That is, the action to be taken by the robot apparatus 1 is determined according to the situation from the input data, and the action sequence is output (step S112).

In the case of human navigation, the obstacle grid calculation unit OG330 shows the recognition result regarding the obstacles existing around the robot apparatus and the absolute coordinate 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. 7 is a diagram schematically showing the flow of data input to the above software. In FIG. 7, 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 (FaceDetector) 371
Is an object that detects a face area in the image frame, and is a color image 202 from an image input device such as a camera.
Is received, and it is reduced and converted into, for example, a nine-step scale image. A rectangular area corresponding to the face is searched from all the images. Information 372 such as the position, size, and feature amount regarding the area finally determined to be the face is output by reducing the overlapping candidate areas, and the face identification unit FI (FaceIden) is output.
tify) Send to 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 EDT371.

Color recognition unit MCT (MultiColorTracker) 3
An object 73 for 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 divides it into continuous areas. . Color recognition unit MCT3
73 outputs information 374 such as the position, size, and feature amount of each divided area to the DIL 379.

Motion detection unit MDT (MotionDete)
(ctor) 375 is a portion that detects a moving portion in the image, and outputs the detected motion area information 376 to the distance information adding unit DIL 379.

Distance information addition unit DIL (Distance)
Information Linker) 379 is an object that adds distance information to the input two-dimensional information and outputs three-dimensional information. ID information 378 from the face detection unit FI377 and each divided area from the color recognition unit MCT373. Information 374 such as the position, size, and feature amount of the
The distance information is added to the motion area information 376 from the motion detection unit MDT375, and the three-dimensional information 380 is stored in the short-term storage unit S.
Output to TM (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 receives a voice recognition result (word, sound source direction, certainty factor) from an Arthur Decoder (not shown). , The position and size of the skin color area and the position and size of the face area from the color recognition unit MCT373, and the face identification unit FI3
The ID information of the person is received from 77. 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 passed to a higher-order module such as a route plan determination unit (situation-dependent action hierarchy) (SBL) 360.

The path plan determination unit SBL is an object that determines the action (action depending on the situation) of the robot apparatus 1 based on the information from the above-mentioned short-term storage unit STM381. For example, a route plan is created as described above, and a command for causing the robot apparatus 1 to walk is issued based on the route plan. Moreover, it is possible to simultaneously evaluate and execute a plurality of actions. In addition, the action can be switched to put the aircraft in a sleep state, and another action can be activated.

The above-mentioned objects will be described in more detail below.

As described above, the CCD cameras 200L, 2
The video captured by 00R is input to the stereo image processing device 210, and the left and right images 2 specifically shown in FIG.
The color image (YUV) 202 and the parallax image (YDR) 312 are calculated from the parallax information (distance information) of 01R and 201L, and are input to the CPU 220. In addition, sensor data 2 from each sensor provided in the robot apparatus 1
40 is input. As shown in FIG. 2, image data 301 including the parallax information and the parallax image, and sensor data 302 that is data such as a joint angle of the robot device are input to the kinematic odometry KINE 310.

Kinematic Odometry KINE310
Is the joint angle of the sensor data 302 at the time when the image of the image data 301 is captured based on the input data consisting of the image data 301 and the sensor data 302, and the robot device 1 uses the joint angle data The robot central coordinate system fixed to is converted into the coordinate system of the camera provided in the head unit. In this case, in the present embodiment, the simultaneous conversion matrix of the camera coordinate system is derived from the robot center coordinate system, and the simultaneous conversion matrix 311 is obtained.
And the parallax image 312 corresponding to this and the plane extraction unit PLEX
Output to 320.

The plane extraction unit PLEX 320 receives the simultaneous conversion matrix 311 and the parallax image 312 corresponding thereto,
An obstacle is recognized according to the processing procedure shown in FIG. 9 below.

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 attachment 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 (θ, φ) shown in FIG. A large parameter vote can be regarded as an obstacle, and a small 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.

Next, the simultaneous conversion matrix 311 of the camera coordinate system
To conversion of the robot to the plantar bottom contact surface as shown in FIG. 11 (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 (1) and (2) by using the fact that the distance d from the plane is smaller than the threshold value D _th .

[0058]

[Equation 1]

[0059]

[Equation 2]

FIG. 12 shows the measurement points (x marks) selected within the range where the threshold value D _th is 1 cm. In FIG. 12, the black dots are points that were not judged to be flat.

Therefore, points (points not on the floor) other than the points lying on the plane (floor) in step S66 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) of FIG. 13)
The height z of the floor extraction image ((b) of FIG. 13) obtained from FIG.
If the coordinates are converted so that (z = 0), it is possible to represent the floor or the obstacle at a two-dimensional position on the 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. In addition, the method used by existing mobile robots can be applied to route planning because it is an obstacle expression on a two-dimensional floor.
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 a y-axis direction on the environment map. And orientation information indicating the direction in which the robot device faces from the direction. Further, the environment map has a grid (obstacle occupied area) in which obstacle information is input from the plane extraction unit PLEX and recognized as an obstacle. This obstacle grid calculation unit OG330
When the robot moves, that is, the odometry 31
When 4 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 of the robot apparatus (difference movement angle) and the movement distance (difference movement amount). Update the environment map and posture direction information. 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. In addition, by appropriately changing the size of the environmental map and its grid,
It is possible to reduce the amount of calculation and the memory copy cost.

The obstacle grid calculator OG330 will be described in detail below with reference to FIGS. 14 to 19. Figure 1
4 is a schematic diagram showing an environment map recognized by the obstacle grid calculation unit OG330. As shown in FIG. 14, the environment map 400 is an environment map (map information in the robot center coordinate system) that is map information of the surroundings around the robot device (moving body) 403, and has a grid of a predetermined size. Four
It is divided into 10. When the robot device 403 moves, the robot device 403 constantly moves the environment map 400.
It is updated so as to be located in the central grid 401 located at the center of the. Environmental map 4 shown in FIG. 14
00 and the size of the grid 410 can be appropriately changed as needed. That is, for example, when a fine environment map is unnecessary, the environment map 400 is made smaller, or the size of the grid 410 is made larger to lower the resolution, thereby reducing the calculation amount and the memory copy cost. be able to.

In FIG. 14, a hatched grid 402 indicates a minimum unit area (obstacle occupied area) recognized as an obstacle. As a method of recognizing an obstacle in the position recognizing device, for example, a method of obtaining from a parallax image by a stereo camera, a method of using rebound of radio waves such as a range finder, and a method of using a distance measuring sensor such as an ultrasonic sensor . Then, the position recognition device recognizes (displays) the position of the obstacle recognized by these methods as an attribute of the grid of the environment map.
In the present embodiment, the obstacle information is input from the plane extraction unit PLEX320 described above.

As described above, when the odometry 314 is input from the kinematic odometry KINE 310 to which the data image and the sensor data are input, the obstacle grid calculation unit OG330 updates the environment map previously stored in the memory. To do. In the memory, the robot device 4
No. 03, which is an environment map 400 that is map information that includes information about obstacles around the robot apparatus, and attitude information 404 that indicates the orientation of the robot apparatus (moving body) on this map. . Posture information 404
Indicates a direction in which the robot device 403 faces from a predetermined direction, for example, the x-axis direction or the y-axis direction. And
As the posture and movement of the robot apparatus change, FIG.
The previously recognized environment map and the information on the posture direction are updated.

First, the method of updating the environment map will be described. In the position recognition device of the present invention, the environment map is not updated depending on the amount of movement of the robot device or the like. That is, if the movement amount is within a predetermined area (grid), the environment map is not updated, and if the movement amount exceeds the size of the grid, the environment map is updated.

15 (a), (b) and (c), (d)
FIG. 9 is a diagram showing a method of updating the environment map according to the movement amount, and is a schematic diagram showing the environment map when the environment map is not updated and when the environment map is updated. FIG. 15 (a)
As shown in FIG.
The robot device 503 is located within 01. A hatched grid 502 indicates an obstacle area. The movement amount 505 of the robot device 503 is small, and
As shown in (b), even in the robot device 506 after the position of the robot device 503 has moved, the robot
If the position of 6 is within the central grid 501, the environment map 500 is not updated. In this case, the center grid 50
Only the movement position information of the robot device in 1 is updated.

On the other hand, similar to FIG.
As shown in (c), the movement amount 5 of the robot apparatus 503 located in the central grid 501 in the environment map 500.
If 15 greatly exceeds the central grid 501, FIG.
As shown in (d), the updated environment map 510 is obtained by shifting and updating the environment map 500 by the number of moved grids so that the robot device 513 after movement is located on the center grid 511 of the environment map.

That is, the size of the grid, which is the minimum unit of the size recognized as the obstacle area, is CS (Cell Siz
e), the position of the robot device in the grid is (Bx, B
y), the movement amount of the robot device in the two-dimensional direction is (dx, d
y), the magnitude of the grid shift (Sx, S
y) is shown by the following mathematical expression (3).

[0073]

[Equation 3]

Here, in the above formula (3), []
Is a Gaussian symbol and represents the maximum integer value that does not exceed the value in []. The robot position (Rx, Ry) in the grid is expressed by the following mathematical expression (4).

[0075]

[Equation 4]

The values of the equations (3) and (4) are shown in FIG.
6 shows. As shown in FIG. 16A, the robot device 6
03 is a central grid 60 in the environment map 600 divided into a plurality of grids having a grid size CS.
Located within 1. The environment map 600 has a plurality of grids 602 that indicate obstacle areas shown by hatching. Here, for example, a point 607 at the lower left corner in the figure in the center grid 601 is a robot device 603 in the grid 601.
Can be the position origin. This robot device 60
The position of 3 is the position (Bx,
By). This robot device 60
When 3 moves by the movement amount (dx, dy) 605 and moves to the grid 608, the position of the robot apparatus 613 after the movement can be represented as (Bx + dx, By + dy).

The robot unit 603 moves the moving amount (dx, d
y) When moving 605, the environmental map shift amount (S
x, Sy) can be calculated from the above equation (1). Here, in the updated environment map 610, the environment map 6
00 is updated by shifting the environment map by the shift amount Sx in the x-axis direction and the shift amount Sy in the y-axis direction, which are shifted in the opposite directions to the shift amount dx in the x-axis direction and the shift amount dy in the y-axis direction, respectively. be able to. In addition, here, the movement amount (dx, dy) of the robot device is positive in the right direction, and the shift amount (Sx, Sy) of the environment map is positive in the left direction which is the opposite direction to the movement direction.

In this way, the robot apparatus 613 after moving is
It is located on the central grid 611 of the updated environment map 610.
Here, the robot position (Ax, Ay) in the center grid 611 of the updated environment map 610 after shifting the environment map of the robot apparatus 613 after movement is the center grid 601 of the pre-shift environment map 600 of the robot apparatus 603 before movement.
From the robot position (Bx, By) and the in-grid movement amount (Rx, Ry) 615, the robot position (Ax, By)
Ay) = (Bx + Rx, By + Ry) = (Bx + dx−
It can be calculated as CS × Sx, By + dy−CS × Sy).

Next, updating of information on the posture direction of the robot device will be described. FIGS. 17A and 17B are schematic diagrams showing the states before and after updating the posture direction. Regarding the posture information of the robot device, the environment map can be shown as an angle with respect to the fixed coordinate system, that is, when the robot device changes only the posture direction dα, only the information of the posture direction of the robot device is updated to change the environment. The map is not updated.

That is, as shown in FIG. 17A, the environment map 7 having a plurality of grids 702 which are obstacle regions.
In the robot apparatus 703 located on the central grid 701 at 00, the robot posture direction before movement is α. The posture direction α can be shown as a tilt from the y-axis or a tilt from the x-axis, with the clockwise direction being positive, for example.
Here, when the robot device 703 rotates at the posture rotation angle dα, the robot posture direction (posture information) after movement can be shown as α + dα, as shown in FIG. 17B.

FIG. 18 shows the obstacle grid calculation unit OG33.
It is a figure which shows the effect which 0 brings. As shown in FIG. 18, the environment map and the information on the posture direction of the robot apparatus itself are updated, that is, the movement amount (odometry) 803 is transferred from the kinematic odometry KINE 801 to the obstacle grid calculation unit 802 every predetermined time. Although it can be automatically performed, it is performed every time the robot device walks one step. As a result, it is possible to reduce the load of the robot device, for example, the CPU, by not performing the update work when not moving and by eliminating the calculation cost and the memory copy cost due to the update work. Further, when the software configuration performs processing in units of objects, the data to be transferred may be only the movement difference amount / rotation difference amount (dx, dy, dα) of the robot device, and the absolute coordinates of the robot device and The amount of data is smaller than that of passing an absolute angle, and high accuracy can be obtained.

Further, when the entertainment robot apparatus performs processing other than position recognition, there are cases where the priority of position recognition may be lowered because it is not directly related to position recognition, such as voice recognition and face recognition. In such a case, if the position recognition device (object of position recognition) is stopped, when the priority of position recognition becomes high,
There is a problem that the position recognition result before the stop cannot be matched due to movement during the stop (Kid
Nap problem) occurs. Therefore, when the priority of the position recognition may be lowered, the position recognition object is stopped by increasing the size CS of the environment map grid or by decreasing the size of the environment map size itself. It is possible to reduce the CPU resource without doing so.

Further, the environment map and the posture information of the robot device obtained by the above-described processing can be displayed on the operation panel shown in FIG. FIG. 19 is a schematic diagram showing an operation panel used by a user for remote control of a robot apparatus, for example. As shown in FIG. 19, the operation panel 900 includes
A camera image 901 of the robot device and an environment map 910 are displayed. In addition, the operation panel 900 is provided with, for example, operation buttons 920 for moving the robot apparatus 913.

As an important usage method of the mobile robot apparatus, there is a usage method in which the user remotely operates in addition to the autonomous operation. 2. Description of the Related Art Conventionally, there is one that displays an image obtained by a camera mounted on a robot device on a control panel of the robot. However, in particular, it is difficult for the user to operate the robot apparatus by grasping the environment around the robot apparatus only with a camera having a limited field of view. However, as in the present embodiment, the CCD installed in the robot device 913
By displaying the environment map 910 on the operation panel 900 for remote control, the operability of robot control is dramatically improved, as compared with the conventional robot control in which remote control is performed using only the camera image 901 from the camera. .

Although it has been described here that the environment map is not updated even if the posture information of the robot changes, in a robot device, an image pickup device such as a CCD camera is usually provided on the front face of the head unit. The image 901 displays an image in front of the robot apparatus. Therefore, when the orientation information changes and the orientation of the robot device is rotated by 180 °, for example, the directions of the camera image and the environment map are reversed. Therefore, when the posture information is rotated by a predetermined angle or more, such as 45 ° or 90 °, the operability of robot control is further improved by rotating the environment map along with the rotation.

Next, the landmark position detector CLS34
As shown in FIG. 20, 0 is, for example, the green portion 10
An artificial color landmark 1004 having a color such as 01, pink portion 1002, or blue portion 1003.
Sensor information of the robot device 1 in an environment including
The self-position (position and orientation) of the robot device is identified based on the motion information performed by the robot device 1.

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, at 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 apparatus is observed as the external observation result, the existence probability p (l) of the robot apparatus can be updated in each grid according to the following mathematical expression (5).

[0089]

[Equation 5]

Further, the existence probability p (l) that the robot device exists in the state, that is, the self position l is
(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 (6), the right side is normalized for normalization by dividing by the probability p (s) of observing the landmark.

[0091]

[Equation 6]

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 rough 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 defines 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 observation position s of the self position l and 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 (7) according to the state model.

[0099]

[Equation 7]

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 the observation, the observed value s converges according to the following equation (8) according to the observation model.

[0101]

[Equation 8]

It should be noted 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.

The 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 motion 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.

[0104]

[Equation 9]

It is estimated that the state 1 of the robot apparatus at a certain time point has 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 (9-1) and (9-2).

[0106]

[Equation 10]

Here, ∇Fl and ∇Fa are as follows.

[0108]

[Equation 11]

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

[0110]

[Equation 12]

Here, each parameter is as follows.

[0112]

[Equation 13]

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 unit 344 controls the operation of the extended Kalman filter unit 344 according to the output result of the Markov localization unit 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 reshobs.

The probability p (s | mlp) of observing a landmark at the grid position mlp is the threshold parameter th.
When it is less than reshobs, it is estimated that the identification solution is not sufficiently converged due to the sensor noise even in the Markov localization unit 342 that is robust against the 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 section 344 also verifies the validity of the self-position estimation result of the extended Kalman filter section 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 internal observation information a relating to 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 above formula (5). Processing is performed (step S201). Next, in the extended Kalman filter unit 343, the self-position estimation value is updated by using the mathematical expression (9) (step S202).

When the external 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 (6). 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 threshobs.

The probability p (s | mlp) of observing a landmark at the grid position mlp is the threshold parameter th.
When it is less than reshobs, it is estimated that the identification solution is not sufficiently converged due to the sensor noise even in the Markov localization unit 342 that is robust against the 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.
If it exceeds hreshobs, the extended Kalman filter unit 343 further uses the above equation (10-1),
The update process is performed using (10-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. . Hereinafter, a bipedal walking type robot device equipped with the above-described route planning 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. 24, 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. 25 schematically shows the joint degree-of-freedom structure 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.

Further, 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 wheel 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 a total of 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. 26 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. 26, 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.

Each joint degree of freedom in the robot apparatus 1 is realized by an actuator corresponding to each joint. That is, in the head unit 3, a neck joint yaw axis actuator A2, a neck joint pitch axis actuator A3, and a neck joint roll axis actuator that represent the neck joint yaw axis 101, the neck joint pitch axis 102, and the neck joint roll axis 103, respectively. A4
Is 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 includes a trunk pitch axis actuator A5, a trunk roll axis actuator A6, and a trunk which respectively represent the trunk pitch axis 104, the trunk roll axis 105, and the trunk yaw axis 106. A yaw axis actuator A7 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 41R / L, an elbow joint unit 42R / L, and a forearm unit 43R / L. The shoulder joint pitch axis 107 and the shoulder joint roll axis are used. 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, shoulder joint pitch axis actuator A8, shoulder joint roll axis actuator A9, upper arm yaw axis An actuator A10, an elbow joint pitch axis actuator A11, an elbow joint roll axis actuator A12, a wrist joint pitch axis actuator A13, and a wrist joint roll axis actuator A14 are provided.

The leg unit 5R / L is subdivided into a thigh unit 51R / L, a knee unit 52R / L, and a tibial unit 53R / L.
5, hip joint pitch axis 116, hip joint roll axis 117, knee joint pitch axis 118, ankle joint pitch axis 119, and ankle joint roll axis 120, which represent each hip joint yaw axis actuator A16 and hip joint pitch axis actuator A1.
7, hip joint roll axis actuator A18, knee joint pitch axis actuator A19, ankle joint pitch axis actuator A20, ankle joint roll axis actuator A21
Has been deployed. It is more preferable that the actuators A2, A3, ... Used for the respective joints are small AC servo actuators of the type that are directly connected to the gear and have a single servo control system integrated into a motor unit. You can

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 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 period 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 command from the main control unit 11 and output drive control signals to the actuators A2, A3 ,. 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 the respective actuators A2, A3.
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 in 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 control section 11 sequentially takes in the sensor data, the image data and the audio data supplied from the above-mentioned respective 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 control section 11 reads out the control program and stores it in the DRAM at the initial stage when the robot apparatus 1 is powered 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 the self and surrounding conditions based on the control program, and can act autonomously according to the instruction and the action 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. 27 to 32. As described above, this control program is stored in the flash ROM 12 in advance and is read out at the initial stage of power-on of the robot apparatus 1.

In FIG. 27, the device driver layer 40 is located at the lowest layer of the control program and is composed of a device driver set 41 composed 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.
A virtual robot 43, which is a software group that provides an interface for accessing hardware such as 81-28n, a power manager 44 that is a software group that manages switching of power supplies, and various other device drivers. A device driver manager 45, which is a software group for managing, and a designed,
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. 28 shows specific software configurations of the middleware layer 50 and the application layer 51, respectively.

The middle wear layer 50, as shown in FIG. 28, 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.

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. 30, "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 certain time has elapsed 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.

In the case of this embodiment, each behavior model is changed from one node (state) NODE0 to NODEn as shown in FIG. 31 to any other node NODE0 to NODEn as a method of determining the next behavior. An algorithm called a finite probability automaton is used to probabilistically determine whether to make a transition based on the transition probabilities P1 to Pn set for the arcs ARC1 to ARCn1 connecting the nodes NODE0 to NODEn.

Specifically, each behavior model is a node NODE0 to NODEn forming its own behavior model.
To correspond to these nodes NODE0-NO
Each DEn has a state transition table 90 as shown in FIG.

In this state transition table 90, the node NO.
Input events (recognition results) that are transition conditions in DE0 to NODEn are listed in the order of priority in the column of "input event name", and further conditions regarding the transition conditions correspond to the columns of "data name" and "data range". It is described in the line.

Therefore, in the node NODE100 represented by the state transition table 90 of FIG. 32, "the ball is detected (B
ALL) ”is given, the“ size (SIZ) of the ball given together with the recognition result is given.
"E)" is in the range of "0 to 1000" and the recognition result of "obstacle detection (OBSTACLE)" is given, the "distance (DISTANCE) to the obstacle given together with the recognition result is given. ) ”Is in the range of“ 0 to 100 ”is a condition for transition to another node.

Further, in this node NODE100, even if there is no recognition result input, among the emotional model 83 and the instinct model 84, which are held by the emotion model 83 and the instinct model 84 which the behavior model periodically refers to, , "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 transition from the nodes NODE0 to NODEn are listed in the row of "transition destination node" in the column of "transition probability to other 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 to E0 to NODEn are respectively described in the corresponding places in the column of "transition probabilities to other nodes", and the actions to be output when transitioning to the nodes NODE0 to NODEn are "transition probabilities to other nodes". It is described in the line of "output action" in the column of ". 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 NODE100 represented by the state transition table 90 of FIG. 32, for example, "a ball is detected (BALL)", and the "SIZE" of the ball is in the range of "0 to 1000". When the recognition result that there is is given, there is a probability of "30 [%]"
ODE120 (node 120) ", and then" A
The action of “CATION 1” will be output.

Each behavior model has nodes NODE0-NO described as such a state transition table 90.
The DENs are configured so as to be connected to each other, and when the recognition result is given from the input semantics converter module 69, the corresponding nodes NODE0 to NODE0.
The next action is stochastically determined using the state transition table of NODEn, 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 behavior model shown on the lower side in FIG. 29 has a higher priority.

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 occurrence probability of the action when "struck (scored)" and "stabbed ( 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], the current parameter value of the emotion is E [t], and the coefficient representing the sensitivity of the emotion is ke, the following mathematical expression (1
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 is updated with the parameter values of all emotions in the same manner.

[0178]

[Equation 14]

The degree of influence of each recognition result and the notification from the output semantics converter module 78 on the variation amount ΔE [t] of the parameter value of each emotion is predetermined, and for example, “striking” is performed. 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), which is information about 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 a coefficient ki representing the sensitivity of the desire, and the following mathematical expression (1
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.

[0184]

[Equation 15]

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 the present embodiment, the parameter values for each emotion and each desire (instinct) are 0 to 10 respectively.
It is regulated so as to fluctuate in a range of up to 0, and the values of the coefficients ke and ki are individually set for each emotion and each desire.

On the other hand, the output semantics converter module 78 of the middleware layer 50, as shown in FIG. 28, 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 (inside) and surroundings (outside), and a user's instruction and action based on the above 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 the present embodiment, the route planning device is
Unnecessary observation when moving to the destination by planning the route by considering the unobserved region and the free space region as movable regions and re-observing only the unobserved region included in the output route. In addition, it is possible to efficiently generate a travel route plan in a short time without performing the distance image calculation process.

[0193]

As described in detail above, according to the route planning apparatus of the present invention, when searching a moving route from an obstacle map, not only the free space region but also the unobserved region is a movable space. By re-observing the unobserved region on the output route, it is possible to efficiently plan the route only by the minimum necessary observation and distance image calculation processing.

Further, according to the route planning method of the present invention, it is assumed that an unobserved region is a movable space in addition to the free space region when searching a moving route from an obstacle map,
By re-observing the unobserved region on the output route, the route can be efficiently planned only by the minimum necessary observation and distance image calculation processing.

Further, by executing the route planning program according to the present invention, the mobile robot device searches the moving route from the obstacle map in the free space region as well as the unobserved region in the movable space. Assuming that there is, re-observing the unobserved region on the output route, it is possible to efficiently move to the destination only by the minimum necessary observation and distance image calculation processing.

Further, the mobile robot apparatus according to the present invention, when searching for a movement route from an obstacle map, assumes that an unobserved area is also a movable space in addition to the free space area,
By re-observing the unobserved region on the output route, it is possible to efficiently move to the destination only by the minimum necessary observation and distance image calculation processing.

[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 functional block diagram of a route planning device.

[Fig. 4] On the obstacle map generated by the obstacle information,
It is the figure which showed the route from the present position to the destination position.

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

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

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

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

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

FIG. 10 is a diagram showing plane parameters detected by a plane extraction unit PLEX.

FIG. 11 is a diagram for explaining a conversion process from a camera coordinate system to a foot sole contact plane coordinate system.

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

FIG. 13 is a diagram showing that the floor surface is extracted from the robot view, and the coordinates are further converted to represent an obstacle in two dimensions (floor plane).

FIG. 14 is a schematic diagram showing an environment map displayed by the obstacle recognition device (robot device) according to the embodiment of the present invention.

15 (a), (b), (c), and (d) are diagrams showing a method of updating the environment map according to the amount of movement of the position recognition device, in the case where the environment map is not updated and the environment, respectively. It is a schematic diagram which shows an environment map at the time of updating a map.

FIG. 16 is a schematic diagram showing each movement amount when updating the environment map in the position recognition device.

17 (a) and 17 (b) are schematic views showing states before and after updating of the posture direction in the position recognition device, respectively.

FIG. 18 is a schematic diagram illustrating an effect of the position recognition device.

FIG. 19 is a schematic diagram showing an operation panel displaying an environment map and attitude information.

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 perspective view showing an external configuration of the robot apparatus according to the present embodiment.

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

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

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

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

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

[Fig. 30] Fig. 30 is a block diagram illustrating a configuration of a behavior model library of an application layer.

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

FIG. 32 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 environment map, 401,501,601,70
1 Center grid, 402, 502, 602 Obstacle grid, 403, 503, 506, 513, 603, 70
3 robot device, 410 grid, 404 information,
900 operation panel

   ─────────────────────────────────────────────────── ─── Continued front page    (72) Inventor Masaki Fukuchi             6-735 Kita-Shinagawa, Shinagawa-ku, Tokyo Soni             -Inside the corporation (72) Inventor Kenta Kawamoto             6-735 Kita-Shinagawa, Shinagawa-ku, Tokyo Soni             -Inside the corporation (72) Inventor Takeshi Ohashi             6-735 Kita-Shinagawa, Shinagawa-ku, Tokyo Soni             -Inside the corporation (72) Inventor Stefan Goodman             6-735 Kita-Shinagawa, Shinagawa-ku, Tokyo Soni             -Inside the corporation F-term (reference) 3C007 AS36 CS08 KS12 KS36 KT03                       KT11 LT06 LT11 MT02 MT08                       WA03 WA13 WA28 WB21                 5B057 AA05 BA02 CA13 CA16 DA06                       DB03 DC03 DC36                 5L096 AA09 BA05 CA02 FA02 FA59                       FA66 FA69 JA11

Claims (14)

[Claims] 1. A path planning device for planning a path for a mobile robot device to move from a current position to a target position while avoiding obstacles in an environment with obstacles, and controls the line of sight of the mobile robot device. Line-of-sight control means, an obstacle map updating means for creating or updating an obstacle map composed of an obstacle area, a free space area and an unobserved area based on the information about the obstacle, and updated by the obstacle map updating means. Path planning means for planning the path of the mobile robot apparatus by assuming that the free space area and the unobserved area on the obstacle map are movable spaces, and evaluates the path plan planned by the path planning means. And a route evaluation means for performing the route evaluation according to whether or not the unobserved area is included in the route plan planned by the route planning means. Path planning apparatus and adjusts the image means and the visual axis control means. 2. The route evaluation means, when the route plan planned by the route planning means includes the unobserved area, revises only the unobserved area so that the line-of-sight control means The route planning device according to claim 1, wherein the control is adjusted. 3. The route evaluation means includes the unobserved region in the route plan planned by the route planning means, and the number of steps of the robot apparatus from the current position to the unobserved region is a predetermined threshold value. 3. When the distance exceeds the range, the route planning by the line-of-sight control means is adjusted so as to direct the line of sight to the destination after outputting the movable route to the unobserved area. apparatus. 4. The route evaluation means includes the unobserved region in the route plan planned by the route planning means, and the number of steps of the robot device from the current position to the unobserved region is a predetermined threshold value. 3. The route planning apparatus according to claim 2, wherein the line-of-sight control by the line-of-sight control unit is adjusted so that the unobserved region is observed at a distance when the value is less than 1. 5. The route evaluation unit causes the route planning unit to output a movable route when the route plan planned by the route planning unit does not include the unobserved area. Item 1. The route planning device according to item 1. 6. A path planning method for planning a path for a mobile robot device to move from a current position to a target position while avoiding obstacles in an environment with obstacles, wherein the line of sight of the mobile robot device is controlled. Eye gaze control step, an obstacle map update step of creating or updating an obstacle map consisting of an obstacle area, a free space area and an unobserved area based on the information about the obstacle, and updated by the obstacle map updating step A route planning step of planning the route of the mobile robot device by assuming that the free space area and the unobserved area on the obstacle map are movable spaces, and evaluating the route plan planned in the path planning step. And a route evaluation step for performing the route evaluation process according to whether or not the unobserved region is included in the route plan planned in the route planning process. Path planning method characterized by adjusting the image process and the sight line control process. 7. The line-of-sight control step so as to re-observe only the unobserved region in the route evaluation step when the unplanned region is included in the route plan planned in the route planning step. 7. The route planning method according to claim 6, wherein the control is adjusted. 8. The route evaluation step includes the step plan of the route planning step including the unobserved area, and the number of steps of the robot apparatus from the current position to the unobserved area is a predetermined threshold value. 8. When it exceeds, the route plan according to claim 7, wherein the line-of-sight control by the line-of-sight control step is adjusted so as to direct the line of sight to the destination after outputting a movable route to the unobserved region. Method. 9. In the route evaluation step, the route plan planned in the route planning step includes the unobserved region, and the number of steps of the robot apparatus from the current position to the unobserved region is a predetermined threshold value. 7. The route planning method according to claim 6, wherein the line-of-sight control in the line-of-sight control step is adjusted so that the unobserved region is observed when the distance is less than. 10. In a path planning program for planning a path for moving a mobile robot device from a current position to a target position while avoiding obstacles in an environment with obstacles, the line of sight of the mobile robot device is controlled. Eye gaze control step, an obstacle map update step of creating or updating an obstacle map consisting of an obstacle area, a free space area and an unobserved area based on the information about the obstacle, and updated by the obstacle map updating step A route planning step of planning the route of the mobile robot device by assuming that the free space area and the unobserved area on the obstacle map are movable spaces, and evaluating the route plan planned in the path planning step. And a route evaluation process for performing the route evaluation process according to whether or not the unobserved region is included in the route plan planned in the route planning process. Route planning program for causing movement while avoiding an obstacle to the target position the mobile robot device from the current position by adjusting the path planning step and the eye control process. 11. The movable leg unit, comprising a head unit, at least one or more movable leg units, and a body unit having information processing means, and the movable leg unit from a current position to a target position under an obstacle environment. In a mobile robot apparatus that moves while avoiding obstacles by using a line-of-sight control unit that controls the line-of-sight by the visual recognition unit disposed in the head unit, an obstacle region based on information about the obstacle, Obstacle map updating means for creating or updating an obstacle map consisting of a spatial area and an unobserved area, and a free space area and an unobserved area movable space on the obstacle map updated by the obstacle map updating means And a route planning means for planning a movement route, and whether or not the unobserved area is included in the route plan planned by the route planning means. Mobile robot apparatus characterized by including a path planning device comprising a route evaluation means for adjusting the path planning unit and the eye control means within the body unit. 12. The line of sight by the line-of-sight control unit, so that the route evaluation unit re-observes only the unobserved region when the route plan planned by the route planning unit includes the unobserved region. The mobile robot apparatus according to claim 11, wherein the control is adjusted. 13. The route evaluation means includes the unobserved area in the route plan planned by the route planning means,
And when the number of steps of the robot apparatus from the current position to the unobserved area exceeds a predetermined threshold, after outputting the movable route to the unobserved area, the line-of-sight control is performed so as to direct the line of sight to the destination direction. 13. The mobile robot apparatus according to claim 12, wherein the line-of-sight control by the means is adjusted. 14. The route evaluation means includes the unobserved area in the route plan planned by the route planning means,
Further, when the number of steps of the robot device from the current position to the unobserved region does not reach a predetermined threshold value, the gaze control by the gaze control means is adjusted so that the unobserved region is observed by a distance. The mobile robot apparatus according to claim 11.