JP5776332B2 - Map processing method and program, and robot system - Google Patents

Description

  The present invention relates to a map processing method and program, and a robot system. The present invention also relates to a computer-readable storage medium storing a map processing program.

  Autonomous mobile robots are able to move freely without colliding with obstacles, walls, etc. in the real environment. I do. There is a method in which a plurality of different maps are used together in order for a robot to reliably and accurately estimate its own position in an actual environment. As a technique for automatically creating an environmental map, for example, SLAM (Simultaneous Localization And Mapping) is known.

  The method of automatically creating a plurality of maps can be generally classified into the following two methods. In the first method, an initial map is created based on measurement information of a certain type of sensor. Thereafter, the measurement information of other types of sensors is added to the first map to create a second map, and the third and subsequent maps are created in the same manner. On the other hand, in the second method, measurement information of a plurality of different types of sensors is input to the estimator as simultaneous observation information, and a plurality of maps are created simultaneously and automatically.

  In the case of the first method, the second and subsequent maps depend on the map created first. For this reason, the accuracy of the map created after the second depends on the accuracy of the map created first. On the other hand, in the case of the second method, as the number of types of sensors increases, the observation space of the estimator increases, so that the calculation amount of the estimator increases and calculation time is required. Also, since different types of sensors use different measurement rates, it is necessary to match the measurement rate of the sensor used with the slowest measurement rate.

  However, when a plurality of maps are generated simultaneously, it is desirable to use a measurement rate unique to each sensor while reflecting different error models of different types of sensors. As described above, in a plurality of maps created independently, the coordinate system of each map is different, and each map includes a different distortion due to an estimation error. For this reason, at the stage where the robot estimates its own position, the self-position estimated by matching the measurement information of each sensor with the corresponding map is expressed in a different coordinate system. In order to perform route planning (or route planning) for planning or determining a route, it is necessary to integrate (or unify) the estimated coordinate system of the self-location.

  In a conventional integration method that integrates the positions and orientations estimated by a plurality of self-position estimators in a navigation system for autonomous mobile robots, for example, the estimation results of each self-position estimator are combined into one vector, and the observation data of the integration process Are integrated by a stochastic filter. However, the conventional integration method has the following disadvantages.

  First, in order to perform integration by a probabilistic filter, it is necessary to match the covariance matrix of the estimation result representing the estimation accuracy of each self-position estimator to a certain same reference. However, since each self-position estimator has been independently developed, there are differences in the calculation method and setting parameters of the covariance matrix among the self-position estimators. Therefore, in such integration processing based on the covariance matrix, it is difficult to integrate the coordinate system of the self-position estimated independently by a plurality of self-position estimators with high accuracy.

  Second, for example, when the estimation accuracy of a certain self-position estimator decreases (for example, even when the estimation process fails), the integration result is affected by the estimation result of the self-position estimator whose estimation accuracy has decreased. Performance will be degraded.

JP 2007-72592 A

  The conventional integration method has a problem that it is difficult to integrate the positions and orientations estimated by a plurality of estimators with high accuracy.

  In view of the above, an object of the present invention is to provide a map processing method and program, and a robot system capable of highly accurately integrating the positions and orientations estimated by a plurality of estimators.

According to one aspect of the present invention, evaluation evaluated using the reliability evaluation model the reliability of a plurality of estimators for estimating its own position, corresponding to said plurality of estimators based on the input from different sensors from each other and processing, for the estimated result from the plurality of estimators, the reliability was performed multiple stages divided different reliability binding processing for different integration processing according to the level of the computer, the reliability An evaluation model is provided with a map processing method including an integration tree that is temporally updated, in which the evaluated reliability and the integration history by the different integration processing are associated with each other.

According to one aspect of the present invention, evaluation evaluated using the reliability evaluation model the reliability of a plurality of estimators for estimating its own position, corresponding to said plurality of estimators based on the input from different sensors from each other procedures and, for the estimated result from the plurality of estimators, the reliability was performed multiple steps to divide the different reliability binding procedure for different integration processing according to the level of the computer, the reliability The evaluation model is provided with a program including an integration tree that is updated in time, in which the evaluated reliability and the integration history of the different integration processes are associated with each other.

According to one aspect of the present invention, different sensors provided in an autonomous mobile robot, a plurality of estimators that estimate self-positions based on inputs from the different sensors, and the reliability of the plurality of estimators the corresponding to said plurality of estimators and evaluation unit which evaluates using the reliability evaluation model for the estimated result from the plurality of estimators, the reliability of the plurality of stages divided into different reliability levels The reliability evaluation model includes a temporally updated integration tree in which the evaluated reliability and the integration history by the different integration processing are associated with each other. A robot system is provided.

  According to the disclosed map processing method and program, and the robot system, the positions and orientations estimated by a plurality of estimators can be integrated with high accuracy.

