JP5493097B2 - Robot self-localization system - Google Patents

Description

  The present invention relates to a robot self-position identification system, and more particularly to a robot self-position identification system that self-identifies a robot in a space where the robot exists.

  Non-Patent Document 1 discloses a technique for identifying a person by receiving a radio wave emitted by an RFID tag to which an individual ID is assigned and detecting the radio wave intensity to estimate the position of the person with the RFID tag. Has been.

Patent Document 1 discloses a position correction device that appropriately corrects position information detected by the position detection device. The position detection device uses GPS and is attached to a vehicle, for example. Then, the position correction device corrects the vehicle position information from the estimated vehicle speed and the vehicle speed estimated based on the GPS positioning.
Takayuki Kanda, Masahiro Shiomi, Tatsuya Nomura, Hiroshi Ishiguro, Norihiro Kajita, "Analysis of the movement trajectory of Science Museum visitors using RFID tags" (Information Processing Society Journal Vol.49 No.5 P.1727-1742) JP 2002-350157 A [G01C 21/00, G01S 5/14, G08G, 1/0969, G09B 29/00, G09B 29/10]

  In recent years, various robot self-position identification systems have been developed. As an example, a system for self-locating a robot by applying the technology of Non-Patent Document 1 and attaching an RFID tag to the robot has been developed.

  However, in the above-described system, an RFID tag must be attached to the robot, and an antenna for receiving the RFID tag must be provided in the environment. In addition, the position can be detected by using a GPS that is widely used in general, but the position detection accuracy of the GPS is low, and the position correction is performed using the technique disclosed in Patent Document 1. However, it cannot be used for the robot self-localization system.

  Therefore, the main object of the present invention is to provide a novel robot self-localization system.

  Another object of the present invention is to provide a robot self-position identification system capable of easily self-locating a robot.

  The present invention employs the following configuration in order to solve the above problems. The reference numerals in parentheses, supplementary explanations, and the like indicate the corresponding relationship with the embodiments described in order to help understanding of the present invention, and do not limit the present invention.

  A first invention is a robot self-position identification system that self-identifies a robot capable of outputting state data including at least its own position data regarding its own movement in a space in which the robot exists, and sequentially acquires from the robot State data storage means for storing a plurality of state data, an environmental sensor for sensing an entity existing in space, a position detection means for detecting the position of the entity based on the output of the environmental sensor, and an entity detected by the position detection means And associating the robot with the entity based on the position data storage means for storing the plurality of position data, the position data of the robot stored by the state data storage means, and the position data of the entity stored by the position data storage means. Based on the means and position data of the entity associated with the robot, The correction data for correcting the position data included in the status data, a transmitting means for transmitting to the robot, the robot, in accordance with correction data, for correcting its position data, a robot self-localization system.

  In the first invention, the robot self-position identification system (100) identifies the robot (18a, 18b, 18c) existing in the space (detection region F) within the space. Further, the robot can output state data including at least its own position data regarding its movement.

  The state data storage means (20, S9) stores the state data sequentially obtained from the robot, for example, in a buffer of the memory (22). The environmental sensor (12) is, for example, an LRF (Laser Range Finder) or the like, and senses an entity (Ea, Eb, Ec, Ed) such as a human being or a robot existing in the space. The position detection means (20, S89) detects the actual position of the output from the environmental sensor by using, for example, a particle filter. The position data storage means (20, S91) stores the detected entity position data in a memory buffer, for example, in the same manner as the state data storage means. The associating means (20, S49) associates the robot and the entity by, for example, obtaining a speed from the plurality of position data of the robot and the plurality of position data of the entity stored in the buffer. Then, the position data of the robot and the position data of the substance are indicated by values in the same plane coordinate system in the space.

  The transmission means (20, S15, S19) transmits, for example, coordinate data indicating the position of the space to the robot as correction data based on the position data of the entity associated with the robot. When the robot receives correction data, for example, it corrects its own position data.

  According to the first aspect of the present invention, it is possible to easily detect the current position of the robot by using the environmental sensor that can be easily installed and the state data that the robot can generally output. Therefore, by transmitting the detected position of the entity to the robot, it becomes possible to easily identify the position of the robot.

The second invention is dependent on the first invention, the state data further includes its own angle data, and the state data storage means stores the state data including the angle data, and is stored by the state data storage means. Based on a plurality of state data of the robot and a plurality of position data of the entity stored by the position data storage means, an angle estimation means for estimating an angle at the next position, and an angle and state data storage estimated by the angle estimation means Angle correction calculation means for calculating angle correction data for correcting the angle data included in the state data based on the angle data included in the state data stored by the means, and the transmission means includes the correction data and the angle correction data. and it includes a first transmission means for transmitting to the robot, the robot follow the correction data and the angle correction data Te, it corrects the position data and angular data itself.

In the second invention, the state data storage means stores state data further including angle data of the robot itself. The angle estimation means (20, S261-S277) estimates the angle of the robot at the next position, for example, by predicting the position of the robot from a plurality of position data of the entity and a plurality of state data of the robot. The angle correction calculation means (20, S279) calculates, for example, an angle difference between the angle data included in the current state data and the estimated angle (θ ^ k) as angle correction data. First transmission means (20, S19) the corrected data and the angle correction data, and transmits to the robot.

  According to the second invention, the accuracy of self-position identification can be improved by correcting the angle of the robot.

  A third invention is dependent on the second invention, and the angle estimation means further includes a storage means for calculating the estimated angle error when the angle is estimated, and storing the estimated angle error as a correction accuracy, The transmission unit includes a second transmission unit that transmits only the correction data to the robot when the correction accuracy stored by the storage unit is low.

In the third invention, the angle estimation means estimates the angle and calculates an estimated angle error (σ ² _{θ ^ k} ). The storage means (20, S281) stores the estimated angle error in the buffer as the correction accuracy. If the estimated angle error is large, the second transmission means (20, S15) transmits only the correction data to the robot.

  According to the third aspect, it is possible to determine whether the angle of the robot can be corrected by using the estimated angle error as the correction accuracy.

  The fourth invention is dependent on the third invention, and the angle estimation means is based on the plurality of state data of the robot stored by the state data storage means and the position data of the entity stored by the position data storage means, An angle observation means for observing the angle of the robot, and an observation angle error calculation means for calculating an observation angle error of the angle observed by the angle observation means. The angle estimation means includes an observed angle, an observation angle error, and a previous estimation. Based on the calculated angle and the previously calculated estimated angle error, the angle of the next position is estimated, and the estimated angle error is calculated.

In the fourth invention, for example, the angle observation means (20, S261-S265) calculates a predicted angle by predicting the next position from a plurality of state data, and calculates a detection angle from the actual position data. The angle observation means observes (calculates) the angle of the robot by subtracting the predicted angle from the detected angle. The observation angle error calculation means (20, S267) calculates the observation angle error by approximating the quotient of the actual position data with respect to the position error (σ ² _po ) in the observation, for example.

For example, the angle estimation means obtains the angle of the next position by obtaining the Kalman gain (K _k ) based on the observed angle, the observed angle error, the previously estimated angle, and the previously calculated estimated angle error. Estimate and calculate the estimated angle error.

  According to the fourth invention, the angle of the next position can be estimated using the Kalman filter.

  The fifth invention is dependent on any one of the first to fourth inventions, and when it is judged by the judging means for judging whether to maintain the association between the robot and the entity, and when not judged to be maintained by the judging means, Release means for releasing the association between the robot and the entity is further provided.

  In the fifth invention, the determination means (20, S117, S119, S121) determines whether to maintain the association based on, for example, the speed difference or distance between the robot and the entity. The canceling means (20, S125) cancels the association between the robot and the entity when, for example, the speed difference between the robot and the entity is large or the distance between the robot and the entity is long.

  According to the fifth aspect, the association between the robot and the entity that causes the error can be canceled.

  The sixth invention is dependent on the fifth invention, and there are at least two or more entities in the space, and the associating means is within a predetermined time after the association between the robot and the entities is released, First speed difference calculating means for calculating a first speed difference between the robot and each entity, and first speed determining means for determining whether the first speed difference calculated by the first speed difference calculating means is greater than a first predetermined speed. Distance determining means for determining whether the distance between the robot and each entity is greater than a predetermined distance; and the first speed determining means determines that the first speed difference with the robot is equal to or less than the first predetermined speed; and distance determining means The first associating means for associating the robot with the entity closest to the robot among the entities determined to be within the predetermined distance by the robot.

  In the sixth invention, there may be, for example, four entities in the space. The first speed difference calculation means (20, S193) calculates the first time difference from the position data of the robot for 5 seconds and the position data of the entity for 5 seconds, for example, within a predetermined time (5 seconds) after the association is released. One speed difference (RMS (Root Mean Square) speed difference) is calculated. The first speed determining means (20, S195) determines whether the first speed difference is greater than the first predetermined speed. The distance determining means (20, S199) obtains a distance from the position data of the robot and the position data of the entity, and determines whether the robot and each entity are separated from the predetermined distance. For example, the first association means (20, S209) is closest to the robot when the first speed difference from the robot is equal to or less than the first predetermined speed and there are a plurality of entities whose distance from the robot is equal to or less than the predetermined distance. Associate an entity with a robot.

  According to the sixth aspect of the present invention, it is possible to easily reassociate the robot and the entity as soon as the association is released.

  The seventh invention is dependent on the fifth invention or the sixth invention, and there are at least two or more entities in the space, and the associating means is predetermined after the association between the robot and the entities is released. When the time has elapsed, a second speed difference calculating means for calculating a second speed difference between the robot and each entity, and determining whether the second speed difference calculated by the second speed difference calculating means is greater than a second predetermined speed The second speed determining means, and when there is only one entity for which the second speed difference of the robot is determined to be equal to or less than the second predetermined speed by the second speed determining means, a second association for associating the entity with the robot Including means.

  In the seventh invention, as in the sixth invention, there may be four entities in the space. For example, the second speed difference calculation means (20, S233) determines that the first time from the position data of the robot for 300 seconds and the position data of the entity for 300 seconds if the predetermined time has passed since the association was released. Two speed difference (RMS speed difference) is calculated. The second speed determining means (20, S235) determines whether the second speed difference is greater than the second predetermined speed. For example, if there is only one entity whose second speed difference with the robot is equal to or less than the second predetermined speed, the second association means (20, S247) associates the entity with the robot.

  According to the seventh invention, even if the association is canceled and a predetermined time has elapsed, the robot and the entity can be associated again.

  According to the present invention, since the position of the robot can be detected by using the environmental sensor that can be easily installed and the state data that the robot can generally output, it is possible to easily identify the position of the robot. it can.

  The above object, other objects, features, and advantages of the present invention will become more apparent from the following detailed description of embodiments with reference to the drawings.

