JP2010009370A - Traveling object control system and traveling object control method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a traveling object control system for precisely acquiring the current location of a traveling object. <P>SOLUTION: The traveling object control system 1 includes: an azimuth acquisition means 104A for acquiring the measurement direction of the traveling object 100; a transmission means 114A for transmitting at least either a sound wave and an electric wave; a distance acquisition means 106A for acquiring a measurement distance to an object in the periphery of the traveling object; a storage means 57S for storing map data 57A showing the location of an obstacle; a first location acquisition means 55B for calculating the measurement location of the traveling object 100 on the basis of at least either the sound wave and the electric wave; and a second location acquisition means for calculating the current location of the traveling object on the basis of the measurement location and the measurement direction and the measurement distance and the map data. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

  The present invention relates to a moving body control system and a moving body control method for controlling the operation of a moving body such as a robot, and in particular, a moving body control system and a movement for acquiring an accurate current position and orientation of a moving body using various sensors. The present invention relates to a body control method.

  Legged mobile robots that perform autonomous walking based on various algorithms, such as robots that perform bipedal walking like humans and robots that perform quadrupedal walking like animals, are known. Some of these legged robots and server devices that control the legged robots acquire the current position of the legged robot based on data obtained from various sensors. The legged robot moves based on, for example, the destination point input from the user and the current position.

  For example, Japanese Patent Laying-Open No. 2003-166824 (Patent Document 1) discloses a self-position identification system. According to Japanese Patent Laying-Open No. 2003-166824 (Patent Document 1), the self-localization system uses a global search device based on grid-based Markov localization and a local search device using an extended Kalman filter. . If the observation result in the global search device is valid, updating of the observation result of the local search device is permitted, but if not, the observation result of the local search device is not updated. If the observation result of the local search device is valid, the local search state is output, but if not valid, the local search device is reinitialized.

  Japanese Patent Laying-Open No. 2007-257226 (Patent Document 2) discloses a self-position detection device and a position detection system for a moving body. According to Japanese Patent Application Laid-Open No. 2007-257226 (Patent Document 2), the self-position detection device is provided in a robot that moves in a moving area having a ceiling to which a plurality of marks for position identification are affixed in an irregular arrangement, Mark arrangement storage that stores an infrared irradiation unit that irradiates infrared rays on the ceiling, a top camera that captures a mark that reflects the irradiated infrared rays, information on the arrangement of marks on the ceiling, and positional information of moving areas , A mark detection unit that detects a plurality of marks as a group characterized by the arrangement of each of the four marks by performing image processing on the captured mark, and a mark arrangement storage unit. A group having the same arrangement as the mark arrangement in the group is searched, and the associated position information is identified as the current position of the robot. Position includes a specific section.

  Japanese Unexamined Patent Application Publication No. 2008-71352 (Patent Document 3) discloses a posture estimation apparatus for a mobile robot based on a particle filter using a plurality of particles. According to Japanese Patent Laying-Open No. 2008-71352 (Patent Document 3), a sensor for detecting a posture change amount of a mobile robot and a posture change amount previously detected by a particle are applied to obtain the posture and weight value of the current particle. A particle filter unit, and an evolution application unit that applies an evolution algorithm to the obtained posture and weight and updates the posture and weight of the current particle.

  Japanese Patent Laying-Open No. 2007-240295 (Patent Document 4) discloses a position estimation device. According to Japanese Patent Laying-Open No. 2007-240295 (Patent Document 4), the position sensor detects the positions of a plurality of persons. The ID sensor detects the ID number of the optical ID tag attached to each person. The position likelihood calculating unit calculates the likelihood of the position of the person using the detected position. The ID likelihood calculating unit calculates the likelihood of the ID number of the optical ID tag using the detected ID number. The position estimation unit estimates the position of each person based on the calculated likelihood of the position of the person and the likelihood of the ID number.