It is a perspective view which shows an example of a robot. It is a block diagram which shows an example of the robot system containing a robot. It is a figure explaining a mode that an integrated tree grows by the sequential input from the estimator side. It is a figure explaining a mode that an integrated tree grows by the sequential input from the estimator side. It is a figure explaining the pruning for suppressing the scale of an integrated tree below fixed. It is a flowchart explaining the outline | summary of operation | movement of an integrated device. It is a figure explaining the increase in the scale of an integrated tree. It is a figure explaining the process which deletes a defective node from the newest layer of an integrated tree. It is a figure explaining the maintenance of an integrated tree. It is a flowchart explaining the operation | movement of an integration device in detail. It is a figure explaining route evaluation. It is a figure explaining the filter process which calculates the uncertainty of the position and orientation produced | generated by the movement of the robot from odometry. It is a figure explaining a hidden Markov model. It is a figure which shows an example of an integrated tree. It is a figure which shows an example of the evaluation score obtained by path | route evaluation with respect to each node. It is a figure explaining an example of the integration result in a comparative example. It is a figure explaining an example of the integration result in an Example.

  In the disclosed map processing method and program, and the robot system, the integrator evaluates the reliability of the input information from each estimator and associates it with the integration history in different manners according to different levels of reliability. With this association, the integration tree representing the integration history is updated in time, and the scale increases. The level of reliability is a level obtained by further dividing the reliability obtained by quantifying the reliability into, for example, three levels of “high”, “medium”, and “low”.

  Alternatively, the estimated path obtained in the past from the latest node of the integrated tree may be evaluated and the lower nodes may be deleted to maintain the entire tree and keep the tree scale below a certain level.

  Hereinafter, embodiments of the disclosed map processing method and program, and robot system will be described with reference to the drawings.

  FIG. 1 is a perspective view showing an example of a robot. A robot 100 shown in FIG. 1 has a control unit formed by, for example, a carriage 101 driven by a motor, wheels 102, various internal and external sensors, and a processor such as a CPU (Central Processing Unit). It can be formed by an autonomous mobile robot having the basic configuration. The internal sensors include laser range finders (LRF) 2 and 3 and a camera 3. LRF2 and LRF3 may be different types of LRFs, for example. Different types of LRFs include LRFs having different measurement ranges.

  FIG. 2 is a block diagram illustrating an example of a robot system including a robot. In this example, the robot system 200 also functions as a navigation system for the autonomous mobile robot 100. Here, for convenience of explanation, a case where there are three estimators will be described, but at least two estimators are sufficient.

  The estimator 4 can be formed by the CPU, and forms a software module (estimation module) that estimates the self-position of the robot 100 based on the LRF 1 and the map 1A stored in the storage unit 5. The estimator 6 can be formed by the CPU, and forms a software module that estimates the self-position of the robot 100 based on the LRF 2 and the map 1B stored in the storage unit 7. The estimator 8 can be formed by the CPU, and forms a software module that estimates the self-position of the robot 100 based on the camera 3 and the map 1C stored in the storage unit 9.

  The integrator 10 can be formed by the CPU, and forms a software module (integrated module) that integrates the self-position estimation results of the estimators 4, 6, and 8. In the integrator 10, the estimator reliability evaluation unit 11 evaluates the reliability of the inputs from the estimators 4, 6, and 8, for example, three levels of reliability “high”> “medium”> “low” Divide into levels. The level of reliability is a level obtained by further dividing the reliability obtained by quantifying the reliability into, for example, three levels of “high”, “medium”, and “low”. Depending on the level of reliability, the connection to the integration tree described later differs. When the level of reliability is “high” (hereinafter, also referred to as “high” for convenience of explanation), the combining unit 12 displays the filter state as an observation value of the filter for integrating the inputs from the estimator. An update process is performed, a new node is created and added to the integration tree, and the estimator data is joined to the integration tree. When the reliability level is “low”, the filter state is updated only by odometry, and a new node is created and added to the integration tree. When the reliability level is “medium”, all the processes performed in the case of “high” and “low” are performed. This increases the scale of the integrated tree.

  In the integrator 10, the connection goodness evaluation unit 13 evaluates the goodness of connection to the integrated tree in order to perform pruning for reducing the scale of the integrated tree below a certain level as necessary. In this evaluation, the motion of the robot 100 is calculated from the position and orientation estimated by the filter that integrates the input from the estimator in the integrator 10, and compared with the calculated motion and the motion measured by the odometry unit 20. This can be done by calculating the likelihood of motion. The odometry unit 20 determines the amount of movement of the robot 100 from an internal sensor such as an encoder that detects rotation of the motor 102 or the wheel 102 of the carriage 101 or a gyro sensor (or gyroscope) provided in the robot 100. It has a known configuration for calculating based on the detection signal. In FIG. 2, an encoder and a gyro sensor are included in the encoder / gyro unit 19.

  The maintenance unit 14 of the integrated tree deletes the lower-level node that is the evaluation target in the estimated route from the integrated tree, and then scans the entire integrated tree to delete nodes that do not have children. Furthermore, a parent node having only one child is merged with the child node. The integrated tree is stored in the storage unit 15.

  The integrator 10 is estimated by the estimator 6 by the self-position of the coordinate system of the map A estimated by the estimator 4 (or the self-position of the coordinate system of the map C estimated by the estimator 8), for example. An integration process for integrating the self-position of the coordinate system of the map B is performed, and the integration result is supplied to, for example, the movement route planning unit 16 that plans or determines the movement route of the robot 100. In this example, the movement route determined by the movement route planning unit 16 is supplied to the cart movement controller 17 that can be formed by the CPU. The trolley movement controller 17 drives the motor 18 according to the movement route and moves the trolley 101 in the direction of the arrow in FIG. The movement amount of the robot 100 calculated by the odometry unit 20 is supplied to each of the estimators 4, 6, and 8.

  The integrator 10, for example, the self-position of the coordinate system of the map A estimated by the estimator 4, the self-position of the coordinate system of the map B estimated by the estimator 6, and the map C estimated by the estimator 8. It goes without saying that an integration process for integrating the self-positions of the coordinate system may be performed.

  When the robot system 200 is formed by the robot 100, each unit illustrated in FIG. 2 is provided in the robot 100. On the other hand, when the robot system 200 is formed by the robot 100 and an information processing apparatus separate from the robot 100 that controls the robot 100 (that is, a computer such as a server), the parts other than the LRFs 1 and 2 and the camera 3 May be provided in the information processing apparatus, the integrator 10 may be provided in the information processing apparatus, or the integrator 10 and the movement path planning unit 16 may be provided in the information processing apparatus. The information processing apparatus is formed by a general-purpose computer having a known configuration including a communication unit (or interface) that can communicate with the robot 100, a processor such as a CPU, and a storage unit that stores programs executed by the processor and various data. Is possible.

  Various known computer-readable storage media including a semiconductor storage device, a magnetic, optical, and magneto-optical recording medium can be used as a storage unit that stores at least a program for executing an integration process performed by the integrator 10.

  3 and 4 are diagrams for explaining how the integrated tree grows by sequential input from the estimator side. 3 shows processing at arbitrary times t1 and t2, and FIG. 4 shows processing at arbitrary times t3 and t4, respectively, where t1 <t2 <t3 <t4. That is, time t1 is the earliest time and time t4 is the latest time. 3 and 4, the left side shows processing on the estimator side, and the right side shows processing on the integrator side. 3 and 4, for convenience of explanation, an integration process for integrating the self-position of the coordinate system of the map A estimated by the estimator 4 and the self-position of the coordinate system of the map B estimated by the estimator 6 is performed. And Also, the estimators 4 and 6 are illustrated as estimators A and B, respectively, and the ID of the estimator 4 is indicated by A and the ID of the estimator 6 is indicated by B.

  At time t1 shown in FIG. 3, the integrator 10 copies the estimated values input from the estimators A and B in step ST1 to the observed values in the information held in the tree, and copies them to the state vector in the information held in the nodes. To do. In step ST1, the odometry input from the estimators A and B is copied to the odometry out of the information held in the tree, and a root node (Root Node) indicated by a circle with hatching in FIG. 3 is generated. Furthermore, step ST1 initializes a hidden Markov model (HMM: Hidden Markov Model) of the estimators A and B among the information of the nodes. In the following description, the root node is represented by a circle with hatching, and the child node is represented by a circle with no hatching.

  At time t2, the integrator 10 copies the estimated value and odometry input from the estimators A and B in step ST2 to the observation value and odometry of the information held in the tree. Thereby, the integrator 10 calculates the reliability of the estimator B using the HMM based on the odometry and the state vector in step ST3. The integration unit 10 adds child nodes depending on whether the reliability of the estimator B calculated in step ST4 is “high”, “medium”, or “low” (in this example, “medium”), The state vector of each child node is updated, and the HMM of the parent node (in this case, the root node) is copied to each child node.

  At time t3 shown in FIG. 4, the integrator 10 copies the estimated value and odometry input from the estimators A and B in step ST5 to the observation value and odometry of the information held in the tree. As a result, the integrator 10 calculates the reliability of the estimator A using the HMM for each node in step ST6 in the same manner as in step ST3, and assigns each node a child node according to the reliability as in step ST4. Is added, the state vector of the child node is updated, and the HMM of the parent node is copied to each child node.

  At time t4, the integrator 10 performs the same process as step ST5 in step ST7, and performs the same process as step ST6 in step ST8.

  In this way, the reliability of a plurality of estimators that estimate self-positions based on inputs (measurement information) from a plurality of different types of internal sensors is calculated using the HMM. Parameters used for calculation of the HMM corresponding to each estimator are included in inputs (estimated values and odometry) from the estimators. Also, an integrated tree is created by adding child nodes to the root node according to the reliability. Accordingly, the scale of the integrated tree stored in the storage unit 15 grows and increases. The scale of the integration tree is determined by evaluating the route from the latest node (node of the latest layer) to the root node in this integration tree to the root node, and detecting and deleting the node to be deleted. Can be kept below a certain level.

  FIG. 5 is a diagram for explaining pruning to keep the scale of the integrated tree below a certain level. In FIG. 5, the nodes are illustrated in the same manner as in FIGS. 3 and 4, and the nodes to be deleted are indicated by ● marks. In FIG. 5, nodes surrounded by broken lines indicate nodes of the latest layer. As a result of the evaluation of the estimated path of the integrated tree shown in FIG. 5A, for example, when defective nodes represented by ● are detected in FIG. 5B, these defective nodes are displayed as shown in FIG. delete. Next, a node represented by a mark ● in FIG. 5D that does not have a child node is extracted. In this case, among the nodes that do not have child nodes, nodes existing in the latest layer are excluded from extraction. Next, as shown in FIG. 5E, the extracted nodes that do not have child nodes are deleted. Further, a node having only one child is extracted as shown by being surrounded by a broken line in FIG. Also in this case, among the nodes having only one child, the nodes existing in the latest layer are excluded from extraction. Then, as shown in FIG. 5G, the child node and the parent node surrounded by a broken line in FIG. 5F are merged into one node. In this way, in this example, as a result of performing pruning to keep the scale of the integrated tree shown in FIG. 5 (a) below a certain level, the scale of the integrated tree becomes the scale shown in FIG. 5 (g). Details of pruning to keep the scale of the integrated tree below a certain level will be described later.

  FIG. 6 is a flowchart for explaining an outline of the operation of the integrator. In FIG. 6, the integrator 10 executes a process surrounded by a one-dot chain line, and (1) to (28) show an example of a flow of information exchanged in each process of FIG. Steps S <b> 104 to S <b> 120 correspond to processing (or procedures) executed by each function of the CPU forming the integration unit 10. The position and orientation of the robot 100 estimated based on the measured values of the estimators 4, 6, and 8 and the maps A, B, and C are stored in the integrator 10 as shown in (1), (2), and (3). Entered.

  When the integrator 10 receives the input from the estimator 4 for the first time, the integrator 10 initializes the integration tree with t = 0. Specifically, one root node as shown in FIG. 5 is generated, and an input as shown in Table 1 from the estimator 4 is copied to the root node, and a data set as shown in Table 2 is provided. In Table 2, N indicates a serial number starting from “1” arbitrarily assigned to the estimator included in the robot system 200, and is different from the ID number of the estimator in this example. In this example, since three estimators 4, 6, and 8 are used, in Table 2, “estimator [1]” is estimator 4, “estimator [2]” is estimator 6, and “estimator [ N] ”represents the estimator 8.

  (1) The estimator 4 inputs data as shown in Table 1 to the integrator 10.

  (2) The estimator 6 inputs data as shown in Table 1 to the integrator 10.

  (3) The estimator 8 inputs data as shown in Table 1 to the integrator 10.

  (4) In step S104, the integrator 10 enters an input waiting state in which it waits for the latest input from the estimators 4, 6, and 8, and manages a flag f1 indicating whether or not there is a new input. As described above, the input to the integrator 10 is data having contents as shown in Table 1. Step S105 writes a new input value to the shared memory when the flag f1 indicates that there is a new input from the estimator. The shared memory can be formed by a storage unit in the integrator 10.

  (5) In step S106, the integrator 10 reads an input value from the shared memory and determines whether or not to initialize the integration tree. When the integrator 10 receives the input from the estimators 4, 6 and 8 for the first time, the determination result is YES, and the integration tree is initialized as described above.

  (6) The integrator 10 inputs a flag f2 indicating whether or not to perform initialization and an input value from the estimator read from the shared memory to step S107 described later.

  (8) The integrator 10 inputs the odometry component included in the input from the estimator to step S110 described later.

  (9) The integrator 10 inputs the position / orientation estimation result component of the robot 100 included in the input from the estimator to step S111 described later.

  (10) The integrator 10 inputs the ID of the estimator included in the input from the estimator and the estimation result component of the position and orientation of the robot 100 to step S112 described later.

  In step S107, the integrator 10 initializes the integration tree as described above.

  (7) In step S <b> 108, the integrator 10 holds the integration tree in a shared memory that can be formed by the storage unit in the integrator 10.

  (11) The integrator 10 inputs the integration tree held in the shared memory to step S109.

  (19) The integrator 10 inputs the integration tree held in the shared memory to step S115 described later.

  (12) In step S109, the integrator 10 acquires all the nodes of the latest layer of the integration tree. If the integration tree has already been initialized, all nodes in the latest layer are acquired as a list. The integrator 10 inputs the list of nodes extracted from the latest layer to Step S110. In step S110, the integrator 10 updates the state of all nodes on the layer based on prediction by odometry. Specifically, prediction update is performed on all nodes (members) in the node list (node list) acquired in step S109 based on formulas (11) and (12) described later with reference to FIG.

  (13) The integrator 10 inputs the predicted and updated filter state for calculating the equation (20) described later with reference to FIG. 10 and the covariance matrix of the predicted observation value to the step S111. In step S111, the reliability of the input value by the HMM of each node is evaluated based on equations (21) to (24) described later with reference to FIG. 10, and the reliability of the estimation result of the position and orientation of the robot 100 input from the estimator is determined. Calculate gender level.

  (14) When the reliability level of the position / orientation estimation result is “high”, the integrator 10 predicts for calculating the reliability level, equations (18) and (19) to be described later with reference to FIG. The updated filter state, the predicted observation value, and the covariance matrix of the predicted observation value are input to step S112. If the reliability level is “high”, in step S112, the node states of the nodes for which the integrator 10 has evaluated the reliability level as “high” are expressed by equations (18) and (19) described later with reference to FIG. ) To update.

  (15) If the reliability level of the position / orientation estimation result is “low”, the integrator 10 inputs the reliability level and the predicted and updated filter state to step S113 described later.

  (15-1) When the reliability level of the position / orientation estimation result is “medium”, the integrator 10 calculates the reliability level, equations (18) and (19) described later together with FIG. The predicted updated filter state, the predicted observation value, and the covariance matrix of the predicted observation value are input to step S112. If the reliability level is “medium”, in step S112, the state of the node of the node for which the integrator 10 has evaluated the reliability level as “medium” is expressed by equations (18) and (19) described later with reference to FIG. ) To update.

  (15-2) If the reliability level of the position / orientation estimation result is “medium”, the integrator 10 further inputs the reliability level and the predicted and updated filter state to step S113 described later.

  (16) The integrator 10 inputs the observed and updated filter state to Step S113. A step S113 generates a child node to be added to the parent node.

  (17) The integrator 10 inputs a list of child nodes having a new filter state in step S114. Thereby, in step S114, the child node generated by the integrator 10 is added to each parent node. Only one child node is added to a node whose reliability level is evaluated as “high”. Only one child node is added to a node whose reliability level is evaluated as “low”. Two child nodes are added to the node whose reliability level is evaluated as “medium”.

  FIG. 7 is a diagram for explaining an increase in the scale of the integrated tree. In FIG. 7, HL, ML, and LL indicate nodes whose reliability levels are “high”, “medium”, and “low”, respectively. For example, when there is the latest input from the estimator at time t + 1, all the nodes created at time t are extracted from the integration tree. However, when t + 1 = 1, only the root node is extracted. Next, the following processing is performed for each node. Here, one new node Φ is created, and the currently processed node is represented by Ψ. The odometry is extracted from the node Ψ, and the estimated state held in the node Ψ is updated together with the latest input odometry as shown in equations (1) to (6) described later with reference to FIG. The updated estimation information is stored in the node Φ, and at the same time, the latest odometry is stored in the node Φ. The level of reliability can be calculated based on the equations (21) to (24) described together with FIG.

  When the reliability level is “high (HL)”, the observation state of the filter state of the new node Φ is updated based on the equations (16) to (19) described later with reference to FIG. It is added as a child node as shown on the left side of t + 1. When the level of reliability is “low (LL)”, the observation update of the filter state of the new node Φ is not performed, and the node Ψ is directly added as a child node as shown in the center at time t + 1 in FIG. When the reliability level is “medium (ML)”, the new node Φ is copied to create another node Φ ′ as shown on the right side of the time t + 1 in FIG. Update the observation of the filter state of the node Φ ′ and add it as a child node to the node Ψ. In addition, the observation update of the filter state of the node Φ is not performed, and the node Ψ is directly added as a second child node.

  (18) The integrator 10 inputs the integration tree to which the child node has been added to step S108. In step S108, the integrator 10 updates the memory holding the integration tree. The integrated tree can be formed by a storage unit in the integrator 10.

  (19) The integrator 10 reads the integration tree from the memory and inputs it to step S115. In step S115, all nodes on the latest layer included in the integration tree are acquired as a list. The node acquired in step S115 is a child node newly added to the integration tree in step S114.

  (20) The integrator 10 inputs the latest node, that is, the child node newly added in step S114, to step S116. In step S116, the integrator 10 extracts all routes obtained by tracing to the root node for each node in the list.

  (21) The integrator 10 inputs the extracted list of all routes to step S117. In step S117, for each path extracted by the integrator 10, the movement pattern measured by odometry and the movement pattern estimated by the filter are compared, and the path in the list based on the equation (25) described later with reference to FIG. Perform path evaluation to evaluate

  (22) The integrator 10 inputs, in step S118, a list of lower nodes whose evaluation score is lower than the threshold among the evaluation scores representing the goodness of the route obtained by the route evaluation. In step S118, the integrator 10 deletes a lower node from the integration tree.