FIG. 1 is an illustrative view showing an outline of a robot self-position identification system according to an embodiment of the present invention. FIG. 2 is an illustrative view showing one example of an electrical configuration of the tracking server shown in FIG. FIG. 3 is an illustrative view showing the appearance of the robot shown in FIG. 1 from the front. FIG. 4 is a block diagram showing an example of the electrical configuration of the robot shown in FIG. FIG. 5 is an illustrative view showing a measurement region of the LRF shown in FIGS. 1 and 2. FIG. 6 is an illustrative view showing one example of a moving locus of an entity acquired using the LRF shown in FIGS. 1 and 2. FIG. 7 is an illustrative view showing one example of a configuration of entity position history data stored in the memory shown in FIG. FIG. 8 is an illustrative view showing one example of a configuration of status history data of the robot stored in the memory shown in FIG. FIG. 9 is an illustrative view showing one example of RTM (Robot Tracking Model) data stored in the memory of the tracking server shown in FIG. FIG. 10 is a graph showing changes in the movement history of the entity shown in FIG. 6 and the movement history of the robot shown in FIG. FIG. 11 is a graph of the RMS error of the movement history of the entity and the movement history of the robot shown in FIG. FIG. 12 is an illustrative view showing one example of a configuration of a tracking entity list temporarily stored in the memory shown in FIG. FIG. 13 is an illustrative view showing one example of a configuration of an association table stored in the memory shown in FIG. FIG. 14 is an illustrative view showing a relationship between numerical values used in an angle correction process executed by the processor shown in FIG. FIG. 15 is an illustrative view showing one example of association and cancellation of association between the human and the robot shown in FIG. FIG. 16 is an illustrative view showing one example of a memory map of the memory shown in FIG. FIG. 17 is an illustrative view showing one example of a data storage area shown in FIG. FIG. 18 is a flowchart showing a part of communication processing of the processor of the tracking server shown in FIG. FIG. 19 is a part of communication processing of the processor of the tracking server shown in FIG. 1, and is a flowchart subsequent to FIG. FIG. 20 is a flowchart showing correction processing of the processor of the tracking server shown in FIG. FIG. 21 is a flowchart showing a part of the tracking process of the processor of the tracking server shown in FIG. FIG. 22 is a part of the tracking process of the processor of the tracking server shown in FIG. 1, and is a flowchart subsequent to FIG. FIG. 23 is a flowchart showing the association release processing of the processor of the tracking server shown in FIG. FIG. 24 is a flowchart showing the association process of the processor of the tracking server shown in FIG. FIG. 25 is a flowchart showing local association processing of the processor of the tracking server shown in FIG. FIG. 26 is a flowchart showing the global association processing of the processor of the tracking server shown in FIG. FIG. 27 is a flowchart showing an angle correction process of the processor of the tracking server shown in FIG.

  Referring to FIG. 1, a robot self-position identification system 100 according to this embodiment includes, for example, autonomously moving robots (hereinafter simply referred to as “robots”) 18a, 18b, and 18c in a space where these robots exist. It is a system for self-localization (also called Localization). In the present embodiment, “self-position identification” refers to the robots 18a, 18b, and 18c (hereinafter referred to as “robot 18” if they are not distinguished from each other) their own position, angle, and speed in the space. Is acquired using its own sensors and environmental sensors.

  The robot self-position identification system 100 includes a tracking server 10, a remote operation device 14, a planning server 16, and a robot 18. The robot 18 is connected to the tracking server 10, the remote operation device 14, and the planning server 16 via the network 400.

  The tracking server 10 includes a plurality of LRFs including LRFs 12a and 12b, which are environment sensors. The LRFs 12a and 12b are installed in a place (environment) where the human and the robot 18 can act. Then, the tracking server 10 detects the position of each entity by sensing the entities including the robot 18 and the human using the LRFs 12a and 12b. For example, the place where the human and the robot 18 can act is a company floor, a museum, a shopping mall, or an attraction venue. Note that the system administrator can easily install the LRFs 12a and 12b in various places. Further, the position data obtained by the tracking server 10 is output to the remote operation server 14 and the planning server 16.

  In addition, the tracking server 10 has a localization function (sometimes referred to as a localization module). Therefore, the tracking server 10 establishes communication with the robot 18, receives (acquires) state data output from the robot 18, and receives correction data for correcting the position and angle (orientation) with respect to the robot 18. Send. The state data includes position data, angle data, and velocity data of the robot 18 itself, and is created based on the output of the sensor provided in the robot 18.

  The remote operation device 14 includes an operation terminal that displays position data obtained from the tracking server 10 and is operated by an operator. The operator can simultaneously monitor a plurality of robots 18a, 18b, and 18c based on the robot 18 displayed on the operation terminal and the position of the person. Then, the operator remotely operates the robot 18 or issues an operation command as necessary. Further, the plan server 16 uses the position data obtained from the tracking server 10 to calculate a route so that the robot 18 performs a communication action with a person existing in the space.

  The robot 18 is also an interaction-oriented robot, and has a function of executing a communication action including at least one of body movements such as gestures and voices with a communication target such as a human. Therefore, the robot 18 approaches a human or performs a communication action based on a command from the remote control device 14 or the plan server 16.

  The robot 18 outputs the above-described state data every 0.1 to 0.3 seconds. Furthermore, each robot 18 is identified (differentiated) by a robot ID configured by a character string.

  Here, for simplicity, only one human and three robots 18 are shown, but the tracking server 10 can simultaneously detect two or more humans and three or more robots 18. Further, the connection between the tracking server 10, the remote control device 14, and the planning server 16 and the network 400 may be a wireless connection or a wired connection. Further, the connection between each server and the device may be a wired connection or a wireless connection.

  FIG. 2 is a block diagram showing an electrical configuration of the tracking server 10. Referring to FIG. 2, the tracking server 10 includes LRFs 12a-12f and a processor 20. The processor 20 includes an RTC (Real Time Clock) 20a. The detected position data is associated with the time data obtained from the RTC 20a and stored in the buffer of the memory 22.

  The processor 20 is also called a microcomputer or CPU, and is connected to the LRF 12c, LRF 12d, LRF 12e, and LRF 12f in addition to the LRF 12a and LRF 12b described above. Further, the processor 20 is also connected to a memory 22, a communication LAN board 24, a wireless communication device 26, and an external output 28. In addition, when it is not necessary to distinguish LRF12a-12f, it is collectively called "LRF12."

  The LRF 12 measures the distance to the object from the time it takes to irradiate the laser, reflect on the object (including the entity) and return. For example, a laser beam emitted from a transmitter (not shown) is reflected by a rotating mirror (not shown), and the front is scanned in a fan shape by a certain angle (for example, 0.5 degrees). Here, as the LRF 12, a laser range finder (model LMS200) manufactured by SICK can be used. When this laser range finder is used, a distance of 8 m can be measured with an error of about ± 15 mm.

  Although not shown, the memory 22 includes a ROM, an HDD, and a RAM, and a control program for controlling the operation of the tracking server 10 is stored in advance in the ROM and the HDD. For example, a program necessary for human detection by the LRF 12 is recorded. The RAM is used as a work memory or a buffer memory.

  The communication LAN board 24 is composed of, for example, a DSP, and sends transmission data given from the processor 20 to the wireless communication device 26, and the wireless communication device 26 sends the transmission data to the robot 18 via the network 400. The transmission data is correction data or the like. On the other hand, the communication LAN board 24 receives data via the wireless communication device 26 and gives the received data to the processor 20. The received data is status data output by the robot 18. The received status data is stored in the buffer of the memory 22.

  The external connection 28 is a port for connecting to the remote control device 14 and the planning server 16, and is connected to a wired cable such as a LAN cable, a SCSI cable, or a USB cable. Then, the processor 20 gives the actual position data to the external output 28, and the external output 28 outputs the position data to the remote control device 14 and the plan server 16.

  FIG. 3 is a front view showing the appearance of the robot 18 of this embodiment. Referring to FIG. 3, the robot 18 includes a carriage 30, and the lower surface of the carriage 30 has a right wheel 32 a and a left wheel 32 b (referred to as “wheel 32” unless otherwise specified) for autonomously moving the robot 18, and One follower wheel 34 is provided. The right wheel 32a and the left wheel 32b are independently driven by the right wheel motor 36a and the left wheel motor 36b (see FIG. 4), respectively, and can move the carriage 30, that is, the robot 18 in any direction of front, rear, left and right. The slave wheel 34 is an auxiliary wheel that assists the wheel 32. Therefore, the robot 18 can move in the arranged space by autonomous control.

  A cylindrical sensor attachment panel 38 is provided on the carriage 30, and a large number of infrared distance sensors 40 are attached to the sensor attachment panel 38. These infrared distance sensors 40 measure the distance from the sensor mounting panel 38, that is, the object (human being, obstacle, etc.) around the robot 18.

  In this embodiment, an infrared distance sensor is used as the distance sensor, but a small LRF, an ultrasonic distance sensor, a millimeter wave radar, or the like can be used instead of the infrared distance sensor.

  A body 42 is provided on the sensor mounting panel 38 so as to stand upright. Further, the above-described infrared distance sensor 40 is further provided in the upper front upper part of the body 42 (a position corresponding to a person's chest), and measures the distance mainly to a person in front of the robot 18. Further, the body 42 is provided with a support column 44 extending from substantially the center of the upper end of the side surface, and an omnidirectional camera 46 is provided on the support column 44. The omnidirectional camera 46 photographs the surroundings of the robot 18 and is distinguished from an eye camera 70 described later. As this omnidirectional camera 46, for example, a camera using a solid-state imaging device such as a CCD or a CMOS can be adopted. In addition, the installation positions of the infrared distance sensor 40 and the omnidirectional camera 46 are not limited to the portions, and may be changed as appropriate.

  An upper arm 50R and an upper arm 50L are provided at upper end portions on both sides of the torso 42 (position corresponding to a human shoulder) by a shoulder joint 48R and a shoulder joint 48L, respectively. Although illustration is omitted, each of the shoulder joint 48R and the shoulder joint 48L has three orthogonal degrees of freedom. That is, the shoulder joint 48R can control the angle of the upper arm 50R around each of three orthogonal axes. A certain axis (yaw axis) of the shoulder joint 48R is an axis parallel to the longitudinal direction (or axis) of the upper arm 50R, and the other two axes (pitch axis and roll axis) are orthogonal to the axes from different directions. It is an axis to do. Similarly, the shoulder joint 48L can control the angle of the upper arm 50L around each of three orthogonal axes. A certain axis (yaw axis) of the shoulder joint 48L is an axis parallel to the longitudinal direction (or axis) of the upper arm 50L, and the other two axes (pitch axis and roll axis) are orthogonal to the axes from different directions. It is an axis to do.

  In addition, an elbow joint 52R and an elbow joint 52L are provided at the respective distal ends of the upper arm 50R and the upper arm 50L. Although illustration is omitted, each of the elbow joint 52R and the elbow joint 52L has one degree of freedom, and the angle of the forearm 54R and the forearm 54L can be controlled around the axis (pitch axis).

  A sphere 56R and a sphere 56L corresponding to a human hand are fixedly provided at the tips of the forearm 54R and the forearm 54L, respectively. However, when a finger or palm function is required, a “hand” in the shape of a human hand can be used. Although not shown, the front surface of the carriage 30, the portion corresponding to the shoulder including the shoulder joint 48R and the shoulder joint 48L, the upper arm 50R, the upper arm 50L, the forearm 54R, the forearm 54L, the sphere 56R, and the sphere 56L, , A contact sensor 58 (shown generically in FIG. 4) is provided. A contact sensor 58 on the front surface of the carriage 30 detects contact of a person or another obstacle with the carriage 30. Therefore, when the robot 18 is in contact with an obstacle during its movement, the robot 18 can detect the contact and immediately stop driving the wheels 32 to suddenly stop the movement of the robot 18. Further, the other contact sensors 58 detect whether or not the respective parts are touched. In addition, the installation position of the contact sensor 58 is not limited to the said site | part, and may be provided in an appropriate position (position corresponding to a person's chest, abdomen, side, back, and waist).

  A neck joint 60 is provided at the upper center of the body 42 (a position corresponding to a person's neck), and a head 62 is further provided thereon. Although illustration is omitted, the neck joint 60 has a degree of freedom of three axes, and the angle can be controlled around each of the three axes. A certain axis (yaw axis) is an axis directed directly above the robot 18 (vertically upward), and the other two axes (pitch axis and roll axis) are axes orthogonal to each other in different directions.

  The head 62 is provided with a speaker 64 at a position corresponding to a human mouth. The speaker 64 is used for the robot 18 to communicate with a person around it by voice or sound. A microphone 66R and a microphone 66L are provided at a position corresponding to a human ear. Hereinafter, the right microphone 66R and the left microphone 66L may be collectively referred to as a microphone 66. The microphone 66 captures ambient sounds, in particular, the voices of humans who are subjects of communication. Furthermore, an eyeball part 68R and an eyeball part 68L are provided at positions corresponding to human eyes. The eyeball portion 68R and the eyeball portion 68L include an eye camera 70R and an eye camera 70L, respectively. Hereinafter, the right eyeball portion 68R and the left eyeball portion 68L may be collectively referred to as the eyeball portion 68. Further, the right eye camera 70R and the left eye camera 70L may be collectively referred to as an eye camera 70.

  The eye camera 70 captures a human face approaching the robot 18 and other parts or objects, and captures a corresponding video signal. The eye camera 70 can be the same camera as the omnidirectional camera 46 described above. For example, the eye camera 70 is fixed in the eyeball unit 68, and the eyeball unit 68 is attached to a predetermined position in the head 62 via an eyeball support unit (not shown). Although illustration is omitted, the eyeball support portion has two degrees of freedom, and the angle can be controlled around each of these axes. For example, one of the two axes is an axis (yaw axis) in a direction toward the top of the head 62, and the other is orthogonal to the one axis and goes straight in a direction in which the front side (face) of the head 62 faces. It is an axis (pitch axis) in the direction to be performed. By rotating the eyeball support portion around each of these two axes, the tip (front) side of the eyeball portion 68 or the eye camera 70 is displaced, and the camera axis, that is, the line-of-sight direction is moved. Note that the installation positions of the speaker 64, the microphone 66, and the eye camera 70 described above are not limited to those portions, and may be provided at appropriate positions.

  As described above, the robot 18 of this embodiment has independent two-axis driving of the wheels 32, three degrees of freedom of the shoulder joint 48 (6 degrees of freedom on the left and right), one degree of freedom of the elbow joint 52 (2 degrees of freedom on the left and right), It has a total of 17 degrees of freedom, 3 degrees of freedom for the neck joint 60 and 2 degrees of freedom for the eyeball support (4 degrees of freedom on the left and right).

  FIG. 4 is a block diagram showing an electrical configuration of the robot 18. Referring to FIG. 4, robot 18 includes a processor 80. The processor 80, also called a microcomputer or CPU, is connected to the memory 84, the motor control board 86, the sensor input / output board 88, and the audio input / output board 90 via the bus 82. The processor 80 includes an RTC 80a and associates time data with the created state data.

  Although not shown, the memory 84 includes an SSD (Solid State Drive), a ROM, and a RAM. In the SSD and ROM, a control program for controlling the operation of the robot 18 is stored in advance. For example, a detection program for detecting the output (sensor information) of each sensor and communication for transmitting / receiving necessary data and commands to / from external computers (the tracking server 10, the remote control device 14, and the planning server 16) Programs etc. are recorded. The RAM is used as a work memory or a buffer memory.

  The motor control board 86 is composed of, for example, a DSP, and controls driving of each axis motor such as each arm, neck joint, and eyeball. That is, the motor control board 86 receives the control data from the processor 80, and shows two motors that control the respective angles of the two axes of the right eyeball portion 68R (in FIG. 4, collectively referred to as “right eyeball motor 92”). ) To control the rotation angle. Similarly, the motor control board 86 receives two control data from the processor 80 and controls two angles of the two axes of the left eyeball portion 68L (in FIG. 4, collectively referred to as “left eyeball motor 94”). .) Control the rotation angle.

  The motor control board 86 receives the control data from the processor 80, and includes a total of three motors that control the angles of the three orthogonal axes of the shoulder joint 48R and one motor that controls the angle of the elbow joint 52R. The rotation angle of four motors (collectively indicated as “right arm motor 96” in FIG. 4) is controlled. Similarly, the motor control board 86 receives control data from the processor 80, and includes three motors for controlling the angles of the three orthogonal axes of the shoulder joint 48L and one motor for controlling the angle of the elbow joint 52L. The rotation angles of a total of four motors (collectively indicated as “left arm motor 98” in FIG. 4) are controlled.

  Further, the motor control board 86 receives the control data from the processor 80 and controls three motors for controlling the respective angles of the three orthogonal axes of the neck joint 60 (in FIG. 4, collectively referred to as “head motor 110”). .) Control the rotation angle. The motor control board 86 receives the control data from the processor 80 and individually controls the rotation angles of the right wheel motor 36a for driving the right wheel 32a and the left wheel motor 36b for driving the left wheel 32b. In this embodiment, a motor other than the wheel motor 36 uses a stepping motor (that is, a pulse motor) in order to simplify the control. However, a DC motor may be used similarly to the wheel motor 36. The actuator that drives the body part of the robot 18 is not limited to a motor that uses a current as a power source, and may be changed as appropriate. For example, in another embodiment, an air actuator or the like may be applied.

  Similar to the motor control board 86, the sensor input / output board 88 is constituted by a DSP, and takes in signals from each sensor and gives them to the processor 80. That is, data relating to the reflection time from each of the infrared distance sensors 40 is input to the processor 80 through the sensor input / output board 88. Further, the video signal from the omnidirectional camera 46 is input to the processor 80 after being subjected to predetermined processing by the sensor input / output board 88 as required. Similarly, the video signal from the eye camera 70 is also input to the processor 80. Further, signals from the plurality of contact sensors 58 described above (collectively indicated as “contact sensors 58” in FIG. 4) are provided to the processor 80 via the sensor input / output board 88.

  The sensor input / output board 88 is further connected to a right wheel speed sensor 112a for detecting the rotational speed (wheel speed) of the right wheel 32a and a left wheel speed sensor 112b for detecting the rotational speed of the left wheel 32b. Then, data relating to the rotation speeds (wheel speeds) of the right wheel 32a and the left wheel 32b at a predetermined time from the right wheel speed sensor 112a and the left wheel speed sensor 112b are sent to the processor 80 through the sensor input / output boat 88, respectively. Entered.

  The voice input / output board 90 is also composed of a DSP like the other input / output boards, and voice or voice according to voice synthesis data given from the processor 80 is outputted from the speaker 64. Also, audio input from the microphone 66 is given to the processor 80 via the audio input / output board 90.

  The processor 80 is connected to the communication LAN board 114 via the bus 82. The communication LAN board 114 is composed of, for example, a DSP, and provides transmission data given from the processor 80 to the wireless communication device 116. The wireless communication device 116 sends the transmission data to an external computer (the tracking server 10, remote control) via the network 400. It transmits to the operating device 14 and the plan server 16). The communication LAN board 114 receives data via the wireless communication device 116 and gives the received data to the processor 80. For example, the transmission data may be surrounding video data taken by the omnidirectional camera 46 and the eye camera 70, or status data.

  Here, the position data included in the state data is obtained based on the rotation speed of the wheel 32 output from the right wheel speed sensor 112a and the left wheel speed sensor 112b. The angle data included in the state data is obtained based on the rotation ratio of the right wheel speed sensor 112a and the left wheel speed sensor 112b. Further, the speed data included in the state data includes wheel speed data output from the right wheel speed sensor 112a and wheel speed data output from the left wheel speed sensor 112b. That is, the robot 18 transmits the wheel speed data of the right wheel 32a and the wheel speed data of the left wheel 32b to the tracking server 10 as speed data included in the state data. The tracking server 10 calculates the speed of the robot 18 based on the received left and right wheel speed data.

  Next, the LRF 12 will be described in detail. Referring to FIG. 5, the measurement range of LRF 12 is indicated by a semicircular shape (fan shape) having a radius R (R≈8 m). That is, the LRF 12 can measure the direction of 90 ° left and right within a predetermined distance (R) when the front direction is the center.

  The laser used is a class 1 laser in Japanese Industrial Standard JIS C 6802 “Safety Standard for Laser Products”, which is a safe level that does not affect human eyes. In this embodiment, the sampling rate of the LRF 12 is 37 Hz. This is to continuously detect the position of the entity that moves or stops.

  Further, as described above, the LRF 12 is arranged in various places. Specifically, each of LRF12a-12f is arrange | positioned so that a measurement area | region may overlap, Although illustration is abbreviate | omitted, it is fixed to the height of about 90 cm from a floor surface. This height is for detecting the human torso and arms (both arms), and is calculated from the average height of a Japanese adult, for example. Therefore, the height at which the LRF 12 is fixed may be appropriately changed according to the location (region or country) where the remote control device 14 is provided and the age or age of the person (for example, children or adults). In this embodiment, six LRFs 12 are set, but any number of LRFs 12 may be installed as long as the number is two or more.

  In the tracking server 10 having such a configuration, the processor 20 estimates changes in the current positions of the human and the robot 18 using the particle filter based on the output (distance data) from the LRF 12. In this embodiment, since the human and the robot 18 have different speeds and shapes, a human shape model (human shape model) or a robot shape model (robot shape model) is adopted, and the likelihood of the particle filter is determined. The amount of calculation is reduced.

  For example, when scanned by the LRF 12, a visible region (open) where an entity such as a human does not exist, a shadow region (shadow) where the entity exists, and an edge of the entity are detected.