Japanese Patent Laying-Open No. 2006-508345 (Patent Document 5) discloses a ranging and positioning method and apparatus. According to Japanese Unexamined Patent Publication No. 2006-508345 (Patent Document 5), such a method and apparatus is used in a building having walls and passages extending in the vertical and horizontal directions. The base station is equipped with an antenna oriented with a cosec ² pattern oriented in the vertical and horizontal directions and measures its distance to determine the position of the mobile station.
JP 2003-166824 A JP 2007-257226 A JP 2008-071352 A JP 2007-240295 A JP 2006-508345 A

  However, for a walkable robot having legs, it is difficult to obtain an accurate movement amount using an encoder or the like. That is, it is difficult to obtain an accurate current position as compared with a robot that travels by wheels. Therefore, as described above, in the measurement of the current position of the conventional robot, in order to improve the measurement accuracy, a large number of markers are installed in the area where the robot moves (in the vicinity of the area). More specifically, the robot or the server device obtains the current position of the robot based on the relative positional relationship after obtaining the relative positional relationship between such a large number of markers and the robot. That is, in the conventional robot, in order to improve the measurement accuracy of the current position, it is necessary to install a large number of markers in the area where the robot moves.

  The present invention has been made to solve such a problem, and an object of the present invention is to provide a moving body control system and a movement for controlling the operation of the moving body without placing a marker in an area where the moving body moves. It is to provide a body control method.

  According to one aspect of the present invention, a moving body control system for controlling the operation of a moving body is provided. The moving body includes a moving mechanism for moving the moving body, an azimuth acquiring means for acquiring the measurement direction of the moving body, a transmitting means for transmitting at least one of sound waves and radio waves, and a measurement distance to an obstacle around the moving body. Distance acquisition means for acquiring. The mobile body control system includes a storage means for storing map data indicating the position of an obstacle, a first position acquisition means for calculating a measurement position of the mobile body based on at least one of sound waves and radio waves, a measurement position And a second position acquisition means for calculating the current position of the moving body based on the measurement direction, the measurement distance, and the map data.

  Preferably, the second position acquisition unit calculates the current position of the moving body based on the previous current position by using a particle filter.

  Preferably, the second position acquisition unit sets a weight for each of the particles based on the particle acquisition unit that acquires a plurality of particles indicating the virtual position of the moving body, the measurement position, the measurement distance, and the map data. Setting means and calculation means for calculating the current position of the moving object based on the virtual position and weight of each particle.

  Preferably, the second position acquisition unit is configured to acquire each particle based on the particle acquisition unit that acquires a plurality of particles indicating the virtual position and virtual direction of the moving body, the measurement position, the measurement distance, the map data, and the measurement direction. The current position of the moving object is calculated based on the setting means for setting the weight to each particle, the virtual position and the weight of each particle, and the current direction of the moving object is calculated based on the virtual direction and the weight of each particle. And calculating means.

  Preferably, the moving body control system includes a means for receiving the destination, a path generation means for generating a moving path of the moving body based on the current position and the destination, with reference to the map data, and the movement path and the current position. And an operation command generating means for generating an operation command including the moving direction and the number of steps. The moving mechanism causes the moving body to follow the moving path by moving the moving body based on the operation command.

  Preferably, the moving body is a legged robot. The moving mechanism includes a plurality of leg portions capable of walking.

  When another situation of this invention is followed, the mobile body control method for controlling operation | movement of a mobile body in the server which can be connected to a mobile body is provided. The moving body includes a moving mechanism for moving the moving body, an azimuth acquiring means for acquiring the measurement direction of the moving body, a transmitting means for transmitting at least one of sound waves and radio waves, and a measurement distance to an obstacle around the moving body. Distance acquisition means for acquiring. The server device includes a storage unit that stores map data indicating the position of an obstacle, and an arithmetic processing unit. The mobile body control method includes a step in which the arithmetic processing unit acquires a measurement position of the mobile body based on at least one of sound waves and radio waves, and the arithmetic processing unit includes a measurement position, a measurement direction, a measurement distance, and map data. And calculating the current position of the moving body, the step of the arithmetic processing unit receiving the destination point, and the arithmetic processing unit referring to the map data based on the current position and the destination point. A step of generating a moving route, a step of generating an operation command including a moving direction and the number of steps based on the moving route and the current position, and a moving mechanism moving the moving body based on the operating command. Causing the moving body to follow the moving path.

  According to the present invention, there is provided a moving body control system that can accurately acquire the current position of a moving body with a relatively low performance sensor without installing a marker in an area in which the moving body moves. . Moreover, even if it is a mobile body which has the mechanism which cannot anticipate the measurement precision of the moving amount like a leg type, the path | route following method which can move to a destination point with high precision is provided.

  Embodiments of the present invention will be described in detail with reference to the drawings. In addition, about the same or equivalent part in a figure, the same code | symbol is attached | subjected and the description is not repeated.

<Overall Configuration of Legged Robot Control System 1>
FIG. 1 is a schematic configuration diagram of a control system 1 for a legged robot 100 according to an embodiment of the present invention.

  Referring to FIG. 1, a control system 1 for a legged robot 100 includes a legged robot 100 that moves in a house 200, an ultrasonic receiver 41 that receives an ultrasonic signal transmitted from the legged robot 100, and And a server device 50 that receives data from various sensors and calculates the current position of the legged robot 100. The legged robot 100 according to the present embodiment has four legs and performs a quadruped walking, but may be a humanoid biped walking.

  However, the present position and current direction acquisition method according to the present invention can be applied not only to legged robots but also to traveling robots having wheels. That is, regardless of the form of the moving mechanism that the robot has, the current position and current direction can be acquired more accurately by the configuration for acquiring the current position and current direction according to the present invention.

  The server device 50 and the legged robot 100 are configured to be capable of data communication with each other. The server device 50 is externally connected to the legged robot 100 via a dedicated line such as a LAN (Local Area Network) or a WAN (Wide Area Network) or a public line such as the Internet or a virtual private network. The operation command for the is received.

  The legged robot 100 typically performs autonomous movement (four-legged walking) in accordance with a movement command from the outside, or images the surroundings of the legged robot 100 in accordance with a shooting command from the outside. it can. More specifically, the legged robot 100 includes a head 101, a trunk 102, and a leg 103. The head 101 and the leg 103 each have a drive mechanism such as an actuator, and various operations are performed by these drive mechanisms. For example, the quadruped walking of the legged robot 100 can be controlled by controlling the actuator.