FIG. 8 is a diagram illustrating processing for deleting a defective node from the latest layer of the integration tree. Assuming that K nodes are included in the latest layer, node deletion processing is performed when K is greater than a preset threshold value K _thresh . In this case, K−K _thresh nodes are deleted. The evaluation score of the path corresponding to each node is, for example, Ω (P (Φ _k )), k = 0, 1,. . . , K,. . . Represented by K. Therefore, by sorting the nodes in the ascending order of the evaluation score Ω (P (Φ _k )) and deleting the lower K−K _thresh nodes from the latest layer, the integrated tree is in the state shown in FIG. The state changes to the state shown in FIG. In FIG. 8A, a defective node to be deleted is represented by a mark ●.

  (23) The integrator 10 inputs an evaluation score representing the goodness of the route obtained by the route evaluation in step S117 to step S120. In step S120, the integrator 10 outputs an integration result and a reliability evaluation result. Specifically, the filter estimation value possessed by the top node of the route evaluation score is set as the processing result of the integrated tree, and is output to the application described later. At the same time, the processing result and the estimated reliability level are fed back to the estimator forming the estimation module.

  (24) The integrator 10 inputs the integrated tree from which the lower nodes have been deleted in step S118 to step S108, and updates the memory holding the integrated tree.

  In step S119, the integrator 10 performs maintenance of the integration tree. FIG. 9 is a diagram for explaining the maintenance of the integrated tree. In the maintenance of the integration tree, as shown in FIG. 9A, nodes that do not have child nodes are extracted from the integration tree except the latest layer, and the extracted child nodes are integrated as shown in FIG. 9B. Remove from the tree. Further, as shown in FIG. 9C surrounded by a broken line, a node having only one child is extracted from the integrated tree except for the latest layer. Then, as shown in FIG. 9D, by merging the child node and the parent node surrounded by a broken line in FIG. 9C into one new node, the data held by the child node is replaced with the new node. Data. Such integration tree maintenance is the same as the pruning for keeping the scale of the integration tree below a certain level described with reference to FIGS.

  (25) The integrator 10 inputs the maintenance integrated tree into step S108 and holds it in the memory.

  (26) The integrator 10 inputs the filter estimation value possessed by the top node of the route evaluation score as the processing result of the integration tree, for example, in step S121 for executing the application. Thereby, the position and orientation information of the robot 100 estimated by the integrated tree method can be input to the application. The application is executed by, for example, the movement path planning unit 16 that plans or determines the movement path of the robot 100 using the position and orientation of the robot 100 estimated from the integrated tree.

  (27) The integrator 10 inputs to the step S104 a flag f3 indicating that the series of integration processes has been completed.

  (28) The integrator 10 feeds back the position and orientation of the robot 100 estimated by the integrated tree method and the estimated reliability level to the estimator forming the estimation module as a diagnosis result to the estimation module.

  FIG. 10 is a flowchart for explaining the operation of the integrator in more detail. 10, (1) to (48) show an example of the flow of information exchanged at each step of FIG. Steps S201 to S234 correspond to processes (or procedures) executed by each function of the CPU forming the integration unit 10. In step S201, the integration processing is started in response to the integration processing start command input from the robot 100 or an information processing apparatus separate from the robot 100.

  (1) A start command indicating the start of the integration process is input to step S202, and step S202 enters a state of waiting for input from the estimators 4, 6, and 8, and whether there is a new input from the estimators 4, 6, and 8. Check whether or not.

  (2) When there is a new input, a flag f1 indicating that there is a new input is input to step S203. In step S203, it is determined whether or not the input flag is an end flag. If the decision result in the step S203 is NO, a flag is inputted to the step S205. On the other hand, if the decision result in the step S203 is YES, the integration process ends in a step S204.

  In step S205, it is determined whether or not the input flag is a flag f1 indicating a new input. If the determination result in step S205 is YES, the process proceeds to step S206, and if the determination result is NO, the process returns to step S202.

  (5) If the decision result in the step S205 is NO, a new input wait command is inputted to the step S202.

  (6) If the decision result in the step S205 is YES, the input data from the estimator is inputted to the step S206 in the format as shown in Table 1 above, for example. In step S206, only the estimation information (that is, the estimation result) of the estimator is extracted from the input data from such an estimator.

  (7) After step S206, the data extraction completion flag f4 and the input data of the estimator are input to step S207. In step S207, for example, odometry is extracted from the input data of the estimator having the format shown in Table 1 above.

  (8) After step S206, the position and orientation of the robot 100 estimated by the estimator and the covariance matrix of the estimation result representing the estimation accuracy of the estimator are input to step S223 described later.

  (10) After step S206, as in (8) above, the position and orientation of the robot 100 estimated by the estimator and the covariance matrix of the estimation result representing the estimation accuracy of the estimator are input to step S218 described later.

  (9) After step S207, a flag f5 indicating that extraction of odometry is completed and input data of the estimator are input to step S208. In step S208, the ID number of the input estimator is identified and extracted.

  (21) After step S207, odometry is input to step S216 described later.

  (11) After step S208, a flag f6 indicating that the extraction of the ID number of the estimator is completed is input to step S209. In step S209, all the nodes in the latest layer of the integration tree are extracted, and a node list of the extracted nodes is created. Here, the latest layer is a layer including a new node added to the integration tree in the previous process.

  (12) After step S208, the extracted ID number of the estimator is input to step S214 described later.

  (13) The number of extracted nodes in the latest layer (number of nodes) is input to step S210. In step S210, it is determined whether or not the number of nodes is zero (0). If the determination result is YES, it is understood that an integrated tree has not been constructed, and thus the process proceeds to step S211. The determination result is NO. If so, the process proceeds to step S212.

  (14) If the decision result in the step S210 is YES, an integration tree initialization command is inputted to the step S211. Step S211 initializes the integration tree as described above in response to the initialization command.

  (15) After step S211, an input waiting command is input to step S202.

  (16) If the decision result in the step S210 is NO, the created node list is inputted to the step S212. In step S212, one node is extracted from the node list and the processing of steps S213, S214, and S215 described below is started, and the processing of setting the extracted node as ND is performed on each unprocessed node in the node list. Against.

  (17) After step S212, the node ND having the data set shown in Table 2 is input to step S213.

  (18) After step S212, the node ND having the data set shown in Table 2 is input to step S214 in the same manner as in (17) above.

  (19) After step S212, the node ND having the data set shown in Table 2 is input to step S215 in the same manner as in (17) above.

  In step S213, the position and orientation of the robot 100 estimated by the filter so far, the covariance matrix, and the odometry are extracted from the filter data of the node ND.

  (20) After step S213, the position and orientation of the robot 100 estimated by the filter and the odometry are input to step S216.

  In step S214, the HMM corresponding to the estimator ID is extracted from the node ND using the ID number of the estimator input in (12) after step S208. The node ND has HMMs corresponding to the number of estimators. In this process, only the HMMs corresponding to the estimators that output data to the integrator 10 are extracted.

  (25) After step S214, the HMM data, that is, the probability in state “1” and the probability in state “2” calculated so far are input to step S218. The HMM data will be described together with FIG.

  In step S215, the evaluation score of the route so far is extracted from the node. Specifically, in order to perform the following route evaluation process, the evaluation score for the route from the route node stored in the node ND is extracted.

  (48) After step S215, the route evaluation score to the node ND is input to step S229 described later.

  In step S216, the position and orientation are predicted and updated by odometry. Specifically, using the new odometry input in (21) and the odometry stored in the node ND in (20), processing up to equation (15) described below is performed.

  FIG. 13 is a diagram for explaining a hidden Markov model (HMM). The HMM is constructed according to the performance of each estimator. In the HMM as shown in FIG. 13, the state “1” represents a state where the input of the estimator is reliable, and the state “2” represents a state where the input of the estimator is not reliable. Observation symbol “0” indicates that the input of the estimator is outside the prediction gate of the filter, and observation symbol “1” indicates that the input of the estimator is within the prediction gate of the filter.

  Next, setting of HMM parameters will be described. The HMM state transition probability parameter is determined by the performance of the estimator to be evaluated using the HMM. For example, there are two estimators (equivalent to the estimator 4 or 6 in FIG. 1 and the estimator 8) that perform self-position estimation by LRF and self-position estimation by a camera (for example, using an image landmark). And In this case, the two estimators have the following properties c1 and c2.

  c1: When self-position estimation is performed using LRF measurement information, the current new position is searched in the vicinity of the previous estimated position of the robot 100. In this case, if the previous estimated position is wrong, the result searched this time is likely to be wrong.

  c2: On the contrary, when using the image landmark, even if the previous estimated position of the robot 100 is incorrect, the result estimated this time approaches the true position.