Here, a variable that identifies a uniformly rose sprinkle particles in space and m, particle state (position and velocity) and X, the observed value vector that is observed in the state Xt certain time t and Z _t Then, the likelihood p _i (Z _t | X _t [m]) of each RLF 12 is calculated according to Equation 1. However, [m] is a subscript for individually identifying particles. In addition, for the convenience of expression, [m] is described without superscript except for mathematical expressions. The same applies hereinafter.

[Equation 1]

That is, the likelihood p _i (Z _t | X _t [m]) is a constant value (p _open ) in the visible region, and the constant value (p _shadow ) and the edge likelihood p _edge (Z _t | X) in the shadow region. _t [m]).

Further, when the number of LRFs 12 is n _sensors , the integrated likelihood p (Z _t | X _t [m]) obtained by integrating the likelihoods of the respective LRFs 12 obtained by Equation 1 is calculated according to Equation 2. Since there may be two people in the shadow region, in order to prevent two particles from tracking the same entity, from the likelihood p _i (Z _t | X _t [m]) of each RLF 12 The correction value P _collocation is subtracted.

[Equation 2]

Based on the integrated likelihood p (Z _t | X _t [m]) obtained in this way, each particle is updated, and a change in the current position of the human or the robot 18 is estimated. Then, based on the estimated change in the current position, position data of the robot 18 or human is obtained, and the position data is stored in the buffer. In this embodiment, all the position data stored in the buffer are collectively referred to as “position history data”.

  Details of person tracking using a particle filter are disclosed in Japanese Patent Laid-Open No. 2008-6105.

  FIG. 6 is an illustrative view showing a map of a certain environment where the LRFs 12a to 12f are installed. Referring to FIG. 6, the place represented by the map is a shopping mall. The three LRFs 12a, 12c, and 12d are installed corresponding to the upper position of the map, and the three LRFs 12b, 12e, and 12f are set corresponding to the lower position of the map. And the area | region where the measurement area | region of two or more LRF12 overlaps is shown as the detection area | region F, and is a hatched area | region in FIG. The position in the detection area F is indicated by a plane coordinate system with the lower left in FIG. 6 as the origin.

  In the detection area F (space), the robot 18a, the robot 18b, the robot 18c, and the human are detected as an entity Ea, an entity Eb, an entity Ec, and an entity Ed (referred to as “entity E” if not distinguished). Further, in FIG. 6, the movement locus M is shown based on the position history data corresponding to each entity E. That is, the position history data of the entity Ea is indicated by the movement locus Ma, the position history data of the entity Eb is indicated by the movement locus Mb, the position history data of the entity Ec is indicated by the movement locus Mc, and the position history data of the entity Ed. Is indicated by a movement trajectory Md.

  FIG. 7 is an illustrative view showing a plurality of position data detected by the tracking server 10, that is, position history data. Referring to FIG. 7, each position data includes a time (T) when the position is detected and a numerical value (X, Y) indicating the plane coordinates of detection area F. Each position history data is stored separately for each entity E. For example, the location history data of the entity Ea, the location history data of the entity Eb, the location history data of the entity Ec, and the location history data of the entity Ed are stored in the buffer of the memory 22 of the tracking server 10.

  FIG. 8 is an illustrative view showing a plurality of state data received by the tracking server 10, that is, state history data. Referring to FIG. 8, each state data includes received time (t), numerical values (x, y) indicating plane coordinates in detection area F, angle (d) indicating the direction of robot 18, and the right wheel of robot 18. It is comprised from the speed (rv) of 32a and the speed (lv) of the left wheel 32b of the robot 18. As with the position history data, each state history data is stored separately for each robot 18. For example, the state history data of the robot 18a, the state history data of the robot 18b, and the state history data of the robot 18c are stored in the buffer of the memory 22.

  Note that the position data and the state data are deleted (overwritten) from the old data after a certain period of time (for example, 360 seconds). That is, the position history data and the state history data do not press the memory capacity of the memory 22.

  In the numerical values (X, Y) and (x, y) indicating the plane coordinates described above, a change in one coordinate corresponds to a distance change of 1 cm. For example, if the position data of the entity E is (X, Y) and the entity E moves 1 cm to the right, the coordinates of the entity E change to (X + 1, Y).

  FIG. 9 is an illustrative view of RTM data composed of the RTM of each robot 18. In this embodiment, when the tracking server 10 establishes communication with each robot 18 in order to receive state data, an RTM is created for each robot 18. Then, correction data transmitted to each robot 18 is determined based on this RTM. Thus, each RTM includes the current robot 18 position, angle and velocity values.