<Outline of the operation of the legged robot control system 1>
Here, an outline of the operation of the control system 1 of the legged robot 100 according to the present embodiment will be described. FIG. 2 is a sequence diagram showing an outline of operation in control system 1 of legged robot 100 according to the embodiment of the present invention. As shown in FIGS. 1 and 2, first, the legged robot 100 moves (step S002). An ultrasonic wave is transmitted from the legged robot 100 (step S004). The ultrasonic receiver 41 arranged in the house 200 or in the area where the legged robot 100 moves receives the ultrasonic waves from the legged robot 100 (step S006).

  The ultrasonic receiver 41 calculates the position of the legged robot 100 based on the received ultrasonic waves, that is, acquires the measurement position of the legged robot 100. The ultrasonic receiver 41 transmits the acquired measurement position to the server device 50 (step S008), and the server device 50 acquires the measurement position (step S010). However, the ultrasonic receiver 41 may transmit the received ultrasonic data as it is to the server device 50, and the server device 50 may calculate the measurement position of the legged robot 100 based on the data.

  The legged robot 100 calculates a measurement direction (measurement direction, measurement direction of the legged robot 100) using an azimuth magnetic sensor, which will be described later, and transmits the measurement direction to the server device 50 (step S012). The server device 50 receives the measurement direction of the legged robot 100 (step S014).

  The legged robot 100 calculates a distance to an obstacle (wall or furniture) using a distance measuring sensor described later, and transmits the distance to the obstacle to the server device 50 (step S016). The server device 50 receives the distance to the obstacle (step S018).

  Then, the server device 50 acquires the current position of the legged robot 100 using the particle filter (step S020). More specifically, the server device 50 determines the measurement position by the ultrasonic wave, the measurement direction by the azimuth magnetic sensor, and the distance to the obstacle by the distance measurement sensor with respect to the virtual position and virtual direction (virtual direction or virtual orientation) of each particle. The weighting is performed with reference to the map data in the house 200 or the map data in the area where the legged robot 100 moves. The server device 50 calculates the current position and the current direction (current direction or current direction) based on the virtual position and virtual direction of each particle and the weight.

  The server device 50 transmits an operation command (movement command, stop command, imaging command, etc.) to the legged robot 100 based on the acquired current position and current direction (step S022). The legged robot 100 receives the operation command from the server device 50 (step S024), and executes various operations (movement, etc.) based on the operation command (step S026).

Hereinafter, a configuration for realizing such a function will be described in detail.
<Hardware configuration of legged robot control system 1>
FIG. 3 is a schematic diagram showing a hardware configuration of control system 1 for legged robot 100 according to the embodiment of the present invention.

  Referring to FIG. 3, legged robot 100 is connected to a wireless LAN access point 49 by wireless communication. For example, the legged robot 100 is connected to the wireless LAN access point 49 using the internal wireless LAN slave unit 119. The wireless LAN access point 49 is connected to a server device (external PC server) 50 via a LAN or the like.

  The server device 50 is connected to the microphone array environment server 20, the security server 30, the ultrasonic environment server 40, a terminal device (not shown), and the like via a LAN. The microphone array environment server 20 is connected to, for example, an external microphone array 21. The security server 30 is connected to a plurality of security sensors 31 installed in the house 200, for example. The ultrasonic environment server 40 is connected to a plurality of ultrasonic receivers 41 installed in the house.

  The server device 50 manages (controls) the legged robot 100, and performs various controls such as movement and photographing with respect to the legged robot 100 via the wireless LAN access point 49 or the LAN, and the legged robot. Provide necessary information to 100.

Hereinafter, the legged robot 100 will be described.
The legged robot 100 includes a battery power source 112 that supplies power to each part of the legged robot 100, a wireless LAN slave unit 119 that communicates with the wireless LAN access point 49, and a LAN hub connected to the wireless LAN slave unit 119. 118, an image transmission unit 113 connected to the LAN hub 118, a sensor control unit 121 connected to various sensors, and a motor control board 107 connected to the actuator 108 and the like.

  The image transmission unit 113 includes an ultrasonic transmission tag 114 that transmits ultrasonic waves to the outside of the legged robot 100, a camera 115 that captures surrounding objects and people, a speaker 116 that outputs sound, and a microphone that receives the surrounding sound. 117. The camera 115 can capture video as digital data. For example, a color CCD (Charge-Coupled Device) camera is used.

  The camera 115 captures the surroundings of the legged robot 100 (for example, the front of the head 101) based on the imaging command from the server device 50. The camera 115 outputs the captured image data to the image transmission unit 113. The image data is transmitted to the server device 50 via the wireless LAN slave device 119. Both the camera 115 and the microphone 117 are disposed on the head 101 of the legged robot 100. The speaker 116 and the like are disposed on the trunk 102 (back) of the legged robot 100.

  The sensor control unit 121 measures an orientation sensor 104 for measuring the orientation of the legged robot 100, a human sensor 105 for detecting the presence of a human, and a distance measurement for measuring a distance to an obstacle (such as a wall or furniture). The sensor 106 is connected to an infrared distance measuring sensor 126 for detecting a distance to a wall or an object, and an infrared distance measuring sensor 136 for detecting a distance to the floor. The distance measuring sensors 106, 126, 136 irradiate infrared rays around the legged robot 100 and receive the reflected light, for example, as in triangulation, and the distance from the legged robot 100 to the obstacle or the floor. Measure.

  The motor control board 107 includes various actuators 108 for moving the head 101 and the leg 103 of the legged robot 100, three one-axis gyro sensors 109 for measuring the orientation of the legged robot 100, leg type It is connected to a three-axis acceleration sensor 110 for measuring the acceleration of the robot 100. Here, the actuator 108 is also connected to the battery power supply 111.

<Hardware Configuration of Server Device 50>
FIG. 4 is a schematic diagram showing a hardware configuration of server device 50 according to the embodiment of the present invention.

  Referring to FIG. 4, server device 50 includes a CPU 55 that executes various programs including an operating system, a memory 56 that temporarily stores data necessary for execution of the program by CPU 55, and a program that is executed by CPU 55. Is stored in a nonvolatile manner. Such a program is read from a CD-ROM 64 or a flexible disk 62 by a CD-ROM (Compact Disk-Read Only Memory) driving device 63 or a flexible disk (FD: Flexible Disk) driving device 61, respectively.

  The CPU 55 receives an operation request from the user via the keyboard 53 and the mouse 54 and outputs text data and image data generated by executing the program to the display unit 52 via the display unit interface 65. The CPU 55 performs data communication with the legged robot 100 via a communication interface 59 such as a LAN card. These parts are connected to each other via an internal bus 58.

  Data transmitted from the legged robot 100 and the ultrasonic receiver 41 via the LAN is received by the communication interface 59. The CPU 55 stores the received data in the memory 56 or the like. In accordance with the received data, the CPU 55 reads data requested from the legged robot 100 from the fixed disk 57 and transmits it to the legged robot 100 via the communication interface 59.

  In addition, the CPU 55 reads a movement command, a shooting command, or the like via the LAN based on a command input from the user via the keyboard 53 or the mouse 54, a command input via the Internet and the communication interface 59, or the like. To the robot 100.