Therefore, in the state transition matrix, for example, in the case of LRF: a00 = 0.90, a01 = 0.10, a10 = 0.01, a11 = 0.99;
For the camera: a00 = 0.95, a01 = 0.05, a10 = 0.10, a11 = 0.90;
Set to here,
a00: The previous time is reliable and this time also indicates the probability of being reliable, and the camera is set higher from the characteristics of the LRF and the camera.
a01: The last time is reliable, and this time is an unreliable probability, and a00 + a01 = 1 is set.
a10: Unreliable last time, this time shows a reliable probability (recovery performance), and in the case of LRF, the recovery function is weak, so it is set lower than the camera.
a11: The probability that the previous time is unreliable and this time is also unreliable is set so that a10 + a11 = 1.
Returning to the description of the processing after step S216 in FIG.

  (22) After step S216, the predicted value for the estimated value of the estimator and the data of the prediction gate (the calculation results of the above equations (14) and (15)) are input to step S218.

  (23) After step S216, the calculation result data of the above equations (11) and (12) is input to step S217. In step S217, one new node is created, and the data input in (23) is stored in that node.

  (24) After step S217, the new node data is input to step S222.

  Step S218 calculates the likelihood of the estimated value of the estimator input based on the above equation (20), and generates an observation symbol by binarizing the likelihood as in the above equation (22). To do.

  (26) After step S218, the observation symbol that is the result of binarization is input to step S219. In step S219, each state of the HMM is updated based on the above equation (21).

  (27) After step S219, the likelihood in each state of the HMM obtained from the calculation result of the above equation (21) is input to step S220. In step S220, the reliability is calculated based on the following equation (23), and the level of reliability is calculated based on the following equation (24).