Referring to FIG. 9, RMT data is stored in association with the robot ID assigned to each robot 18. For example, in the RTM of the robot 18a whose robot ID is “001”, (RX ₁ , RY ₁ ) indicates a position, RD ₁ indicates an angle, and RV ₁ indicates a speed.

Here, when the tracking server 10 receives the state data from the robot 18, the position, angle and speed of the RTM corresponding to the robot 18 are updated. For example, in the state data received from the robot 18a, if the position is (x ₁ , y ₁ ), the angle is d ₁ , and the velocities are rv ₁ and lv ₁ , then in the RTM with the robot ID “001”, (RX ₁ , RY ₁ ), RD ₁ , RV ₁ are updated to the values of the received status data. The speed RV ₁ is updated to a speed calculated based on rv ₁ and rv ₁ .

  However, the wheel 32 of the robot 18 slips slightly with respect to the ground each time the robot 18 stops or bends, so there is an error between the position and angle determined from the rotation of the wheel 32 and the actual position and angle. Occurs. Since the error increases as the robot 18 moves, the reliability of the position data and angle data included in the state data decreases as the robot 18 moves. Therefore, if the RTM for the robot 18 is updated based only on the state data output by the robot 18, the RTM cannot indicate the correct state (position, angle, and speed) of the robot 18.

  Therefore, in this embodiment, the entity E detected in the detection region F is associated with the robot 18 moving in the detection region F, and the position data of the entity E detected using the LRF 12 and the associated robot 18 are detected. The RTM is updated based on the state data. As a result, each RTM can indicate the correct state of the robot 18.

  First, the association between the robot 18 and the entity E will be described. In the present invention, attention is paid to the moving distance per second of the entity E and the robot 18, that is, the speed. Note that the speed of each entity E and the robot 18a is obtained by obtaining a distance (mm) from a coordinate at a certain time and a coordinate after one second based on the three-square theorem. be able to.

  The exercise history graph of FIG. 10 is a graph in which distance (mm) is taken on the vertical axis and time (seconds: s) is taken on the horizontal axis. The exercise history graph shows a change in the speed of the entity Ea-entity Ed for 15 seconds and a change in the speed of the robot 18a. In this movement history graph, graphs corresponding to the speed of the entity Ea, the entity Eb, the entity Ec, and the entity Ed are indicated by a solid line La, a solid line Lb, a solid line Lc, and a solid line Ld. A graph corresponding to the speed of the robot 18a is indicated by a dotted line DL.

  For example, from the motion history graph, changes in dotted line DL (speed of robot 18a) are changes in solid line La (speed of entity Ea), solid line Lc (speed of entity Ea), and solid line Ld (speed of entity Ea). Although different, it can be seen that the change in the solid line Lb (the speed of the entity Eb) is similar.

  Therefore, when an RMS (Root Mean Square) error between the speed of the robot 18a and the speed of each entity E is obtained based on the following equation (3), an RMS error graph shown in FIG. 11 can be obtained. it can. Note that the RMS error in this embodiment may be referred to as an RMS speed difference.

[Equation 3]

  Referring to FIG. 11, the RMS error graph is a graph in which the vertical axis represents the RMS error and the horizontal axis represents the entity ID, and each RMS error is calculated with RT of Equation 3 as 15 (seconds). First, in the RMS error graph, focusing on the RMS errors of the entity Ea, the entity Ec, and the entity Ed, the RMS error between the entity Ea having the entity ID “001” and the robot 18a is 330 mm. The RMS error between the entity Ec with the entity ID “003” and the robot 18a is 500 mm, and the RMS error between the entity Ed with the entity ID “004” and the robot 18a is 600 mm. On the other hand, the RMS error between the entity Eb having the entity ID “002” and the robot 18a is 30 mm. That is, it can be seen that the RMS error between the robot 18a and the entity Eb having similar speed changes is smaller than the RMS error between the other entities Ea, Ec, and Ed.

  Therefore, in this embodiment, an RMS error, that is, an RMS speed difference (first speed difference) is 350 mm / s (first predetermined speed) or less and a current speed difference is 350 mm / If there is only one entity E below s, a certain robot 18 is associated with that entity E.

  For example, referring to FIG. 11, in robot 18a, entity E having an RMS speed difference of 350 mm / s or less is entity Ea and entity Eb. Furthermore, with reference to FIG. 10, when the time of 15 seconds is considered as the current speed, the entity E whose current speed difference is 350 mm / s or less is only the entity Eb. Therefore, in such a case, the robot 18a and the entity Eb are associated with each other. Then, the processor 20 associates the other robot 18 with the other entity E by making the above-described determination with respect to each robot 18.

  When the processor 20 performs the above-described determination, the tracking entity list shown in FIG. 12 is used. Referring to FIG. 12, the tracking entity list includes an entity ID and a flag column. In the flag column, “1” is set if the above-described conditions are satisfied, and “0” is set if the conditions are not satisfied.

  For example, the processor 20 determines whether or not the condition is satisfied for each entity E with respect to the robot 18a, and records the determination result in a flag column. When the determination is completed for all entities E, the processor 20 determines whether or not there is only one entity ID for which “1” is set in the flag column of the tracking entity list. At this time, if the entity ID with the flag set to “1” is “002”, that is, only the entity Eb, the processor 20 associates the robot 18a with the entity Eb.

  Further, the processor 20 records the association result in the association table shown in FIG. The association table includes a column indicating the robot ID and a column indicating the entity ID. For example, the processor 20 sets the entity ID “002” of the entity Eb for the robot ID “001” of the robot 18a in the association table.

  In the present embodiment, the process of associating the robot 18 with the entity E through such a process is referred to as a “global association process”. In the above description, for the sake of simplicity, the RSM speed difference is calculated by setting RT to 15, but in the actual global association processing, RT is set to 300.

  Next, RTM update will be described. The location of the RTM is updated based on the location data of the associated entity. For example, if the latest position data of the entity Eb associated with the robot 18a is (30, 40), the position of the RTM corresponding to the robot 18a is updated to (30, 40).

The angle of the RTM can be updated by obtaining an angle (estimated angle) and an estimated angle error amount estimated at the next position using the Kalman filter from the position data of the entity E and the state data of the robot 18. Specifically, referring to FIG. 14, the next position is predicted from the received state data of robot 18, and the predicted angle is calculated from the predicted position (predicted position) and the previous position. Further, the current position is acquired from the tracking server 10, and the detection angle is calculated from the previous position and the current position. Then, the observation angle Z _k is calculated (observed) from the detected angle and the predicted angle. However, when “Z _k ” is a subscript, for the sake of expression, “k” is not a subscript of “Z” except for mathematical expressions, and is described as a subscript. The same applies hereinafter.

Furthermore, the position error amount σ ² _po is always included in the observation of the tracking server 10. Therefore, the observation angle error amount σ ² _Zk can be approximated according to _Equation 4.

[Equation 4]

Here, in Equation 4, the displacement amount ds is the distance from the previous position to the current position in the position history data, and the angle error amount σ ² _Zk decreases when the displacement amount ds is large. That is, as the moving distance of the entity E increases, the angle error amount σ ² _Zk decreases.

Further, the Kalman gain K _k is calculated from the previous estimated angle error amount σ ² _θ-k and angle error amount σ ² _{Zk according} to _Equation 5. Then, the Kalman gain K _k, the observation angle Z _k and the previous estimated angle theta ^- the _k, by substituting the equation according to Equation 6, it calculates the estimated angle theta ^{^} _k.

However, when “θ ⁻ k” is a subscript, for reasons of expression, “−” is not a subscript of “θ” except for mathematical expressions, and is described as a subscript. To do. Further, the subscript “^” is added above “θ” in mathematical expressions, but for convenience of expression, it is described as a superscript “θ” except for mathematical expressions. The same applies hereinafter.

[Equation 5]

[Equation 6]

Furthermore, the Kalman gain K _k, the previous estimated angle theta ^- By substituting the equation according to _k and the number 7 the process noise sigma ² _pr hourly can calculate the estimated angle error amount σ ² _{θ ^ k.} In this embodiment, the noise σ ² _pr is calculated as 0.0001 radians.

[Equation 7]

When the estimated angle θ ^{^} _k is calculated in this way, the angle difference from the current RTM angle is calculated, the RTM angle is corrected to the estimated angle, and the calculated angle difference is stored as corrected angle data. To do.

Incidentally, if the previous value does not exist, for example, by the first angle correction process, the previous estimated angle theta ^- the predetermined value in place of _k and the estimated angular error amount sigma ² _{theta ^ k,} the formula of several 5-7 substituting Then, the estimated angle θ ^{^} _k and the estimated angle error amount σ ² _{θ ^ k} are calculated. Further, the predetermined value may be “0” or a value corresponding to the initial state of the robot 18.

  In this embodiment, when the status data is received (acquired) from the robot 18, the tracking server 10 transmits the correction angle data stored in the robot 18 and the position data indicated by the RTM to the robot 18 as correction data. To do. Then, the robot 18 corrects its own position and angle according to the received position data and correction angle data. Accordingly, the tracking server 10 can identify the position of the robot 18 within the detection region F.

Thus, the robot self-position identification system 100 can improve the accuracy of self-position identification by correcting not only the position but also the angle of the robot 18. In this embodiment, the estimated angle θ ^{^} _k can be _obtained using a Kalman filter.