Next, the functional configuration of the server device 50 will be described in detail.
<Functional configuration of server device 50>
FIG. 5 is a block diagram showing a functional configuration in server device 50 according to the embodiment of the present invention. Referring to FIG. 5, server device 50 has a main processing unit 55 </ b> A that controls each unit of server device 50, a storage unit 57 </ b> S that stores a program, a database, and the like, and a first that detects the position of legged robot 100. Received from the position acquisition unit 55B, the route follower 55C for moving the legged robot 100 on the set route, the security unit 55D, and a plurality of microphones or microphone server 20 arranged indoors via the LAN A voice recognition unit 55E that recognizes a conversation based on the voice data, a human detection unit 55F that detects the presence and position of a human, a face recognition unit 55G that recognizes an area where a human face is located, and a LAN. A voice receiving unit 55H for receiving voice data from the legged robot 100 via the LAN, and operation data relating to the leg movement from the legged robot 100 via the LAN. Including and a command receiving unit 55J for receiving such data regarding the presence or absence of an obstacle, the direction data receiving unit 55K that receives the azimuth data from the legged robot 100 via the LAN.

  Here, the main processing section 55A and the path following section 55C are realized by the CPU 55 reading out a program stored in advance in the fixed disk 57 or the like to the memory 56 and executing it. The route follower 55C generates a movement route of the legged robot 100 along various algorithms based on the calculated current position and the input destination (or the calculated destination). The path following unit 55C generates a specific movement command for the legged robot 100 so that the legged robot 100 moves along the movement path.

  The first position acquisition unit 55B is realized by the CPU 55 and the communication interface 59, and based on the data received from the ultrasonic server 40 (ultrasonic receiver 41), the current position of the legged robot 100 ( (Measurement position) is calculated.

  The security unit 55D is realized by the CPU 55 and the communication interface 59. The security unit 55D switches the security mode based on the security information received from the security server 30, and issues an operation command related to security (for example, an alarm output command). Or generate.

  The voice recognition unit 55E is realized by the CPU 55 and the communication interface 59. The speech recognition unit 55E inputs speech recognition text corresponding to the speech input to the legged robot 100 to the main processing unit 55A based on the speech data received from the legged robot 100.

  The human detection unit 55F is realized by the CPU 55 and the communication interface 59. The human detection unit 55F determines whether or not there is a person around the legged robot 100 based on the reaction data and image data of the human sensor 105 received from the legged robot 100, and the person exists. Get the position.

  The face recognition unit 55G is realized by the CPU 55 and the communication interface 59. The face recognition unit 55G detects and authenticates a human face based on the image data received from the legged robot 100.

  The voice receiving unit 55H is realized by the CPU 55 and the communication interface 59. The voice receiving unit 55H receives voice data from the legged robot 100.

  The command receiving unit 55J is realized by the CPU 55 and the communication interface 59. The command receiving unit 55J receives data indicating that there is an obstacle in the vicinity of the legged robot 100 from the legged robot 100, data indicating that the legged robot 100 is in contact with the obstacle, and status regarding the legged robot 100. Get information.

  The orientation data receiving unit 55K is realized by the CPU 55 and the communication interface 59. The azimuth data receiving unit 55K receives the measurement direction (measurement direction) of the legged robot 100 from the legged robot 100 via the LAN.

<Functional Configuration of Legged Robot Control System 1>
FIG. 6 is a block diagram showing a functional configuration in control system 1 of legged robot 100 according to the embodiment of the present invention. Referring to FIG. 6, control system 1 for legged robot 100 includes legged robot 100, server device 50, and ultrasonic receiver 41.

  Legged robot 100 includes a moving mechanism 108A, a transmission / reception unit 119A, a transmission unit 114A, an orientation acquisition unit 104A, and a distance acquisition unit 106A.

  As shown in FIGS. 3 and 6, the transmission / reception unit 119A is realized by a wireless LAN slave unit 119, a LAN hub 118, and the like. The transmission / reception unit 119A receives various commands from the server device 50 via the LAN, and transmits various data to the server device 50 via the LAN. More specifically, the transmission / reception unit 119A transmits a measurement distance and a measurement direction to the server device 50, and receives a movement command and a photographing command from the server device 50.

  The moving mechanism 108 </ b> A is realized by a plurality of legs 103 including a plurality of actuators 108 for operating the legs 103, for example. For example, the moving mechanism 108A moves the legged robot 100 by operating the actuator 108 in accordance with a movement command input via the transmission / reception unit 119A. For example, the moving mechanism 108A stops the legged robot 100 by stopping the actuator 108 in response to a stop command input via the transmission / reception unit 119A. More specifically, each actuator 108 (leg portion 103) is actuated based on the movement command “○ step forward, shoulder angle OO degrees” received from the server device 50, whereby the legged robot 100 is operated. Moving.

  The transmitter 114A is realized by the ultrasonic transmission tag 114, for example. The transmitter 114A periodically transmits ultrasonic waves. The ultrasonic wave transmitted from the transmitter 114 </ b> A is received by the ultrasonic receiver 41. However, the transmitter 114A may transmit radio waves.

  The direction acquisition unit 104A is realized by the direction sensor 104, the sensor control unit 121, and the like, for example. The orientation acquisition unit 104A acquires the measurement direction (measurement direction) of the legged robot 100 and transmits the measurement direction to the server device 50 via the transmission / reception unit 119A.

  The distance acquisition unit 106A is realized by a plurality of distance measuring sensors 106 and sensor control units 121 provided around the legged robot 100, for example. The distance acquisition unit 106A measures the distance from the legged robot 100 to the obstacle, and transmits the measurement distance to the server device 50 via the transmission / reception unit 119A. Here, the obstacle refers to various objects arranged in the house 200 such as a wall, a door, and furniture.

  As illustrated in FIG. 6, the server device 50 includes an input unit 53A, a storage unit 57S, a first position acquisition unit 55B, a particle acquisition unit 55L, a setting unit 55M, a calculation unit 55N, and a path generation unit. 55P, a command generation unit 55Q, and a transmission / reception unit 59A.

  As shown in FIGS. 4 and 6, the input unit 53A is realized by a keyboard 53, a mouse 54, a communication interface 59, and the like. More specifically, the input unit 53A directly receives an operation command from the user via the keyboard 53, the mouse 54, and the like. Further, the input unit 53A receives an operation command via the communication interface 59 via a LAN, the Internet, an external information device, or the like.

  The storage unit 57S is realized by the fixed disk 57 and the memory 56. The storage unit 57S includes map data 57A relating to an area where the legged robot 100 moves, a particle file 57B that stores various data necessary for calculating the current position and current direction of the legged robot 100 using a particle filter, A current information file 57C for storing the current position and current direction calculated using the particle filter is stored.

  FIG. 7 is an image diagram showing the contents of the map data 57A. As shown in FIG. 7, the map data 57 </ b> A has information indicating the positions (hatching portions in FIG. 7) of obstacles such as walls, doors, and furniture arranged in the house 200 where the legged robot 100 moves. is doing. The map data 57A also indicates the location of a television device or audio device, for example.

  The map data 57A stores information indicating a contact area within a predetermined distance from each obstacle. The contact area is defined as an area for ensuring safety. That is, when the legged robot 100 enters the contact area, the legged robot 100 may come into contact with an obstacle. The route generation unit 55P, which will be described later, refers to the map data 57A to determine the movement route from the current position (start point) to the destination point (goal point) so that the legged robot 100 does not enter the contact area. Generate.

FIG. 8 is an image diagram showing the data structure of the particle file 57B. As shown in FIG. 8, the particle file 57B includes, for each particle, an X coordinate X _{1 of} the previous current position, a Y coordinate Y _{1 of} the previous current position, a previous current direction θ ₁ , a predetermined weight W ₁ (1 (Number divided by the number of particles)). The particle file 57B stores a virtual position (X ₂ , Y ₂ ) obtained by adding noise to each of the previous X ₁ , Y ₁ , and θ ₁ and a virtual direction θ ₂ for each particle.

The particle file 57B stores, for each particle, the observation probabilities P _US of X ₂ and Y ₂ calculated based on a Gaussian function in consideration of the measurement position from the ultrasonic receiver 41. Here, the observation probability P _US means the probability that the measurement position is observed when the legged robot 100 is at its current position.

Similarly, the particle file 57B stores, for each particle, an observation probability P _COM of θ ₂ calculated based on a Gaussian function in consideration of the measurement direction from the legged robot 100.

Similarly, the particle file 57B stores, for each particle, an observation probability P _DIS of X ₂ or Y ₂ calculated based on a Gaussian function in consideration of a measurement distance from the legged robot 100 to the obstacle. Here, the particle file 57B stores, for each particle, predetermined probabilities P _OUT when the data acquired by various sensors is within the allowable range of the sensor and when the data is outside the allowable range.

  Thus, the particle file 57B stores such observation probabilities for each particle and for each sensor.

The particle file 57B stores an observation probability multiplication value Eval obtained by multiplying all the observation probabilities P _US , P _COM , P _DIS , and P _OUT . Particle file 57B, like the sum of the particles of the observation probability multiplied value Eval is 1, the each of the particles of the observation probability multiplication value Eval obtained by normalizing, and stores as the weight W ₂ of each particle.

FIG. 9 is an image diagram showing the data structure of the current information file 57C. As illustrated in FIG. 9, the current information file 57 </ _b > C stores a value obtained by weighted averaging the X coordinate X ₂ of the virtual position of each particle with the weight W ₂ as the X coordinate indicating the current position of the legged robot 100. The current information file 57 </ _b > C stores a value obtained by weighted averaging the Y coordinate Y ₂ of the virtual position of each particle with the weight W ₂ as the Y coordinate indicating the current position of the legged robot 100. The current information file 57C stores a value obtained by weighting and averaging θ ₂ of each particle with a weight W ₂ as a value θ indicating the current direction of the legged robot 100.

  4 and 6, the first position acquisition unit 55B, the particle acquisition unit 55L, the setting unit 55M, the calculation unit 55N, the path generation unit 55P, and the instruction generation unit 55Q are the CPU 55. Is realized by reading a program previously stored in the fixed disk 57 or the like into the memory 56 and executing it. That is, the first position acquisition unit 55B, the particle acquisition unit 55L, the setting unit 55M, the calculation unit 55N, the path generation unit 55P, and the instruction generation unit 55Q are functions that the CPU 55 of the server device 50 has. .

  The first position acquisition unit 55B calculates the measurement position of the legged robot 100 based on the data related to the sound wave received from the ultrasonic receiver 41. However, the legged robot 100 is configured to transmit radio waves, and the first position acquisition unit 55B calculates the measurement position of the legged robot 100 based on data related to radio waves received from a radio receiver (not shown). There may be.

The particle acquisition unit 55L acquires or newly generates a plurality of particles each indicating the virtual position and virtual direction of the legged robot 100. More particularly, the particles obtaining unit 55L may be added noise corresponding like this moving direction and number of steps to the X coordinate X ₂ and Y-coordinate Y ₂ and directions theta ₂ of the particle when calculating the current position of the previous To update the particles. At this time, the weight W ₂ when calculating the current position of the last, remove the particle is less than a predetermined threshold value, may generate a new particle. As a method of generating a new particle, the weight W ₂ when calculating the current position of the last may be divided largest particles into two.

Setting unit 55M is based on the measurement position and the measured distance and the map data 57A and the measurement direction, it sets the weight W ₂ to each of the particles.

More specifically, the setting unit 55M calculates the observation probabilities P _US of X ₂ and Y ₂ calculated based on the Gaussian function while referring to the measurement position from the ultrasonic receiver 41. Here, the observation probability P _US means the probability that the measurement position is observed when the legged robot 100 is at its current position. Similarly, the setting unit 55M calculates, for each particle, the observation probability P _COM of θ ₂ calculated based on the Gaussian function while referring to the measurement direction from the legged robot 100. Similarly, the setting unit 55M calculates, for each particle, the observation probability P _DIS of X ₁ and Y ₂ calculated based on the Gaussian function while referring to the measurement distance from the legged robot 100 to the obstacle. . Here, the setting unit 55M calculates, for each particle, predetermined probabilities P _OUT when the data acquired by the various sensors is within the allowable range of the sensor and when the data is outside the allowable range. Thus, the setting unit 55M calculates such an observation probability for each particle and for each sensor.

The calculation unit 55N calculates the current position (X, Y) of the legged robot 100 based on the virtual position (X ₂ , Y ₂ ) and the weight W _{2 of} each particle. That is, the calculation unit 55N calculates the current position (X, Y) of the legged robot 100 by performing a weighted average of the virtual positions (X ₂ , Y ₂ ) of the respective particles based on the corresponding weights W _2. .

Calculation unit 55N is currently calculating directional theta legged robot 100 based on the virtual direction theta ₂ of each particle and the weight W _2. That is, the calculation unit 55N calculates the current direction θ of the legged robot 100 by performing a weighted average on the virtual direction θ ₂ of each particle based on the corresponding weight W ₂ .

  In this manner, the particle acquisition unit 55L, the setting unit 55M, and the calculation unit 55N constitute a second position acquisition unit. The second position acquisition unit uses the particle filter to calculate the current position of the legged robot 100 based on the previous current position, measurement position, measurement direction, measurement distance, and map data. That is, the second position acquisition unit corrects the measurement position obtained from the data received from the ultrasonic receiver 41 based on the previous current position, measurement direction, measurement distance, and map data 57A. Get the current position more accurate than the measurement position.

  The route generation unit 55P refers to the map data 57A and generates a movement route of the legged robot 100 based on the current position and the destination point according to various algorithms. More specifically, the route generation unit 55P sets a plurality of sub destination points (hereinafter referred to as sub goals) between the current position and the destination point. The subgoal corresponds to a point that the legged robot 100 can reach by one rotational motion and forward motion.

  The purpose of navigation of the legged robot 100 by the route generating unit 55P and the command generating unit 55Q is to move the legged robot 100 from the current position to a destination point designated by the user. If the destination is far from the current location, the robot is unlikely to be able to reach the destination with a single movement, and an algorithm combining several motion steps is required to arrive at the destination. It becomes. This function is usually performed in two stages. First, as a first stage of navigation, the route generation unit 55P generates a route from the current position to the destination point. More specifically, the route generation unit 55P generates a route to the destination point, and generates a list of continuous subgoals that the legged robot 100 should pass to arrive at the destination point.

  The command generation unit 55Q generates a movement command based on the movement route and the current position. For example, the command generation unit 55Q generates a request to “open shoulder” and a request to “go forward” based on the movement route and the current position, and sends the request via the transmission / reception unit 59A. To the legged robot 100. The command generation unit 55Q generates an operation command so that the legged robot 100 passes through the subgoals in order. Hereinafter, the function of generating an operation command for the legged robot 100 in the command generation unit 55Q will be described in detail.

  That is, as the second stage of navigation, the command generation unit 55Q determines an action to be performed to reach the first subgoal of the route plan. When the subgoal is reached, the legged robot 100 repeats the same process for the next subgoal until it reaches the final destination point. While moving to the destination point, the current position of the legged robot 100 may be different from the planned position. Alternatively, an obstacle may appear on the path along which the robot moves. In that case, the robot must re-plan a new route to the destination.

  Here, the instruction generation unit 55Q uses the following four operation patterns to approach the destination point. That is, the command generation unit 55Q includes (1) straight forward movement of each step, (2) right curve forward movement of each step, (3) left curve forward movement of each step, and (4) rotational movement of each step. Therefore, the operation pattern with the highest operation efficiency is selected. An operation pattern with good operation efficiency refers to an operation pattern that requires few operation steps until the legged robot 100 arrives at the next subgoal. In other words, the command generation unit 55Q determines which of the movements (opening of the shoulders) is performed after determining which of the right-turn forward and the right-turn and then the straight-forward movement is effective for the legged robot 100. Condition) and the number of steps.

  The transmission / reception unit 59A is realized by the communication interface 59 or the like. The transmission / reception unit 59A receives data on the measurement distance, the measurement direction, and the like from the legged robot 100 via the LAN, and passes the data to the setting unit 55M.

  In general, in order to move the legged robot 100 to the destination, there is a method in which the landing position of the sole is planned step by step based on the trajectory of the dynamic center of gravity, and the landing is accurately performed by controlling the joint. It is used. However, in this case, a precise hardware configuration and an advanced motion control method are required (for example, JP 2008-49458 A and JP 2004-209562 A).

  The legged robot control system 1 according to the present embodiment eliminates such problems, and allows the legged robot 100 to move to the destination while having a simple hardware and software configuration. Is possible. More specifically, in the legged robot control system 1 according to the present embodiment, the landing position plan of the sole based on the dynamic center of gravity trajectory is not performed. In addition, the legged robot control system 1 according to the present embodiment generates a route to the destination point, sets a subgoal, and follows the route so that the closest subgoal can be reached within a certain likelihood. Further, the legged robot control system 1 according to the present embodiment has a function capable of executing three types of operations of straight movement, turning, and circular arc in units of steps, and the current position and the planned movement route Based on the positional relationship with the subgoal, the optimum operation (movement direction, number of steps, etc.) is determined. In addition, the legged robot control system 1 according to the present embodiment can smoothly move to a destination point with high accuracy by executing the above-described operation continuously for each subgoal.

  That is, the control system 1 for the legged robot according to the present embodiment does not require the planning of the landing position of the toe, and can tolerate it even when the self position deviates from the destination point. The legged robot 100 can accurately move to the destination point.