(28) After step S220, the reliability level ε _t calculated based on the equation (24) is input to step S221. In step S221, it is determined whether the reliability level ε _t is HL (high), ML (medium), or LL (low). If the determination result of step S221 is HL, the process proceeds to step S222. If the determination result of step S221 is LL, the process proceeds to step S224. If the determination result of step S221 is ML, the process proceeds to step S222 and step S224.

  (29) If the determination result in step S221 is HL, a processing flag indicating HL is input to step S222.

  (30) If the determination result in step S221 is LL, a processing flag indicating LL is input to step S224.

  (31) If the determination result in step S221 is ML, a processing flag indicating ML is input to steps S222 and S224.

  In step S222, one new node is created, and the data input in (24) is stored in the node, thereby duplicating the new node.

  (32) After step S222, the copied node data is input to step S223. In step S223, the state of the duplicated node is updated based on the above equations (16) to (19), and the calculation result data of equations (18) and (19) is stored in the duplicated node.

  (33) After step S223, the data of the replication node whose state has been updated is input to step S225.

  On the other hand, in step S224, one new node is created, and the data input in (24) is stored in the node, thereby duplicating the new node.

  (34) After step S224, the data stored in the new node is input to step S225. In step S225, as described with reference to FIG. 7, in the case of HL, one node processed in step S223 is added, and in the case of LL, one node copied in step S224 is added as a child node to the parent node ND. In some cases, the two nodes processed in step S223 and step S223 are added as child nodes of the parent node ND, and the node is added as a child node to the parent node.

  (35) After step S225, the processed node ND data is input to step S226. In step S226, it is determined whether or not all nodes have been processed. If the determination result is NO (36), the process returns to step S212, and if the determination result is YES, the process proceeds to step S227. . Specifically, in step S226, the node ND is deleted from the node list input in (16), and it is determined whether the number of remaining nodes in the node list is zero (0). If the number of remaining nodes is zero, the determination result is YES. If the number of remaining nodes is not zero, the determination result is NO.

  (37) If the decision result in the step S226 is YES, a process completion flag indicating that the processes of all the nodes are completed is inputted to the step S227. In step S227, all nodes in the latest layer are extracted from the integration tree to create a node list. In this case, the latest node is the child node just added in step S225.

  (38) After step S227, the node list is input to step S228. In step S228, one unprocessed node is extracted from the node list.

  (39) After step S228, the extracted node is input to step S229. In step S229, the evaluation score representing the goodness of the route traced from the extracted node to the root node is updated based on the equation (12), and the calculation result of the equation (12) is stored in the node.

  (40) After step S229, the processed node data is input to step S230. In step S230, it is determined whether or not all nodes have been processed. If the determination result is NO, the process returns to step S228, and if the determination result is YES, the process proceeds to step S231. Specifically, in step S230, the processed node input in (40) is deleted from the node list input in (38), and the number of remaining nodes in the node list is zero (0). It is determined whether or not. If the number of remaining nodes is zero, the determination result is YES. If the number of remaining nodes is not zero, the determination result is NO.

  (41) If the decision result in the step S230 is YES, a process completion flag, each route and an evaluation score for the route are inputted in a step S231.

  (42) If the decision result in the step S230 is NO, the data of the remaining nodes in the node list are inputted to the step S228.

  In step S231, the evaluation scores for each route are sorted in ascending order, and a child node included in a lower route having a low evaluation score is extracted to create a list.

  (43) After step S231, a list of child nodes included in the lower path with a low evaluation score to be deleted is input to step S232. In step S232, a child node deletion process is performed in which the child node included in the list input in (43) is deleted from the integration tree by performing the processing described with reference to FIG.

  (44) After step S232, the integrated tree data from which the child nodes have been deleted is input to step S233. In step S233, the integrated tree maintenance process is performed as described with reference to FIG. Specifically, the node that does not have a child node is deleted by tracing (scanning) from the latest layer of the integration tree to the root node. In addition, a parent node having only one child is merged with the child node.

  (45) After step S233, the maintenance-integrated tree data is input to step S234. In step S234, reliability evaluation result and integration result output processing is performed. Specifically, the data calculated in step S220 and input in (28) and the position / posture state of the robot 100 at the node having the maximum path evaluation score are written in the shared memory and shared with the application. The application is executed by, for example, the movement path planning unit 16 that plans or determines the movement path of the robot 100 using the position and orientation of the robot 100 estimated from the integrated tree.

  (46) After step S234, an input waiting command is input to step S202.

  (47) After step S234, the level of reliability to the estimator and the result of the position and orientation of the robot estimated from the integration tree are input to step S235. Step S235 is executed outside the integrator 10, for example, by the movement route planning unit 16 shown in FIG. In step S235, based on the reliability level to the estimator and the result of the position and orientation of the robot estimated from the integration tree, for example, a movement path plan is executed.

  FIG. 14 is a diagram illustrating an example of an integrated tree. In FIG. 14, a node is represented by a circle, a latest node is represented by a circle, and the darker the central portion of the circle, the higher the evaluation score of the route. A band-like region surrounded by two adjacent horizontal lines indicates one layer, and a number shown beside the node indicates a path number.

  FIG. 15 is a diagram showing an example of an evaluation score obtained by route evaluation for each node of the integrated tree shown in FIG. In FIG. 15, evaluation scores of 10 different node numbers are shown in one line. The node numbers of the nodes are assigned to the nodes in the order from the left to the right in FIG. 14, for example, from the highest layer to the lowest layer of the integrated tree.

  FIG. 16 is a diagram illustrating an example of the integration result in the comparative example. FIG. 16 and FIG. 17 to be described later show a plan view of a movement path obtained from the estimation result of the self position when the robot 100 autonomously moves in the room. 16 and 17, the five quadrilaterals in the room are desks, for example.

  16A shows a path based on the estimation result of the self-position of the robot 100 by the estimator 8 using the camera 3 by a one-dot chain line L1, and FIG. 16B shows an estimation using the LRF 2 having a measurement range of 5.0 m. A path based on the estimation result of the self-position of the robot 100 by the device 6 is indicated by a two-dot chain line L2, and FIG. Show. Further, in the portion surrounded by the broken line, although the route is accurately estimated in FIG. 16A, the accuracy of the route is remarkably lowered in FIG. 16B. In the path shown in FIG. 16C in which b) is integrated, the integration result is significantly degraded due to the adverse effect of the path in which the accuracy in FIG. As described above, in the comparative example, it was confirmed that the accuracy of the route at the portion surrounded by the broken line in the integration result was low.

  FIG. 17 is a diagram illustrating an example of the integration result in the embodiment. 17A shows a path based on the estimation result of the self-position of the robot 100 by the estimator 8 using the camera 3 by a one-dot chain line L1, and FIG. 17B shows an estimation using the LRF 2 having a measurement range of 5.0 m. A path based on the estimation result of the self-position of the robot 100 by the device 6 is indicated by a two-dot chain line L2, and (c) indicates a path based on the integration result using the integration tree of the above embodiment by a solid line L30. Further, a portion surrounded by a broken line indicates a portion where the route is accurately estimated in FIG. 17A, but the route accuracy is significantly lowered in FIG. 17B. However, according to the present embodiment, as shown in FIG. 17C, since the integration tree is used, it was confirmed that good accuracy of the route is maintained even in the portion surrounded by the broken line of the integration result. . That is, it was confirmed that the position and orientation estimated by a plurality of estimators can be integrated with high accuracy.

  In the case of the above comparative example, the integration result is affected by, for example, the estimation result of the broken estimator, and in the worst case, the accuracy of the estimation path is lowered so that the integration result cannot be used. On the other hand, by using the integrated tree as in the above embodiment, for example, a failed estimator can be identified and isolated from the integrated tree. For this reason, it is possible to perform processing so that the integration result is not affected by the estimation result of the broken estimator, for example, and the stability of the robot system or the navigation system can be greatly improved.

  Next, the reliability evaluation of input information, the different integration processing based on the reliability, the pruning of the integration tree, and the feedback of the diagnosis function in the above embodiment will be described.