The tracking server 10 stores the calculated estimated angle error amount σ ² _{θ ^ k} as correction accuracy, and when this correction accuracy is low, that is, when the estimated angle error amount σ ² _{θ ^ k} is large, The angle correction data is not transmitted to the robot 18. That is, only correction data for correcting the position is transmitted to the robot 18. Thus, by using the estimated angle error amount σ ² _{θ ^ k} as the correction accuracy, it becomes possible to determine whether the angle can be corrected.

  In this embodiment, a local tracking error or a global tracking error may occur after associating the robot 18 with the entity E. The local tracking error occurs when a problem occurs in the tracking server 10 or the particle filter that tracks the entity E disappears for a while. Further, a global tracking error occurs when the robot 18 and the entity E are erroneously associated with each other under a specific environment.

  Therefore, in this embodiment, in order to eliminate the local tracking error and the global tracking error, the association between the set of robots 18 and the entity E is canceled when at least one of the following three conditions is satisfied. That is, the three conditions are: (1) the associated entity E has not been tracked, (2) the entity E associated with the robot 18 is separated (for example, 2 m), (3) the robot 18 The speed of the entity E associated with the speed is different (for example, 0.35 mm / s).

  That is, the robot self-position identification system 100 can cancel the association between the robot 18 and the entity E that cause an error.

  Then, if the association between the robot 18 and the entity is released and within a predetermined time (for example, 5 seconds), the tracking server 10 executes the “local association process” to determine the robot 18 that has been released from the association. Associate with other entity E.

  In the local association process, the RMS speed difference (second speed difference) is 350 mm / s (second predetermined speed) or less, the current speed difference is 350 mm / s or less, and the robot 18 and the entity E If there are a plurality of entities E having a distance of 1 m (predetermined distance) or less, the entity E closest to the robot 18 is associated with the robot 18.

  Further, since the local association processing calculates RT shown in Equation 3 as 5 (seconds), the processing speed is faster than the global association processing described above. Hereinafter, the cancellation of the association and the local association processing will be described with a specific example.

  FIG. 15A is an illustrative view showing a positional relationship between a human and the robot 18. Further, in the state shown in FIG. 15A, the global association process is not executed. In the detection area F, the robot 18 performs a communication action on a human. At this time, the tracking server 10 detects the robot 18 at the position (200, 300) and detects a human at the position (250, 300). Then, the robot 18 outputs state data including position data (240, 300) and speed data (0 mm / s).

  When the global association process is executed in this state, as shown in FIG. 15B, the entity E corresponding to the human and the robot 18 are erroneously associated. In other words, if the communication action is performed, the robot 18 and the human do not move. Therefore, if there is no other human or the robot 18 that is stopped, as shown in FIG. The entity E and the robot 18 are associated with each other by mistake. The position of the RTM in the robot 18 is corrected to the human position (250, 300).

  However, when the communication action is finished and the person leaves from the front of the robot 18, the state shown in FIG. In other words, the position data of the state data output by the robot 18 is (250, 300), but the position data of the entity E corresponding to the human is (500, 300), and the distance between the RTM position and the human position is Is 250 cm (= 500-250) away. At this time, since the condition (2) for releasing the association is satisfied, the association between the entity E corresponding to the human and the robot 18 is released.

  Then, if the association is released and within a predetermined time, the local association process is executed. For example, in the state shown in FIG. 15C, the entity E corresponding to the robot 18 satisfies the local association conditions (1) to (3), so that the entity E and the robot 18 are correctly associated. In this way, the robot 18 and the entity E can be easily re-associated as soon as the association is released.

  Further, even if the local association process is executed, if the robot 18 and the entity E cannot be associated within 5 seconds, the global association process is executed. That is, even if the association is canceled and a predetermined time has elapsed, the robot 18 and the entity E can be associated again.

  FIG. 16 is an illustrative view showing one example of a memory map 300 of the memory 22 shown in FIG. As shown in FIG. 16, the memory 22 includes a program storage area 302 and a data storage area 304. The program storage area 302 stores a communication program 312 and a correction program 314 as programs for operating the tracking server 10. Note that the localization function is enabled by processing such as the communication program 312 and the correction program 314.

  The communication program 312 is a program for receiving status data from the robot 18 and transmitting correction data to the robot 18. The correction program 314 is a program for correcting RTM data, and includes subroutines of a tracking program 314a, an association release program 314b, an association program 314c, and an angle correction program 314d.

  The tracking program 314a is a program for updating the likelihood of the particle filter and detecting the position of the entity E. The association release program 314b is a program for releasing the association between the robot 18 and the entity E. The association program 314c is a program for associating the robot 18 with the entity E, and further includes subroutines of a local association program 316 and a global association program 318. The angle correction program 314d is a program for estimating the angle using the above-described Kalman filter and obtaining angle correction data.

  Although not shown in the drawings, the program for operating the tracking server 10 includes a program for counting time when the association is released, a program for synchronizing the time between the tracking server 10 and the robot 18, and the like. including.

  Referring to FIG. 17, the data storage area 304 includes an LRF buffer 330, a position history buffer 332, a state history buffer 334, a tracking entity list buffer 336, a correction angle buffer 338, a correction accuracy buffer 340, and a previous value buffer 342. Is provided. The data storage area 340 stores association table data 344 and RTM data 346, and an initial flag 348, an initial activation counter 350, and a release counter 352 are also provided.

The LRF buffer 330 is a buffer in which distance data measured by each LRF 12 is temporarily stored. The position history buffer 332 is a buffer in which the position history data shown in FIG. 7 is temporarily stored. The state history buffer 334 is a buffer in which the state history data shown in FIG. 8 is temporarily stored. The tracking entity list buffer 336 is a buffer in which data of the tracking entity list shown in FIG. 12 is temporarily stored. The correction angle buffer 338 is a buffer in which the angle difference between the estimated angle θ ^{^} _k and the RTM angle is temporarily stored as correction angle data. The correction accuracy buffer 340 is a buffer for temporarily storing the estimated angle error amount σ ² _{θ ^ k} as correction accuracy data. Previous value buffer 342, the estimated angle theta ^{^} _k and the estimated angular error amount sigma ² _{theta ^ k} calculated by the angle correction processing, the previous estimated angle theta ^- temporarily as _k and the estimated angular error amount sigma ² _{theta ^ k} This is a buffer for storing data.

  The association table data 344 is data of the association table shown in FIG. As shown in FIG. 9, the RTM data 346 is data composed of a plurality of RTMs corresponding to each robot 18.

  The initial flag 348 is a flag for determining whether or not a certain time has elapsed since the tracking server 10 was executed. For example, the initial flag 348 is composed of a 1-bit register. When the initial flag 348 is turned on (established), a data value “1” is set in the register. On the other hand, when the initial flag 348 is turned off (not established), a data value “0” is set in the register. The initial flag 348 is turned on when the tracking server 10 is activated, and turned off when a predetermined time has elapsed since the activation of the tracking server 10.

  The initial activation counter 350 is a counter for counting time until a predetermined time elapses after the tracking server 10 is activated. The release counter 352 is a counter for counting the time since the association between the robot 18, the entity, and E is released.

  Although not shown, the data storage area 304 temporarily stores data communication results between the remote control device 14 and the plan server 16 and a buffer that temporarily stores Kalman filter calculation results. And other counters and flags necessary for the operation of the tracking server 10 are also provided.

  Hereinafter, a flowchart of the present invention executed by the tracking server 10 will be described. 18 and 19 show processing by the communication program 312, and the flowchart in FIG. 20 shows processing by the correction program 314. 21 and 22 show processing by the tracking program 314a, and the flowchart of FIG. 23 shows processing by the association release program 314b. Further, the flowchart of FIG. 24 shows processing by the association program 314c, the flowchart of FIG. 25 shows processing by the local association program 316, the flowchart of FIG. 26 shows processing by the global association program 318, The flowchart of 27 shows the processing by the angle correction program 314d.

  Referring to FIG. 18, for example, when the tracking server 10 is powered on, a communication process is executed. Then, the processor 20 establishes communication with each robot 18 in step S1. For example, if the robots 18a to 18c exist in the detection area F, communication with those robots 18 is established. Subsequently, in step S3, the number of robots 18 that have established communication is set in the variable Rn. For example, if communication with three robots 18 (18a, 18b, 18c) is established, “3” is set in the variable Rn. The variable Rn is defined as a variable indicating the total number of robots 18 with which communication has been established. Subsequently, in step S5, Rn RTMs are created. For example, if “3” is set in the variable Rn, three RTMs are created. That is, the RTM data 346 includes three created RTMs.

  Subsequently, referring to FIG. 19, in step S7, the state data output by the robot 18 is acquired. For example, if the robot 18a transmits state data, the state data is received. Subsequently, in step S9, the state data is stored in the buffer based on the robot ID. For example, when the status data of the robot 18 a whose robot ID is “001” is acquired, the status data is stored in the status history buffer 334. Further, the state data of the robot 18a is added to the state history data of the robot 18a (see FIG. 8). The processor 20 that executes the process of step S9 functions as a state data storage unit.

  Subsequently, in step S11, it is determined whether or not correction is possible. That is, it is determined whether the robot 18 and the entity E are associated with each other. Further, in this embodiment, the robot 18 and the entity E cannot be associated with each other unless a certain period of time has elapsed since the tracking server 10 was activated. Therefore, in step S11, it is determined whether or not the initial flag 348 indicating that a certain time has elapsed since the start is off. If “NO” in the step S11, that is, if a predetermined time has not elapsed since the tracking server 10 is started, the process proceeds to a step S17.

If “YES” in the step S11, that is, if a predetermined time has elapsed since the tracking server 10 is started and the robot 18 and the entity E are associated with each other, whether or not the correction accuracy is high in a step S13. Judging. That is, it is determined whether or not the estimated angle error amount σ ² _{θ ^ k} stored in the correction accuracy buffer 340 is equal to or less than a threshold value (for example, 2 degrees). If “NO” in the step S13, that is, if the estimated angle error amount σ ² _{θ ^ k} temporarily stored in the correction accuracy buffer 340 is not less than or equal to the threshold value, the RTM position data is converted into the robot ID in a step S15. Send based on. For example, if the state data is acquired from the robot 18a with the robot ID “001”, the RTM position data is transmitted to the robot 18a with the same robot ID. Subsequently, in step S17, the RTM is updated based on the robot ID. That is, the angle and speed of the RTM are updated based on the angle data included in the acquired state data. For example, in the state data of the robot 18a, if the angle data is 30 degrees, the RTM angle of the robot 18a is updated to 30 degrees.

If “YES” in the step S13, that is, if the estimated angle error amount σ ² _{θ ^ k} temporarily stored in the correction accuracy buffer 340 is equal to or less than the threshold value, the RTM position data and the correction angle data are obtained in a step S19. Transmit based on the robot ID. For example, RTM position data and correction angle data are transmitted based on the robot ID of the robot 18 that has received the status data. The correction angle data transmitted to the robot 18 is read from the correction angle buffer 338.

  Subsequently, in step S21, the current angle is calculated from the acquired angle and correction angle data. For example, if the angle data of the state data is 25 degrees and the correction angle data is −2 degrees, the current angle is 23 degrees (= 25−2). In step S17, the RTM angle is updated to the angle calculated in step S21. For example, if the RTM angle is 30 degrees, the angle is updated to 23 degrees calculated in step S21.

  The processor 20 that executes the process of step S15 or step S19 functions as a transmission unit. In particular, the processor 20 that executes the process of step S15 functions as a second transmission unit, and the processor 20 that executes the process of step S19 functions as a first transmission unit.

  If “NO” is determined in step S11, the position, angle, and speed of the RTM are updated in step S17 based on the position data, angle data, and speed data included in the state data. That is, in a state where the robot 18 and the entity E are not associated with each other, the RTM is updated based on the state data output from the robot 18.

  In step S23, it is determined whether or not communication with each robot 18 is continued. For example, it is determined whether or not communication with the robots 18a to 18d is continued. If “NO” in the step S23, that is, if the communication with the robot 18 is interrupted, the RTM is deleted based on the robot ID in which the communication is interrupted in a step S25. For example, if communication with the robot 18d with the robot ID “004” is interrupted, the RTM with the robot ID “004” constituting the RTM data 346 is deleted. And if the process of step S25 is complete | finished, it will return to step S3. That is, the variable Rn is decremented so that the number of RTMs included in the RTM data 346 matches the numerical value of the variable Rn.

  If “YES” in the step S23, that is, if communication with each robot 18 is continued, it is determined whether or not communication with the new robot 18 is established in a step S27. For example, it is determined whether or not the fifth robot 18 has entered the detection area F. If “NO” in the step S27, that is, if communication with the new robot 18 is not established, the process returns to the step S7. On the other hand, if “YES” in the step S27, for example, if communication with the fifth robot 18 is established, the process returns to the step S3. That is, the variable Rn is updated and a new RTM is added to the RTM data 346.

  FIG. 20 is a flowchart showing the processing of the correction program 314. For example, when the power of the tracking server 10 is turned on, the processor 20 executes a tracking process in step S41. Since this tracking process will be described later with reference to the flowcharts shown in FIGS. 21 and 22, a detailed description thereof will be omitted here.

  Subsequently, in step S43, it is determined whether or not the process is the first time. That is, it is determined whether or not correction processing is executed for the first time after the tracking server 10 is activated. Specifically, it is determined whether or not the initial flag 348 is on. If “NO” in the step S43, that is, if the initial flag 348 is turned off, the process proceeds to a step S47. On the other hand, if “YES” in the step S43, that is, if the initial flag 348 is turned on, it is determined whether or not a predetermined time has passed in a step S45. For example, the predetermined time is 300 seconds, and in step S45, it is determined whether or not 300 seconds have elapsed since the tracking server 10 was activated. Specifically, it is determined whether or not the value of the initial activation counter 350 that counts the time since the activation of the tracking server 10 exceeds a value indicating 300 seconds.

  If “NO” in the step S45, that is, if a predetermined time has not elapsed since the tracking server 10 is started, the process returns to the step S41. That is, since the robot 18 cannot be associated with the entity E until the position data and the state data are stored, the processes of steps S41 to S45 are repeatedly executed.

  If “YES” in the step S45, that is, if a certain time has elapsed since the tracking server 10 is started, the initial flag 348 is turned off and the association releasing process is executed in a step S47. Note that the association release processing in step S47 will be described later with reference to the flowchart shown in FIG. 23, and thus detailed description thereof is omitted here. In the first process, even if the association release process is executed, the association between the robot 18 and the entity E is not released.

  Subsequently, an association process is executed in step S49. Since this association processing will be described later with reference to the flowchart shown in FIG. 24, detailed description thereof is omitted here. The processor 20 that executes the process of step S49 functions as an association unit.