<Current position acquisition processing>
FIG. 10 is a flowchart illustrating a processing procedure of the current position acquisition process in the server device 50. As shown in FIG. 10, first, the CPU 55 determines whether data indicating that the legged robot 100 has moved from the legged robot 100 has been received (step S102). When data indicating that the robot has moved from the legged robot 100 is received (YES in step S102), the CPU 55 determines whether the previous current position is stored in the storage unit 57S (particle file 57B or current information file 57C). whether, i.e. the current position before the mobile _(X 1, _{Y 1)} to determine whether it is calculated (step S106).

  On the other hand, when data indicating that the server device 50 has moved from the legged robot 100 is not received (NO in step S102), the CPU 55 determines whether or not a predetermined time has elapsed since the previous current position was acquired. Is determined (step S104). If the predetermined time has not yet elapsed since the previous current position was acquired (NO in step S104), CPU 55 repeats the processing from step S102. On the other hand, when a predetermined time has elapsed since the previous current position was acquired (YES in step S104), CPU 55 executes the processing from step S106.

If the previous current position is stored in storage unit 57S (YES in step S106), CPU 55 performs resampling of particles (step S108). That is, the CPU 55 adds noise to the X coordinate X ₁ , Y coordinate Y ₁ , and angle (direction) θ ₁ of each particle stored in the particle file 57B, and the data X ₂ , Y ₂ , storing theta ₂ to the particle file 57B.

  And CPU55 is based on the data acquired from various sensors, for example, the measurement position obtained from the ultrasonic receiver 41, the measurement direction obtained from the gyro sensor 109, the measurement distance obtained from the ranging sensor 106, etc. Each particle is weighted by calculating the measurement probability (occurrence probability) of each particle (step S200). The weighting process (step S200) will be described later.