Reliability evaluation of input information:
The reliability of the input information from the estimator is evaluated by using the HMM for the reliability evaluation of the input information. The integrator adds a state quantity called reliability to the input information such as position, posture, and position / posture covariance matrix from each estimator, and converts the input information into position, posture, position / posture covariance matrix, and reliability. Expand. The reliability is estimated by the integrator based on the past estimation history. Each estimator also inputs the odometry position at the point where the estimation is performed together with the above estimation information.

  The integrator prepares as many reliability evaluation models as the number of estimators. The reliability evaluation model is constructed based on the HMM. The HMM has two states, and two symbols can be observed in each state. This symbol is a binary value of “0” and “1” indicating whether or not the actual input value is in the vicinity of the new input value predicted from the integration history of the integrator. When this symbol is observed, the state inside the HMM is updated. Reliability is calculated from the ratio of the probabilities of these two states.

  HMM parameters are set according to the performance of each estimator. For example, the error recovery function of an estimator using a camera is higher than that of an estimator using LRF, and the transition probability from an unreliable state to a reliable state is set high.

Different integration processes based on reliability:
Different integration processes based on reliability perform different integration processes on input information with different reliability by using an integration tree. The reliability estimated by the integrator is discretized into, for example, three levels of “high”, “medium”, and “low”, and different integration processing is performed for each level. Introduce an integration tree to do different processing for each level of reliability. The depth direction of the integration tree corresponds to the time direction. The lowest layer in the integration tree corresponds to the latest time. Each node of the integration tree is an integration result at the time corresponding to the node, and the current position of the robot obtained by the integration is included. A node is created each time a new input is received from the estimator, and includes information on the odometry input at that time. When a new input is received from the estimator, a new child node is added to the current node, and the integration result of the parent node is updated and stored in the child node.

  For example, child nodes are added as follows. First, when it is determined that the reliability level of the new input from the estimator is “high” for all nodes currently in the latest layer, only one child node is added to the parent node. Using the input information as observation information, the state of the parent node is updated and stored in the child node. Further, the odometry information included in the new input is also stored in the child node.

  When it is determined that the reliability level is “low”, only one child node is added to the parent node. The input information is not used as observation information, and the state of the parent node is updated using only odometry information and stored in the child node.

  When it is determined that the reliability level is “medium”, the processing when the reliability level is determined to be “high” and “low” is performed, and as a result, the parent node has two children. .

  In this way, the number of nodes and the depth of the integrated tree increase as new inputs enter each time, and the scale of the tree increases.