Subsequently, in step S51, the variable ix is initialized. This variable ix is a variable for designating the robot ID. Therefore, when the variable ix is initialized, “1” is set. In the following description, the robot 18 with the robot ID “ _ix ” is described as “robot _ix ”. For example, the robot 18 with the robot ID “001” is described as “Robot ₁ ”. The same applies to the drawings.

Subsequently, in step S53, the position of the RTM corresponding to the robot _ix is updated. For example, if the variable ix is “1”, the RTM position of the robot 18a with the robot ID “001” is updated. Also, the position data for updating is read from the position history data of the robot ₁ stored in the position history buffer 332. Therefore, if the latest position in the position history data of the robot ₁ is (300, 400), the RTM position of the robot ₁ is updated to (300, 400). Subsequently, in step S55, an angle correction process is executed. Since this angle correction processing will be described later with reference to the flowchart shown in FIG. 27, detailed description thereof is omitted here.

  Subsequently, in step S57, the variable ix is incremented. That is, the variable ix is incremented to specify the next robot ID. Subsequently, in step S59, it is determined whether or not the variable ix is larger than the variable Rn. That is, it is determined whether or not the variable ix for specifying the robot ID is larger than the variable Rn indicating the total number of the robots 18. Thereby, it is possible to determine whether or not the RTM positions and angles have been updated for all the robots 18. If “NO” in the step S59, that is, if the variable ix is equal to or less than the variable Rn, the process returns to the step S53. That is, the process returns to step S53 in order to update the RTM position and angle of the other robot 18. On the other hand, if “YES” in the step S59, that is, if the variable ix is larger than the variable Rn, the process returns to the step S41. That is, when the RTM positions and angles of all the robots 18 are updated, the correction process is executed again.

  FIG. 21 is a flowchart showing the processing of the tracking program 314a. When the tracking process is executed in step S41, the processor 20 acquires LRF data in step S71. That is, the distance data measured by each LRF 12 that is temporarily stored in the LRF buffer 330 is acquired. In step S73, it is determined whether a new entity E is detected. For example, it is determined whether or not an entity E different from the entities Ea-Ed is detected in the detection area F. If “NO” in the step S73, that is, if a new entity E is not detected, the process proceeds to a step S79. On the other hand, if “YES” in the step S73, that is, if a new entity E is detected in the detection region F, the number of the entities E is set to the variable En in a step S75. For example, if four entities Ea, Eb, Ec, Ed are detected in the detection area F, “4” is set to the variable En. The variable En is defined as a variable indicating the total number of detected entities E.

Subsequently, in step S77, En position history data are created. For example, if “4” is set in the variable En, four position history data are created in the position history buffer 332 as shown in FIG. Subsequently, in step S79, the variable IX is initialized. This variable IX is a variable for designating the entity ID. Therefore, “1” is set when the variable IX is initialized. In the following description, the entity E whose entity ID is “ _IX ” is described as “entity _IX ”. For example, the entity E having the entity ID “001” is described as “entity ₁ ”. The same applies to the drawings.

Subsequently, referring to FIG. 22, in step S81, it is determined whether or not the robot 18 is associated with the entity _IX . For example, if “1” is set in the variable _IX , the association table is referenced to determine whether or not the robot 18 is associated with the entity Ea whose entity ID is “001”. If “YES” in the step S81, that is, if the robot 18 is associated with the entity _IX , the likelihood of the particle filter is updated using the robot shape model. That is, in step S83, the likelihood of the particle filter is updated using a model suitable for the robot 18 according to the equations shown in Equation 1 and Equation 2. On the other hand, if “NO” in the step S81, that is, if the robot 18 is not associated with the entity _IX , the likelihood of the particle filter is updated using the human shape model in a step S85. That is, in step S85, the likelihood of the particle filter is updated according to the equations shown in Equations 1 and 2 using a model suitable for humans.

Subsequently, in step S87, the position of the particle is updated. That is, the position of the particle is updated based on the calculated likelihood of the particle filter in the processing of step S83 and step S85. Subsequently, in step S89, the position of the entity _IX is detected. That is, the position of the entity _IX , that is, the plane coordinates in the detection area F is detected based on the position of the moved particle. The processor 20 that executes the process of step S89 functions as a position detection unit.

Subsequently, in step S91, the position of the detected entity _IX is stored in the buffer. That is, the position data is added based on the entity ID “IX” in the position history data stored in the position history buffer 332. For example, if “1” is set in the variable IX, the position data of the entity ₁ is added to the position history data with the entity ID “001”. The processor 20 that executes the process of step S91 functions as a position data storage unit.

  Subsequently, in step S93, the variable IX is incremented. That is, the variable IX is incremented to specify the next entity ID. Subsequently, in step S95, it is determined whether or not the variable IX is larger than the variable En. That is, it is determined whether or not the variable IX specifying the entity ID is larger than the variable En indicating the total number of entities E. Thereby, it is possible to determine whether or not positions have been detected for all entities E. If “NO” in the step S95, that is, if positions have not been detected for all the entities E, the process returns to the step S81. On the other hand, if “YES” in the step S95, that is, if positions are detected for all the entities E, the tracking process is ended and the process returns to the correction process.

  FIG. 23 is a flowchart showing the processing of the association cancellation program 314b. When the association process is executed in step S47, the processor 20 determines whether or not the robot 18 is associated in step S111. That is, the association table data 344 is read, and it is determined whether or not at least one robot 18 is associated with the entity E. If “NO” in the step S111, that is, if all the robots 18 are not associated with the entity E, the association releasing process is ended, and the process returns to the correction process. For example, when the tracking server 10 is activated and the association process in step S49 has never been executed, “NO” is determined in step S111.

  On the other hand, if “YES” in the step S111, that is, if at least one robot 18 is associated with the entity E, the number of the robots 18 associated in the step S113 is set to the variable ARn. For example, if there are three robots 18 associated with the entity E, “3” is set in the variable ARn. The variable ARn is defined as a variable indicating the total number of robots 18 associated with the entity E.

  Subsequently, in step S115, the variable Dx is initialized. For example, in the association cancellation process, the first local ID is assigned to the associated robot 18. The variable Dx is a variable for designating the first local ID. Therefore, when the variable Dx is initialized, “1” indicating the first ID of the first local ID is set.

The first local ID is given every time the association release process is executed. That is, the same first local ID is not necessarily given even for the same robot 18. Also, the robot ID and the first local ID do not necessarily match. Then, similarly to the variable ix, in the following description, the robot 18 having the first local ID “ _Dx ” is described as “robot _Dx ”. This description is the same in FIG.

In the next step S117, it is determined whether or not the entity E associated with the robot _Dx is still being tracked. That is, the entity ID of the entity E corresponding to the robot _Dx is read based on the association table data 344. Then, with reference to the position history buffer 332, it is determined whether or not position history data corresponding to the read entity ID exists. If “NO” in the step S117, that is, if the entity E associated with the robot _Dx does not exist in the detection region F, the process proceeds to a step S125.

If “YES” in the step S117, that is, if the entity E associated with the robot _Dx exists in the detection region F, whether or not the entity E associated with the robot _Dx is separated in the step S119. Determine whether. For example, as in step S117, the entity ID corresponding to the robot _Dx is read, and the latest position data of the position history data corresponding to the read entity ID is acquired from the position history buffer 332. Further, the position data included in the latest state data is acquired from the state data with the robot ID “Dx” from the state history buffer 334. Then, the position data of the read state data is compared with the position data of the position history data, and it is determined whether or not, for example, it is 2 m or more away. If “YES” in the step S119, that is, if the entity E associated with the robot _Dx is separated, the process proceeds to a step S125.

If “NO” in the step S119, for example, if the entity E associated with the robot _Dx exists within 2 m, whether or not the speed of the entity E associated with the robot _{Dx in} the step S121 is different. Judging. As in step S119, the current position and the position one second before are acquired from the position history data, and the speed of the entity E is calculated. Similarly to step S119, the current position and the position one second before are acquired from the state history data, and the speed of the robot _Dx is calculated. Then, it is determined whether or not the difference between the speed of the robot _{Dx and} the speed of the associated entity E is 0.35 mm / s or more, for example. If “YES” in the step S121, for example, if there is a speed difference of 0.35 mm / s or more, the process proceeds to a step S125. If “NO” in the step S121, for example, if there is no speed difference of 0.35 mm / s or more, the association of the robot _Dx is maintained in a step S123. For example, in step S123, a flag indicating that it is not necessary to cancel the association of the robot _Dx is turned on. Then, when the process of step S123 ends, the process proceeds to step S127.

If “NO” is determined in step S117, “YES” is determined in step S119, or “YES” is determined in step S121, and the process of step S125 is executed, the association of the robot _Dx is released. That is, in step S125, the entity ID associated with the robot _Dx is deleted from the association table data 344. When the association is released in step S125, the release counter 352 measures the time after the association is released.

  The processor 20 that executes the processes of steps S117, S119, and S121 functions as a determination unit. Further, the processor 20 that executes the process of step S125 functions as a canceling unit.

  Subsequently, in step S127, the variable Dx is incremented. That is, the variable Dx is incremented to specify the next first local ID. Subsequently, in step S129, it is determined whether or not the variable Dx is larger than the variable ARn. That is, it is determined whether or not the variable Dx specifying the first local ID is larger than the variable ARn indicating the total number of robots 18 associated with the entity E. Thereby, it is possible to determine whether or not it is necessary to cancel the association with respect to all the robots 18 associated with the entity E.

  If “NO” in the step S129, that is, if the variable Dx is equal to or less than the variable ARn, the process returns to the step S117. That is, the process returns to step S117 in order to determine whether or not the other robot 18 needs to cancel the association. On the other hand, if “YES” in the step S129, that is, if the variable Dx is larger than the variable ARn, the association releasing process is ended, and the process returns to the correction process.

  The release counter 352 is reset every time the association is released, and the time is measured. For example, when the association between the two robots 18 is released in the association release process, the time after the association is released is measured only for the robot 18 that has been released later.

  FIG. 24 is a flowchart showing the processing of the association program 314c. When the association process is executed in step S49, the processor 20 determines whether there is a robot 18 that is not associated in step S151. That is, the association table data 344 is read, and it is determined whether there is a robot 18 that is not associated with the entity E. If “NO” in the step S151, that is, if all the robots 18 are associated with the entity E, the associating process is ended and the process returns to the correcting process.

  On the other hand, if “YES” in the step S151, that is, if there is the robot 18 not associated with the entity E, the number of the robots 18 not associated in the step S153 is set to the variable DRn. For example, if there is one robot 18 that has been disassociated from the entity E, “1” is set in the variable DRn. Note that the variable DRn is defined as a variable indicating the total number of robots 18 that have been disassociated from the entity E.

  Subsequently, in step S155, the variable ax is initialized. For example, in the association process, the second local ID is assigned to the robot 18 that is not associated. The variable ax is a variable for designating the second local ID. Therefore, when the variable ax is initialized, “1” indicating the first ID of the second local ID is set.