The CPU 55 multiplies the X-coordinate X ₂ , Y-coordinate Y ₂ , and angle θ ₂ after applying noise to each particle by the weight W _2, thereby obtaining a load average value (X, Y, θ of the X-coordinate, Y-coordinate, and angle θ). ) Is obtained (step S112).

  On the other hand, when the previous current position is not stored in the storage unit 57S (NO in step S106), the CPU 55 acquires the measurement position of the legged robot 100 based on the data from the ultrasonic receiver 41. (Step S114). And CPU55 performs the process from step S108, when a measurement position can be acquired (when it is YES in step S116), and when a measurement position cannot be acquired (when it is NO in step S116). Executes the processing from step S102.

<Weighting process>
FIG. 11 is a flowchart showing the processing procedure of the weighting process in the server device 50. As shown in FIG. 11, here, a case where the current position and the current direction are acquired by using M sensors and using N particles will be described.

  First, the CPU 55 calculates an observation probability for the first particle based on the data obtained by the first sensor (step S202) (steps S204 to S206). More specifically, for example, the CPU 55 acquires data indicating a measurement distance from the distance measuring sensor 106 (step S204), and the current position is determined from the X and Y coordinates of the target particle and the map data. The probability that the measurement distance is measured when the coordinates coincide with each other is calculated (step S206).

  The CPU 55 proceeds to the process for the next sensor (step S208). If there is a next sensor (NO in step S210), the process from step S204 is repeated. On the other hand, if there is no next sensor for each particle (YES in step S210), CPU 55 calculates the observation probability for the particle by multiplying all the observation probabilities for each particle (step S212).

  The CPU 55 proceeds to the process for the next particle (step S214). If there is a next particle (NO in step S216), the process from step S204 is repeated. On the other hand, if there is no next particle (YES in step S216), CPU 55 normalizes the observation probability related to each particle so that the total observation probability related to each particle becomes “1”. (Step S218).