Integrated tree pruning:
The pruning of the integrated tree is performed in order to keep the scale of the integrated tree below a certain level based on the evaluation based on the estimated path speed pattern. In order to prun the integrated tree, the goodness of the path from the latest node of the tree to the root node is evaluated. The latest node of the route with a low evaluation score is deleted. The route is a sequence of a plurality of nodes arranged in the order of time extracted from the node in the latest layer to the root node of the tree. A speed pattern is obtained from the odometry stored in each node on the route, and the similarity between this and the speed pattern obtained from the position and orientation of the robot obtained by the integration processing at each node is calculated as an evaluation score. Each route is sorted in ascending order by the evaluation score obtained by evaluating each route, and the latest node is deleted from the integrated tree for the lower route. After that, the entire integration tree is scanned to remove nodes that do not have children. Merge a parent node with only one child with a child node to create a new node. The new node loses the parent node information and stores only the child node information.

Diagnostic feature feedback:
In the feedback of the diagnostic function, the reliability evaluation result that the integrator performs for each estimator is output together with the integration result as a diagnosis result to the estimator. Thus, the estimator can check the diagnosis result and reset the estimator.

  As described above, in the disclosed map processing method and program, and in the robot system, the integrator evaluates the reliability of the input information from each estimator and associates it with the integration history in different ways according to different levels of reliability. . With this association, the integration tree representing the integration history is updated in time, and the scale increases. In addition, by evaluating an estimated route obtained by tracing the latest node of the integrated tree in the past and deleting lower nodes, the entire tree can be maintained and the tree scale can be maintained below a certain level. Furthermore, for example, the estimation result of an estimator with reduced estimation accuracy can be isolated from the integration process, so that the integration result is not affected by the estimation result of the estimator with reduced estimation accuracy, for example, improving navigation stability. Can do.