Note that the second local ID is assigned every time the association process is executed, similarly to the first local ID. That is, the same second local ID is not always given to the same robot 18. Similarly to the variable Dx, in the following description, the robot 18 whose second local ID is “ _ax ” is described as “robot _ax ”. This description is the same in the drawings.

  Subsequently, in step S157, a tracking entity list is created. For example, the tracking entity list shown in FIG. 12 is created based on the variable En indicating the total number of entities E. The created tracking entity list is stored in the tracking entity list buffer 336. Subsequently, in step S159, it is determined whether or not the association is released within 5 seconds (predetermined time). That is, it is determined whether or not the association between the entity E and the robot 18 is released within 5 seconds. Specifically, it is determined whether or not the value of the release counter 352 is equal to or less than a value indicating 5 seconds. If “NO” in the step S159, that is, if the association between the entity E and the robot 18 is released and it has exceeded 5 seconds, the process proceeds to a step S171.

  On the other hand, if “YES” in the step S159, that is, if it is within 5 seconds after the association between the entity E and the robot 18, the variable IX is initialized in a step S161. That is, since it is necessary to identify all entities E in the local association process described later, the variable IX is initialized. Note that “1” is set in the initialized variable IX. In step S163, the tracking entity list is initialized. That is, all of the flag columns of the tracking entity list are set to “1 (on)”.

  Subsequently, in step S165, local association processing is executed. Note that the local association processing in step S165 will be described later with reference to the flowchart shown in FIG. 25, and thus detailed description thereof is omitted here. Subsequently, in step S167, the variable ax is incremented. That is, the variable ax is incremented to specify the next second local ID. Subsequently, in step S169, it is determined whether or not the variable ax is larger than the variable DRn. That is, it is determined whether or not the variable ax specifying the second local ID is larger than the variable DRn indicating the total number of robots 18 that are not associated. Thereby, it is possible to determine whether or not the local association process has been executed for all the robots 18 that are not associated with each other.

  If “NO” in the step S169, that is, if the variable ax is equal to or less than the variable DRn, the process returns to the step S161. That is, the process returns to step S161 in order to execute the local association process for the other robot 18 that is not associated. On the other hand, if “YES” in the step S169, that is, if the variable ax is larger than the variable DRn, the associating process is ended and the process returns to the correcting process.

  Here, if the association between the robot 18 and the entity E is not released within 5 seconds, the processing of steps S171 to S179 is executed. Note that, in steps S171 to S179, detailed description of the same processing as steps S161 to S169 is omitted.

  In step S171, the variable IX is initialized, and in step S173, the tracking entity list is initialized. Subsequently, in step S175, a global association process is executed. Since this global association process will be described later with reference to the flowchart shown in FIG. 26, a detailed description thereof is omitted here.

  Subsequently, in step S177, the variable ax is incremented, and in step S179, it is determined whether or not the variable ax is larger than the variable DRn. That is, in step S179, it is determined whether global association processing has been executed for all the robots 18 that are not associated with each other. If “NO” in the step S179, that is, if the global associating process is not executed for all the robots 18 not associated with the entity E, the process returns to the step S171. On the other hand, if “YES” in the step S179, that is, if the global association process is executed for all the robots 18 not associated with the entity E, the association process is ended.

FIG. 25 is a flowchart showing the processing of the local association program 316. When the local association process is executed in step S165, the processor 20 determines whether or not the entity _IX is associated with another robot 18 in step S191. That is, the association table 344 is read, and it is determined whether or not the entity _IX is associated with another robot 18. If “YES” in the step S191, that is, if the entity _IX is associated with another robot 18, the process proceeds to a step S201. On the other hand, if “NO” in the step S191, that is, if the entity _IX is not associated with the other robot 18, the RMS speed difference (the first time) between the entity _IX and the robot _ax in 5 seconds (first time) in the step S193. 1 speed difference) is calculated. That is, in step S193, the position history data of the entity _IX for 5 seconds is read from the position history buffer 332, and the state history data of the robot _ax for 5 seconds is read from the state history buffer 334. Then, the RT speed difference is calculated by setting RT to 5 in Equation 3. The processor 20 that executes the process of step S193 functions as a first speed difference calculation unit.

Subsequently, in step S195, it is determined whether the RMS speed difference between the entity _IX and the robot _ax is large. For example, it is determined whether or not the RMS speed difference calculated in step S193 is greater than a first predetermined speed (350 mm / s). If “YES” in the step S195, that is, if the RMS speed difference is larger than the first predetermined speed, the process proceeds to a step S201. The processor 20 that executes the process of step S195 functions as a first speed determination unit.

On the other hand, if “NO” in the step S195, that is, if the RMS speed difference is equal to or less than the first predetermined speed, it is determined whether or not the current speed difference between the entity _IX and the robot _ax is large in a step S197. That is, the current speed difference is obtained by calculating the speed of the entity _{IX and} the speed of the robot _ax from the position history data of the entity _IX and the state history data of the robot _ax read in step S193. Then, it is determined whether or not the current speed difference is larger than 350 mm / s, for example. If “YES” in the step S197, that is, if the current speed difference is larger than 350 mm / s, the process proceeds to a step S201.

If “NO” in the step S197, that is, if the current speed difference is 350 mm / s or less, it is determined whether or not the entity _IX is separated from the robot _ax in a step S199. That is, in step S199, the position data of the entity _{IX and} the position of the robot _ax are acquired from the position history data of the entity _IX and the state history data of the robot _ax read in step S193. Then, in step S199, it is determined whether or not the distance between the entity _IX and the robot _ax is greater than 1 m, for example. If “NO” in the step S199, that is, if the entity _IX is not separated from the robot _ax , the process proceeds to a step S203. Note that the processor 20 that executes the process of step S199 functions as a distance determination unit.

On the other hand, if “YES” in the step S199, that is, if the entity _IX is separated from the robot _ax , the flag of the entity _IX in the tracking entity list is turned off in a step S201. For example, if the variable IX is “1”, “0 (off)” is set in the flag field corresponding to the entity Ea.

  Subsequently, in step S203, the variable IX is incremented. That is, the variable IX is incremented to specify the next entity ID. Subsequently, in step S205, it is determined whether or not the variable IX is larger than the variable En. That is, it is determined whether or not the processing from step S191 to step S201 has been executed for all entities E. If “NO” in the step S205, that is, if the processing from the step S191 to the step S201 is not executed for all the entities E, the process returns to the step S191. On the other hand, if “YES” in the step S205, that is, if the processing from the step S191 to the step S201 is executed for all the entities E, it is determined whether or not there is an entity E whose flag is turned on in the step S207. to decide. That is, it is determined whether or not there is an entity E for which “1” is set in the flag column in the tracking entity list.

If “YES” in the step S207, that is, if there is an entity E for which “1” is set in the flag column in the tracking entity list, the closest entity E is associated with the robot _ax in a step S209. That is, the current position data of the entity E whose flag is on in the tracking entity list is read from the position history buffer 332. Further, the latest position data of the robot _ax is read from the state history buffer 334. Then, the distance between each of the entities E with the flag turned on and the robot _ax is calculated, and the entity E with the shortest distance is associated with the robot _ax . In addition, the result associated in step S209 is recorded in the association table.

For example, if the flags of the entity Ea and the entity Eb are on and the robot _ax is the robot 18a, the distances between the entity Ea and the entity Eb and the robot 18a are calculated. If the distance between the robot 18a and the entity Eb is shorter, the robot 18a and the entity Eb are associated with each other. Thereby, the entity ID of the entity Eb is recorded with respect to the robot ID of the robot 18a as in the association table shown in FIG. The processor 20 that executes the process of step S209 functions as a first association unit.

On the other hand, if “NO” in the step S207, that is, if the entity E whose flag is on is not in the tracking entity list, the entity E is not associated with the robot _ax in the step S211. That is, when step S211 is executed, the entity ID is not recorded in the association table.

  And if the process of step S209 or step S211 is complete | finished, a local correlation process will be complete | finished and it will return to a correlation process.

  FIG. 26 is a flowchart showing the processing of the global association program 318. Here, in the global association processing, detailed description of the same processing as the local association processing is omitted.

When the global association process is executed in step S175, it is determined in step S231 whether the entity _IX is associated with another robot 18 or not. If “YES” in the step S231, that is, if the entity _IX is associated with another robot 18, the process proceeds to a step S239. On the other hand, if “NO” in the step S231, that is, if the entity _IX is not associated with the other robot 18, the RMS speed difference (the second time) between the entity _IX and the robot _ax in 300 seconds (second time) in the step S233. 2 speed difference) is calculated. The processor 20 that executes the process of step S233 functions as a second speed difference calculating unit.

In step S235, it is determined whether the RMS speed difference between the entity _IX and the robot _ax is large. For example, it is determined whether or not the RMS speed difference between the entity _IX and the robot _ax is greater than the second predetermined speed (350 mm / s). If “YES” in the step S235, that is, if the RMS speed difference between the entity _IX and the robot _ax is larger than the second predetermined speed, the process proceeds to a step S239. The processor 20 that executes the process of step S235 functions as a second speed determination unit.

On the other hand, if “NO” in the step S235, that is, if the RMS speed difference between the entity _IX and the robot _ax is equal to or smaller than the second predetermined speed, whether the current speed difference between the entity _IX and the robot _ax is large in a step S237. Judge whether or not. For example, it is determined whether or not the current speed difference is greater than 350 mm / s.

If “YES” in the step S237, that is, if the current speed difference is larger than 350 mm / s, the flag of the entity _IX in the tracking entity list is turned on in a step S239, and the process proceeds to the step S241. On the other hand, if “NO” in the step S237, that is, if the current speed difference is 350 mm / s or less, the variable IX is incremented in a step S241, and it is determined whether or not the variable IX is larger than the variable En in a step S243. To do. If “NO” in the step S243, that is, if the variable IX is equal to or less than the variable En, the process returns to the step S231. On the other hand, if “YES” in the step S243, that is, if the variable IX is larger than the variable En, it is determined whether or not there is only one entity E whose flag is turned on in a step S245. That is, in step S245, it is determined whether or not there is only one entity E for which “1” is set in the flag column of the tracking entity list.

If “NO” in the step S245, the robot _ax is associated with the entity E whose flag is turned on in a step S247. For example, if the robot _ax indicates the robot 18b and only the flag of the entity Ec is on in the tracking entity list, the entity ID 003 is recorded for the robot ID “002” as in the association table shown in FIG. The The processor 20 that executes the process of step S247 functions as a second association unit.

On the other hand, if “NO” in the step S245, that is, if there is not only one entity E whose flag is on, the entity E is not associated with the robot _ax in the step S249 similarly to the step S211.

  And if the process of step S247 or step S249 is complete | finished, a global correlation process will be complete | finished and it will return to a correlation process.