[Other embodiments]
The program according to the present invention may be a program module that is provided as a part of a computer operating system (OS) and that calls necessary modules in a predetermined arrangement at a predetermined timing to execute processing. . In that case, the program itself does not include the module, and the process is executed in cooperation with the OS. A program that does not include such a module can also be included in the program according to the present invention.

  The program according to the present invention may be provided by being incorporated in a part of another program. Even in this case, the program itself does not include the module included in the other program, and the process is executed in cooperation with the other program. Such a program incorporated in another program can also be included in the program according to the present invention.

  The provided program product is installed in a program storage unit such as a hard disk and executed. Note that the program product includes the program itself and a storage medium in which the program is stored.

  Furthermore, part or all of the functions realized by the program according to the present invention may be configured by dedicated hardware.

  The embodiment disclosed this time should be considered as illustrative in all points and not restrictive. The scope of the present invention is defined by the terms of the claims, rather than the description above, and is intended to include any modifications within the scope and meaning equivalent to the terms of the claims.

1 is a schematic configuration diagram of a control system for a legged robot according to an embodiment of the present invention. It is a sequence diagram which shows the operation | movement outline | summary in the control system of the legged robot according to embodiment of this invention. It is a schematic diagram which shows the hardware constitutions of the control system of the legged robot according to embodiment of this invention. It is a schematic diagram which shows the hardware constitutions of the server apparatus according to embodiment of this invention. It is a block diagram which shows the function structure in the server apparatus according to embodiment of this invention. It is a block diagram which shows the function structure in the control system of the legged robot according to embodiment of this invention. It is an image figure which shows the content of map data. It is an image figure which shows the data structure of a particle file. It is an image figure which shows the data structure of a present information file. It is a flowchart which shows the process sequence of the present position acquisition process in a server apparatus. It is a flowchart which shows the process sequence of the weighting process in a server apparatus.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 Control system, 20 Microphone array environment server, 21 External microphone array, 30 Security server, 31 Security sensor, 40 Ultrasonic environment server, 41 Ultrasonic receiver, 49 Access point, 50 Server apparatus, 52 Display part, 53 Keyboard, 53A input unit, 54 mouse, 55A main processing unit, 55B position acquisition unit, 55C route tracking unit, 55D security unit, 55E voice recognition unit, 55F human detection unit, 55G face recognition unit, 55H voice reception unit, 55J command Receiving unit, 55K orientation data receiving unit, 55L particle acquisition unit, 55M setting unit, 55N calculation unit, 55P route generation unit, 55Q command generation unit, 56 memory, 57 fixed disk, 57A map data, 57B particle file, 57C current information file, 57S storage unit, 58 internal bus, 59 communication interface, 59A transmission / reception unit, 65 display unit interface, 100 legged robot, 101 head, 102 torso, 103 leg, 104 orientation sensor, 104A orientation acquisition , 105 Human sensor, 106 Distance sensor, 106A Distance acquisition unit, 107 Motor control board, 108 Actuator, 108A Moving mechanism, 109 Gyro sensor, 109 1 axis gyro sensor, 110 3 axis acceleration sensor, 111 Battery power supply, 112 Battery power source, 113 image transmission unit, 114 ultrasonic transmission tag, 114A transmission unit, 115 camera, 116 speaker, 117 microphone, 118 LAN hub, 119 wireless LAN slave unit, 119A transmission / reception unit, 1 21 Sensor control unit, 200 house.