The following supplementary notes are further disclosed with respect to the embodiments including the above examples.
(Appendix 1)
An evaluation process for evaluating the reliability of a plurality of estimators that estimate self-positions based on inputs from different sensors using a reliability evaluation model corresponding to the plurality of estimators;
A map processing method, comprising: causing a computer to perform a combining process for performing different integration processes according to the reliability on estimation results from the plurality of estimators.
(Appendix 2)
The map processing method according to claim 1, wherein the reliability evaluation model is constructed by a hidden Markov model in which parameters are set in accordance with the performance of each estimator, and the parameters are input from each estimator. .
(Appendix 3)
In the evaluation process, the reliability is discretized into three levels, and an integration tree including a plurality of nodes is created to perform different integration processes depending on each level.
The depth direction of the integration tree corresponds to the time direction, the lowest layer corresponds to the latest time, and each node is the integration result at the corresponding time of the node and includes the position obtained as a result of the integration process. The map processing method according to appendix 1 or 2, which is characterized.
(Appendix 4)
The combining process includes
If all nodes in the latest layer of the integration tree have determined that the new input from one estimator has a high level of reliability, the parent node will have only one child node. And adding the input from the estimator as observation information to update the state of the parent node and storing it in the child node, storing the odometry information included in the new input in the child node,
When it is determined that the reliability level is “low”, only one child node is added to the parent node, and the input from the estimator is not used as observation information, and the state of the parent node is changed to odometry. Update using only the information of and save to the child node,
When it is determined that the reliability level is “medium”, the processing when the reliability level is determined as “high” and “low” is performed, respectively. As a result, the parent node has two children. The map processing method according to supplementary note 3, characterized by:
(Appendix 5)
From the odometry stored in each node on the route, an evaluation score for evaluating the goodness of the route formed by a sequence of a plurality of nodes arranged in order of time extracted from the latest node to the root node of the integration tree is extracted. A process of calculating from the similarity between the obtained speed pattern and the speed pattern obtained by the integration process at each node on the route;
The map processing method according to appendix 3 or 4, wherein the computer is further caused to perform a maintenance process for deleting the latest node on the route whose evaluation score is lower than a threshold value.
(Appendix 6)
The maintenance process deletes the latest node on the route, scans the entire integration tree to delete nodes that do not have children, and merges a parent node that has only one child with a child node. The map processing method according to appendix 5, wherein:
(Appendix 7)
An evaluation procedure for evaluating the reliability of a plurality of estimators that estimate self-positions based on inputs from different sensors using a reliability evaluation model corresponding to the plurality of estimators,
A program for causing a computer to execute a combination procedure for performing different integration processes according to the reliability on estimation results from the plurality of estimators.
(Appendix 8)
The program according to claim 7, wherein the reliability evaluation model is constructed by a hidden Markov model in which parameters are set in accordance with the performance of each estimator, and the parameters are input from each estimator.
(Appendix 9)
The evaluation procedure discretizes the reliability into three levels, and creates an integration tree including a plurality of nodes to perform different integration processing for each level,
The depth direction of the integration tree corresponds to the time direction, the lowest layer corresponds to the latest time, and each node is the integration result at the corresponding time of the node and includes the position obtained as a result of the integration process. The program according to appendix 7 or 8, which is characterized.
(Appendix 10)
The coupling procedure includes:
If all nodes in the latest layer of the integration tree have determined that the new input from one estimator has a high level of reliability, the parent node will have only one child node. And adding the input from the estimator as observation information to update the state of the parent node and storing it in the child node, storing the odometry information included in the new input in the child node,
When it is determined that the reliability level is “low”, only one child node is added to the parent node, and the input from the estimator is not used as observation information, and the state of the parent node is changed to odometry. Update using only the information of and save to the child node,
When it is determined that the reliability level is “medium”, the processing when the reliability level is determined as “high” and “low” is performed, respectively. As a result, the parent node has two children. The program according to appendix 9, characterized in that:
(Appendix 11)
From the odometry stored in each node on the route, an evaluation score for evaluating the goodness of the route formed by a sequence of a plurality of nodes arranged in order of time extracted from the latest node to the root node of the integration tree is extracted. A procedure for calculating from the similarity between the obtained speed pattern and the speed pattern obtained by the integration process at each node on the route;
The program according to appendix 9 or 10, further causing the computer to execute a maintenance procedure for deleting the latest node on the route whose evaluation score is lower than a threshold.
(Appendix 12)
The maintenance procedure deletes the latest node on the path, scans the entire integration tree to delete nodes that do not have children, and merges a parent node that has only one child with a child node. The program according to appendix 11, wherein:
(Appendix 13)
Different sensors provided on the autonomous mobile robot,
A plurality of estimators for estimating a self-position based on inputs from the different sensors;
An evaluation unit that evaluates reliability of the plurality of estimators using a reliability evaluation model corresponding to the plurality of estimators;
A robot system, comprising: a combining unit that performs different integration processing according to the reliability on estimation results from the plurality of estimators.
(Appendix 14)
14. The robot system according to appendix 13, wherein the reliability evaluation model is constructed by a hidden Markov model in which parameters are set in accordance with the performance of each estimator, and the parameters are input from each estimator.
(Appendix 15)
The evaluation unit discretizes the reliability into three levels, and creates an integration tree including a plurality of nodes in order to perform different integration processing depending on each level,
The depth direction of the integration tree corresponds to the time direction, the lowest layer corresponds to the latest time, and each node is the integration result at the corresponding time of the node and includes the position obtained as a result of the integration process. 15. The robot system according to appendix 13 or 14, which is characterized.
(Appendix 16)
The coupling portion is
If all nodes in the latest layer of the integration tree have determined that the new input from one estimator has a high level of reliability, the parent node will have only one child node. And adding the input from the estimator as observation information to update the state of the parent node and storing it in the child node, storing the odometry information included in the new input in the child node,
When it is determined that the reliability level is “low”, only one child node is added to the parent node, and the input from the estimator is not used as observation information, and the state of the parent node is changed to odometry. Update using only the information of and save to the child node,
When it is determined that the reliability level is “medium”, the processing when the reliability level is determined as “high” and “low” is performed, respectively. As a result, the parent node has two children. The robot system according to supplementary note 15, wherein
(Appendix 17)
From the odometry stored in each node on the route, an evaluation score for evaluating the goodness of the route formed by a sequence of a plurality of nodes arranged in order of time extracted from the latest node to the root node of the integration tree is extracted. An evaluation unit that calculates from the similarity between the obtained speed pattern and the speed pattern obtained by the integration process at each node on the route;
The robot system according to appendix 15 or 16, further comprising a maintenance unit that deletes the latest node on the route whose evaluation score is lower than a threshold.
(Appendix 18)
The maintenance unit deletes the latest node on the route, then scans the entire integration tree to delete nodes that do not have children, and merges a parent node that has only one child with a child node. 18. The robot system according to appendix 17, wherein:

  The disclosed map processing method and program, and robot system have been described above by way of examples. However, the present invention is not limited to the above examples, and various modifications and improvements can be made within the scope of the present invention. Needless to say.

4, 6, 8 Estimators 5, 7, 9 Storage unit 10 Integration unit 11 Estimator reliability evaluation unit 12 Coupling unit 13 Coupling evaluation unit 14 Integrated tree maintenance unit 15 Storage unit (integrated tree)
20 Odometry section

Claims (5)

And evaluation procedure for evaluating the reliability of a plurality of estimators for estimating its own position, using the reliability evaluation model corresponding to said plurality of estimators based on the input from different sensors,
The relative estimation results from multiple estimators, the reliability was performed multiple steps to divide the different reliability of the coupling procedure for different integration processing according to the level on the computer,
The reliability evaluation model includes a temporally updated integration tree in which the evaluated reliability and the integration history of the different integration processes are associated with each other.   The program according to claim 1, wherein the reliability evaluation model is constructed by a hidden Markov model in which parameters are set in accordance with performance of each estimator, and the parameters are input from each estimator. The evaluation procedure is the reliability discretized to the level of reliability of the three steps, create the integrated tree including a plurality of nodes in order to perform the different integrating process by each level,
The depth direction of the integration tree corresponds to the time direction, the lowest layer corresponds to the latest time, and each node is the integration result at the corresponding time of the node and includes the position obtained as a result of the integration process. The program according to claim 1 or 2, wherein the program is characterized. Different sensors provided on the autonomous mobile robot,
A plurality of estimators for estimating a self-position based on inputs from the different sensors;
An evaluation unit that evaluates reliability of the plurality of estimators using a reliability evaluation model corresponding to the plurality of estimators;
The relative estimation results from multiple estimators, provided with a coupling portion for performing the reliability of the corresponding to the level of reliability that is different divided into a plurality of stages different integration process,
The robot system according to claim 1, wherein the reliability evaluation model includes a temporally updated integration tree in which the evaluated reliability and the integration history of the different integration processes are associated with each other. And evaluation process for evaluating with the reliability of a plurality of estimators for estimating its own position, the reliability evaluation model corresponding to said plurality of estimators based on the input from different sensors,
The relative estimation results from multiple estimators, the reliability was performed multiple stages divided different reliability binding processing for different integration processing according to the level of the computer,
The map processing method, wherein the reliability evaluation model includes a temporally updated integration tree in which the evaluated reliability and the integration history by the different integration processing are associated with each other.

Images