FIG. 27 is a flowchart showing the processing of the angle correction program 314d. When the angle correction process in step S55 is executed, the processor 20 predicts the next position in step S261. Specifically, the latest position and the position one second before are read from the state history data of the robot _ix , and the amount of position displacement in the vertical (Y) axis and horizontal (X) axis directions is calculated. Then, in the RTM of the robot _ix, the predicted position as shown in FIG. 14 is obtained by adding the calculated position displacement amount to the previous position (previous position).

Subsequently, in step S263, a predicted angle and a detected angle are calculated. Specifically, the predicted angle is calculated from the angle between the predicted position obtained in step S261 and the previous position with reference to the horizontal axis (0 degree). As for the detected angle, first, the current position (data) in the entity E associated with the robot _ix is read from the position history buffer 332. Next, in the entity E associated with the robot _ix , the angle between the current position and the previous position is set as a detection angle, and calculation is performed with reference to the horizontal axis. Subsequently, in step S265, an observation angle _Zk is calculated from the detected angle and the predicted angle. For example, as shown in FIG. 14, the observation angle _Zk is calculated (observed) by subtracting the predicted angle from the detected angle. Note that the processor 20 that executes the processes in steps S261 to S265 functions as an angle observation unit.

Subsequently, in step S267, the observation angle error amount σ ² _Zk is approximated based on the position error amount σ ² _po and the displacement amount ds in the observation. That is, according to _Equation 4, the angular error amount σ ² _Zk is approximated from the quotient of the position error amount σ ² _po and the displacement amount ds. The processor 20 that executes the process of step S267 functions as an observation angle error calculation unit.

Subsequently, in step S269, it is determined whether or not there is a previous value. In other words, the previous estimated angle theta to the previous value buffer 342 ^- determines whether _k and the previous estimated angular error amount sigma ² _theta-k is stored. If “NO” in the step S269, for example, if the angle correction process is executed for the first time after the tracking server 10 is started, the observation angle error amount σ ² _Zk and a predetermined value are determined in a step S271. Update the Kalman filter. That is, in Equation 5, a Kalman gain K _k is calculated by substituting a predetermined value (for example, 0) in place of the previous estimated angle error amount σ ² _θ-k .

Subsequently, in step S273, an estimated angle error amount σ ² _{θ ^ k} and an estimated angle θ ^{^} _k are calculated using predetermined values, and the process proceeds to step S279. That is, in step S273, an estimated angle θ ^{^} _k is calculated by substituting a predetermined value in the equation shown in Equation 6 instead of the previous estimated angle θ ^- _k . Further, in the equation shown in Equation 7, by substituting a predetermined value in place of the estimated angle error amount σ _{² θ-k} of the previous, it calculates the estimated angular error amount σ ² θ _{^ k.}

Furthermore, if "YES" in the step S269, that is the previous estimated angle theta to the previous value buffer 342 ^- _k and if it is stored estimated angle error amount σ ² _θ-k of the previous, the observation angle error at step S275 The Kalman filter is updated based on the quantity σ ² _Zk and the previous value. That is, in Equation 5, the previous estimated angle error amount σ ² _θ-k read from the previous value buffer 342 is substituted to calculate the Kalman gain K _k . Subsequently, in step S277, the estimated angle error amount σ ² _{θ ^ k} and the estimated angle θ ^{^} _k are calculated using the previous values. That is, in the step S277, the equations shown in Equation 6, the last estimated angle theta read from the previous value buffer 342 ^- calculates an estimated angle theta ^{^} _k by substituting _k. Further, in the equation shown in Equation 7, the estimated angle error amount σ ² _{θ ^ k} is calculated by substituting the previous estimated angle error amount σ ² _θ-k read from the previous value buffer 342.

  In addition, the processor 20 which performs the process of step S261-S277 functions as an angle estimation means.

Subsequently, in step S279, an angle difference between the estimated angle θ ^{^} _k and the RTM angle of the robot _ix is calculated. For example, if the RTM angle of the robot _ix is 32 degrees and the estimated angle θ ^{^} _k is 30 degrees, −2 degrees (= 30−32) is calculated as the angle difference. Note that the processor 20 that executes the process of step S279 functions as an angle correction calculation unit.

Subsequently, in step S281, the estimated angle error amount σ ² _{θ ^ k} is set as the correction accuracy, and the calculated angle difference is temporarily stored as correction angle data. That is, the estimated angle error amount σ ² _{θ ^ k} is stored in the correction accuracy buffer 340, and the calculated angle difference is stored in the correction angle buffer 338. The processor 20 that executes the process of step S281 functions as a storage unit.

Subsequently, in step S283, the RTM angle of the robot _ix is updated based on the estimated angle θ ^{^} _k . For example, if the estimated angle θ ^{^} _k is 30 degrees, the RTM angle of the robot _ix is set to 30 degrees. And if the process of step S283 is complete | finished, an angle correction process will be complete | finished.

Note that the robot ID of the robot _ix is associated with the angle difference and the estimated angle error amount σ ² _{θ ^ k} . That is, a plurality of angle correction data and correction accuracy are stored in the correction angle buffer 338 and the correction accuracy buffer 340, and therefore a robot ID is associated with each other so that each can be identified.

  In the angle correction process, the previous correction angle data is reflected in the angle data included in the current state data. For example, if the correction angle data is calculated as -2 degrees in the previous angle correction process and is not transmitted to the robot 18, -2 degrees is reflected from the angle data included in the state data in this process. Is executed. However, when the correction angle data is transmitted to the robot 18, the correction angle buffer 338 is reset. That is, only when the correction angle data is transmitted to the robot 18, the previous correction angle data is reflected in the angle data included in the current state data.

  According to this embodiment, the robot self-localization system 100 includes a tracking server 10 and a robot 18 connected via a network 400. The tracking server 10 tracks the entity E by sensing the entity E in the detection area F by the LRF 12, and the robot 18 outputs state data.

  The tracking server 10 stores the state data output from the robot 18 and the detected position data of the entity E in the buffer of the memory 22. Further, the tracking server 10 executes an association process, and associates each robot 18 with each entity E based on the stored position history data and state history data. Further, the tracking server 10 updates the RTM corresponding to each robot 18 based on the position history data and the state history data, and transmits correction data based on the RTM to the robot 18. Then, the robot 18 corrects its own position and angle based on the correction data transmitted by the tracking server 10.

  As described above, in this embodiment, the robot 18 and the entity E can be associated with each other using the LRF 12 that can be easily installed and the state data that the robot 18 can generally output. Self-localization can be performed.

  In the robot self-position identification system 100, the time set for each server and the robot 18 is synchronized every predetermined time (for example, 20 seconds). That is, the time data associated with the state data output by the robot 18 and the position data stored by the tracking server 10 indicate the same time.

  Further, the operator of the remote operation device 14 may correct the position data of the robot 18. In this case, the robot 18 transmits a reset signal to the tracking server 10. When the robot 18 transmits a reset signal, the robot 18 does not correct the position and angle based on the correction data transmitted by the tracking server 10. Further, when receiving the reset signal, the tracking server 10 corrects the RTM based on the state data acquired immediately after the reset signal is interrupted. That is, when the position of the robot 18 is corrected by the operator, the correction data transmitted by the tracking server 10 is temporarily invalidated.

  The robot self-position identification system 100 may include a server dedicated to the localization function. Further, the robot self-position identification system 100 may not include the remote control device 14 and the plan server 16.

  Further, the robot self-position identification system 100 according to the present embodiment is not limited to the robot 18 according to the present embodiment, and can simultaneously perform self-position identification of various types of robots. Therefore, the position and angle of the state data output by the robot 18 may be detected using not only the wheel speed sensor 112 but also various sensors.

  The numerical values (predetermined time, first predetermined speed, second predetermined speed, first speed difference, second speed difference, first time, second time, predetermined distance, etc.) of the present embodiment are the robot self-position identification system. It may be arbitrarily changed according to the environment in which 100 is installed.

  In addition to the Kalman filter, the angle can be corrected using a filter (state estimation filter) that can estimate the state of the robot 18 such as a particle filter and an extended Kalman filter.

  In another embodiment, the robot 18 and the entity E may be associated by using a correlation coefficient instead of RMS.

10 ... Tracking server 12a-12f ... LRF
DESCRIPTION OF SYMBOLS 18 ... Robot 20 ... Processor 22 ... Memory 24 ... Communication LAN board 26 ... Wireless communication apparatus 32a, 32b ... Wheel 36a, 36b ... Wheel motor 100 ... Robot self-position identification system 112 ... Wheel speed sensor 114 ... Communication LAN board 116 ... Wireless Communication device 400 ... network

Claims (7)

A robot self-position identification system for self-locating a robot capable of outputting state data including at least its own position data with respect to its movement in a space in which the robot exists.
State data storage means for storing a plurality of state data sequentially obtained from the robot;
An environmental sensor for sensing an entity existing in the space;
Position detecting means for detecting the position of the entity based on the output of the environmental sensor;
Position data storage means for storing a plurality of position data of an entity detected by the position detection means;
Association means for associating the robot with the entity based on the plurality of position data of the robot stored by the state data storage unit and the plurality of position data of the entity stored by the position data storage unit; Transmission means for transmitting correction data for correcting position data included in the state data to the robot based on position data of the entity associated with the robot;
The robot self-position identification system in which the robot corrects its own position data according to the correction data. The state data further includes its own angle data,
The state data storage means stores state data including the angle data,
An angle estimation means for estimating an angle at the next position based on a plurality of state data of the robot stored by the state data storage means and a plurality of position data of the entity stored by the position data storage means; and Angle correction calculation means for calculating angle correction data for correcting the angle data included in the state data based on the angle estimated by the angle estimation means and the angle data included in the state data stored in the state data storage means. Further comprising
The transmission means includes first transmission means for transmitting the correction data and the angle correction data to the robot,
The robot self-position identification system according to claim 1, wherein the robot corrects its own position data and angle data according to the correction data and the angle correction data. The angle estimating means calculates an estimated angle error when estimating the angle,
Storage means for storing the estimated angle error as correction accuracy;
3. The robot self-position identification system according to claim 2, wherein the transmission unit includes a second transmission unit that transmits only the correction data to the robot when the correction accuracy stored by the storage unit is low. The angle estimation means observes the angle of the robot based on the plurality of state data of the robot stored by the state data storage means and the position data of the entity stored by the position data storage means. And an observation angle error calculation means for calculating an observation angle error of an angle observed by the angle observation means,
The angle estimation means estimates the angle of the next position based on the observed angle, the observed angle error, the previously estimated angle, and the previously calculated estimated angle error, and calculates an estimated angle error. The robot self-position identification system according to claim 3. A determination unit that determines whether to maintain the association between the robot and the entity, and a release unit that cancels the association between the robot and the entity when the determination unit determines not to maintain the association. The robot self-position identification system according to any one of 1 to 4. There are at least two entities in the space,
The association means includes a first speed difference calculation means for calculating a first speed difference between the robot and each entity within a predetermined time after the association between the robot and the entity is canceled, and the first speed First speed determining means for determining whether the first speed difference calculated by the difference calculating means is greater than a first predetermined speed; distance determining means for determining whether the distance between the robot and each entity is greater than a predetermined distance; And an entity in which a first speed difference with the robot is determined to be less than or equal to the first predetermined speed by the first speed determining means, and a distance from the robot is determined to be within a predetermined distance by the distance determining means, 6. The robot self-position identification system according to claim 5, further comprising first association means for associating an entity closest to the robot with the robot. There are at least two entities in the space,
The association means includes second speed difference calculation means for calculating a second speed difference between the robot and each entity when a predetermined time has elapsed since the association between the robot and the entity is canceled, Second speed determination means for determining whether the second speed difference calculated by the speed difference calculation means is greater than a second predetermined speed, and the second speed difference of the robot is determined by the second speed determination means to be the second predetermined speed. The robot self-position identification system according to claim 5 or 6, further comprising: a second associating unit that associates the entity with the robot when only one entity is determined as follows.

Images