Claims (7)

A moving body control system for controlling the operation of a moving body,
The moving body is
A moving mechanism for moving the moving body;
Orientation acquisition means for acquiring the measurement direction of the moving body;
A transmission means for transmitting at least one of a sound wave and a radio wave;
A distance acquisition means for acquiring a measurement distance to an obstacle around the moving body,
Storage means for storing map data indicating the position of the obstacle;
First position acquisition means for calculating a measurement position of the moving body based on at least one of the sound wave and radio wave;
A moving body control system comprising: a second position acquisition unit that calculates a current position of the moving body based on the measurement position, the measurement direction, the measurement distance, and the map data. The second position acquisition means includes
The moving body control system according to claim 1, wherein a current position of the moving body is calculated based on a previous current position by using a particle filter. The second position acquisition means includes
Particle acquisition means for acquiring a plurality of particles indicating the virtual position of the moving body;
Setting means for setting a weight for each of the particles based on the measurement position, the measurement distance, and the map data;
The moving body control system according to claim 2, further comprising calculation means for calculating a current position of the moving body based on the virtual position and the weight of each of the particles. The second position acquisition means includes
Particle acquisition means for acquiring a plurality of particles indicating the virtual position and virtual orientation of the moving body;
Setting means for setting a weight for each particle based on the measurement position, the measurement distance, the map data, and the measurement direction;
Calculation for calculating the current position of the moving object based on the virtual position and the weight of each particle, and calculating the current direction of the moving object based on the virtual direction and the weight of each particle The mobile body control system according to claim 2, comprising: means. Means for receiving the destination point;
Referring to the map data, route generating means for generating a moving route of the moving body based on the current position and the destination point;
An operation command generating means for generating an operation command including a moving direction and the number of steps based on the moving route and the current position;
5. The moving body control system according to claim 1, wherein the moving mechanism causes the moving body to follow the moving path by moving the moving body based on the operation command. 6. The moving body is a legged robot,
The said moving mechanism is a moving body control system of any one of Claim 1 to 5 containing the several leg part which can be walked. A mobile control method for controlling the operation of the mobile body in a server connectable to the mobile body,
The moving body is
A moving mechanism for moving the moving body;
Orientation acquisition means for acquiring the measurement direction of the moving body;
A transmission means for transmitting at least one of a sound wave and a radio wave;
A distance acquisition means for acquiring a measurement distance to an obstacle around the moving body,
The server device
A storage unit for storing map data indicating the position of the obstacle;
Including an arithmetic processing unit,
The moving body control method includes:
The arithmetic processing unit acquiring a measurement position of the moving body based on at least one of the sound wave and radio wave;
The arithmetic processing unit calculates a current position of the moving body based on the measurement position, the measurement direction, the measurement distance, and the map data;
The arithmetic processing unit accepting a destination point;
The arithmetic processing unit refers to the map data and generates a moving route of the moving body based on the current position and the destination point;
The arithmetic processing unit generates an operation command including a moving direction and the number of steps based on the moving route and the current position;
The moving mechanism includes a step of causing the moving body to follow the moving path by moving the moving body based on the operation command.

Images