Mobile body control system

Abstract

To achieve control for moving respective mobile bodies sharing a field to their respective destinations by achieving both safety assurance and low cost.SOLUTION: A mobile body control system provided with a plurality of mobile bodies having a control input determination part for destination movement for controlling movement to a destination, a control input correction part for correcting an input value to the control input determination part for destination movement, a deadlock detection part for detecting a deadlock, and a drive part for making automatic drive. In the mobile body control system, the control input correction part of the mobile body determines the necessity of avoidance movement if the deadlock detection part detects the deadlock, and generates an avoidance track on the basis of self position information, obstacle position information, other person robot position information and other person robot lock position information in the case of receiving an instruction of the avoidance movement from the control input correction part. An output part outputs self robot position information showing a self present position and a position which is on the generated avoidance track and the mobile body has not passed yet as self robot lock position information to other mobile bodies.SELECTED DRAWING: Figure 4

Description

[Technical field]

[0001] The present invention relates to a mobile object control system for controlling a plurality of mobile objects. [Background technology]

[0002] Against the backdrop of labor shortages, there are high hopes for the development of robots, vehicles, and UAVs (Unmanned Aerial Vehicles) that move autonomously on the ground or in the air in logistics warehouses, construction sites, airports, roads, etc. In these cases, since multiple moving objects exist in the same field, it is necessary to move each moving object safely to its designated destination without colliding with obstacles or other moving objects.

[0003] As a technology related to this field, there is a technology described in Patent Document 1. In this publication, the objective is to provide a robot cooperative transport planning technology that can reduce the calculation time and computer storage capacity required for planning calculations while considering the existence of multiple robots. As a means of solving this, it is described that the Markov state space is hierarchically configured (the first hierarchical level is the item trajectory calculation process, the second hierarchical level is the switching position determination process, and the third hierarchical level is the movement path planning process), and a search for the operation plan is performed from a hierarchical level with a low change frequency (the first hierarchical level), and the search range in a hierarchical level with a high change frequency is limited using the search calculation result in the hierarchical level with a low change frequency, and a search is performed in the lower hierarchical levels with a high change frequency (the second hierarchical level and the third hierarchical level) in the limited state space.

[0004] According to Patent Document 2, the objective is to provide a motion prediction control device and method that can generate a control command value at a determined control cycle without being affected by the time required for updating the internal state when estimating the internal state of one or both of an object and a robot and generating a predicted value (control command value) required for controlling the robot based on the internal state. To achieve the objective, the present invention provides a motion prediction control device that includes a measurement device that acquires sensor information by measuring one or both of the object and the robot, a state estimation device that predicts and updates the internal state of one or both of the object and the robot based on the sensor information, a data storage device that stores the internal state, and a robot control device that controls the robot, wherein the state estimation device updates the internal state at any timing that is not dependent on the control cycle of the robot, and the robot control device calculates a predicted value required for controlling the robot based on the latest internal state stored in the data storage device at the control cycle. [Prior art documents] [Patent documents]

[0005] [Patent Document 1] JP 2012-227349 A [Patent Document 2] International Publication No. 2012 / 153629 Summary of the Invention [Problem to be solved by the invention]

[0006] According to Patent Document 1, when the possible states of a plurality of robots are directly considered, the number of states becomes enormous. However, by creating a Markov transition model in which the state space is divided into finite parts and searching for a trajectory or action plan on the transition model, it is possible to reduce the calculation time and computer storage capacity required for the plan calculation. However, this known example is made assuming the cooperative operation of robots, and it is necessary to prepare an action plan for each robot in advance. Therefore, if a situation occurs during operation that differs from the action plan, such as a delay or interruption of some communication packets, it is necessary to stop all the robots once and prepare the action plan again. This reduces the availability of the entire system. In particular, with regard to robots and vehicles that move autonomously in logistics warehouses, construction sites, airports, roads, etc., not all robots or vehicles necessarily work in cooperation with each other, and as the number of robots or vehicles that constitute the robots increases and the communication volume increases, delays or interruptions of one or more packets can easily occur with high frequency. Therefore, this method cannot be applied because it seriously reduces the availability of the entire system.

[0007] On the other hand, according to Patent Document 2, a method is presented in which sensing information from outside the robot or information from other robots is predicted using a technique such as a Kalman filter before receiving the information, and the received information is reflected in the prediction as an observation result. This makes it possible to continue control by correcting the prediction even if a situation that differs from the initial plan occurs, such as a communication interruption. However, the calculation of such predictive control is complex, and verification tests must be performed under all conditions to confirm that correct calculation results are always obtained within a certain control period, which is costly. In addition, it is not self-evident how the correction of the prediction due to the occurrence of a situation that differs from the initial plan will affect each robot or the entire system. In particular, when multiple robots need to be considered at the same time, the number of combinations of states to be verified and tested becomes enormous, making it impossible to verify all of them.

[0008] In light of the above, the present invention aims to realize the missions (e.g., destination movement and surveillance) assigned to each moving object sharing a field while ensuring safety and at low cost, and the challenge is to maintain both safety assurance and low cost even when unexpected interruptions to communication packets occur or when the positions of obstacles in the field or the number of moving objects differ (change) from what was initially assumed. [Means for solving the problem]

[0009] In order to solve the above problems, a mobile body control system according to one embodiment of the present invention is a mobile body control system having a plurality of mobile bodies each having a control input determination unit for movement to a destination that controls movement to a destination, a control input modification unit that modifies a value input to the control input determination unit for movement to a destination, a deadlock detection unit that detects deadlock, and a drive unit that drives the mobile body, and each of the mobile bodies has a dynamic generation unit for finite state transition model for evasive movement and an output unit, and when the deadlock detection unit detects a deadlock, the control input modification unit of the mobile body determines that evasive movement is necessary and instructs the dynamic generation unit for finite state transition model for evasive movement, and when the dynamic generation unit for finite state transition model for evasive movement receives an instruction to make evasive movement from the control input modification unit, dynamically generates a finite state transition model for evasive movement based on self-position information, obstacle position information, other robot position information, and other robot lock position information to generate an avoidance trajectory, and the output unit outputs self robot position information indicating its current position and a position on the generated avoidance trajectory that the mobile body has not passed yet as self robot lock position information to other mobile bodies. Effect of the Invention

[0010] According to the present invention, by treating communication outages as non-deterministic transitions on a finite state transition model, it is possible to realize control of moving each mobile object sharing a field to its respective destination while ensuring safety and at low cost. Further features related to the present invention will become apparent from the description of the present specification and the accompanying drawings. Furthermore, the objects, configurations and effects other than those described above will become apparent from the following description of the embodiments. [Brief description of the drawings]

[0011] [Figure 1] FIG. 1 is a diagram showing the overall configuration of a control system 0 according to a first embodiment. [Diagram 2] FIG. 4 is a diagram showing a coordinate system in the present embodiment. [Diagram 3] FIG. 2 is a diagram showing an abstraction control system 6 in this embodiment. [Figure 4] FIG. 2 is a block diagram showing the functional configuration of the mobile object control system according to the present embodiment. [Diagram 5] FIG. 2 is a diagram showing an example of the structure of a mobile robot according to the present embodiment. [Figure 6] FIG. 2 is a diagram showing a part of a first finite state transition model 13 as seen from a first high-level control 7 in this embodiment. [Figure 7A] FIG. 13 is a diagram showing an example of source code for generating a first finite state transition model 13. [Figure 7B] FIG. 13 is a diagram showing an example of source code for generating a first finite state transition model 13. [Figure 7C] FIG. 13 is a diagram showing an example of source code for generating a first finite state transition model 13. [Figure 8] 1 is a diagram showing a part of a first directed graph 16 generated from a first finite state transition model 13 for generating a first high-level control 7 in this embodiment. [Figure 9] FIG. 2 is a diagram showing the shortest path and input sequence determined by the first high-level control 7 in this embodiment. [Figure 10] FIG. 13 is a diagram showing a communication packet 19 communicated between each mobile robot in this embodiment. [Figure 11] FIG. 2 is a diagram showing the processing performed by each mobile robot within each time step. [Figure 12] 4 is a pseudo code showing a determination of transmission or non-transmission in a safety check of a control input for the first mobile robot 3 in this embodiment. [Figure 13] This figure shows the state immediately before a deadlock occurs due to a safety check of the control input. [Figure 14] A diagram showing the state at the time when a deadlock occurs due to safety check of the control input. [Figure 15] FIG. 13 is a diagram showing a procedure for generating an avoidance trajectory. [Figure 16] FIG. 2 shows a lock position packet 20. [Figure 17] 15 is a diagram showing the avoidance movement of the third moving robot 5 after the third moving robot 5 in FIG. 14 generates an avoidance trajectory, and the movements of the other moving robots during that time. FIG. [Figure 18] FIG. 18 shows a lock position packet 20 transmitted by the third mobile robot 5 in the state shown in FIG. 17. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0012] This embodiment relates to a control system for controlling a plurality of moving objects. An example of a preferred embodiment (Example) of the present invention will be described below. In this example, an example in which three mobile robots are safely moved to a destination will be described, but the number of mobile robots may be other than three, the mobile robots may be vehicles, and the size of the area (field) in which the mobile robots move, the positions of obstacles, and the initial positions and destinations of each mobile robot are merely examples and are not limited to these.

[0013] <Overall configuration of control system 0> FIG. 1 is a diagram showing the overall configuration of a control system 0 in this embodiment. 1, the control system 0 includes a field 1, an obstacle 2, a first mobile robot 3, a second mobile robot 4, and a third mobile robot 5. In this embodiment, the size of the field 1 is 2 meters square, and the position of the obstacle 2 is as shown in the figure, with a width of 10 centimeters.

[0014] <Coordinate system> FIG. 2 is a diagram showing a coordinate system in this embodiment. The center of the 2 meter square field 1 is set as the origin, and the X-axis and Y-axis are set as shown in the figure. The angle indicating the direction of each mobile robot is expressed as a value between -π radians and +π radians, with counterclockwise angles from the X-axis being positive. The state of the first mobile robot 3 is expressed as (X_1, Y_1, θ_1), the state of the second mobile robot 4 as (X_2, Y_2, θ_2), and the state of the third mobile robot 5 as (X_3, Y_3, θ_3). As a result, the state of the control system 0 is expressed as ((X_1, Y_1, θ_1), (X_2, Y_2, θ_2), (X_3, Y_3, θ_3)). Therefore, the state space of control system 0 is a Cartesian product of six real numbers from -1 to 1 (X_1, X_2, X_3, Y_1, Y_2, Y_3) and three real numbers from -π to π (θ_1, θ_2, θ_3).

[0015] <Abstract Control System 6> FIG. 3 is a diagram showing the abstraction control system 6 in this embodiment. The abstract control system 6 is a model that reduces the state space of the control system 0. By using the abstract control system 6, it is possible to divide the control into a low level and a high level. In other words, the high level control creates a movement plan for the mobile robot and issues instructions to the low level control, and the low level control receives instructions from the high level control to drive the actuators.

[0016] In this embodiment, a grid with a width of 0.2 meters is formed in the X-axis and Y-axis directions, and each moving robot moves in a translational direction by ±0.2 meters. The direction of each moving robot is set to 0, π / 2 radians, π radians, or -π / 2 radians, and the rotation of each moving robot is set to ±π / 2 radians. At each time step (the time when the control period starts), the high-level control transmits to the low-level control one of the following instructions: translation of ±0.2 meters, rotation of ±π / 2 radians, or keeping the previous state without doing anything (hereinafter, this is called stop). The low-level control drives the actuator according to the instruction. Both the high-level control and the low-level control are installed in each moving robot. Thus, the first mobile robot 3 is equipped with a first high-level control 7 and a first low-level control 8, the second mobile robot 4 is equipped with a second high-level control 9 and a second low-level control 10, and the third mobile robot 5 is equipped with a third high-level control 11 and a third low-level control 12.

[0017] <System Functionality> FIG. 4 is a block diagram showing the overall functional configuration of the mobile object control system in this embodiment. As shown in Fig. 4, the mobile object control system includes multiple mobile objects 100. Other robot position information 201 and other robot lock position information 202 are input to each mobile object 100, and the mobile object 100 outputs own robot position information 206 and own robot lock position information 207. In the present invention, a locked position is a position on the field where a certain robot can enter exclusively. Each robot can specify a part of the field as a locked position by using a locked position packet 20, which will be described later.

[0018] The moving body 100 has a self-position acquisition unit 101, a control input determination unit 102 for moving to a destination, a control input correction unit 103, a deadlock detection unit 104, a dynamic generation unit 105 of a finite state transition system for avoidance movement, a driving unit 106, a self-lock position output unit 107, and a self-position output unit 108.

[0019] Each of the moving objects 100 is equipped with a CPU (Central Processing Unit) and memory (not shown), and the functions of the above-mentioned functional units are realized by the CPU executing various programs stored in the memory.

[0020] The self-position acquisition unit 101 acquires the coordinate position of the robot itself by using a GPS (Global Positioning System). The destination movement control input determination unit 102 calculates a movement route for moving from the current position to the destination from the acquired positional relationship between the self-position and the destination and obstacle position information 203, and determines the necessary control as the destination movement control input 204. The control input correction unit 103 calculates a route to be avoided from the destination movement control input 204, other robot position information 201, and other robot lock position information 202, and outputs it as the final control input 205.

[0021] The deadlock detection unit 104 detects a deadlock based on the output from the control input correction unit 103. The avoidance movement finite state transition system dynamic generation unit 105 calculates an avoidance path that allows the own robot to move to a destination while avoiding obstacles and other robots from the detection result of the deadlock detection unit 104, the own position, other robot position information 201, other robot lock position information 202, and obstacle position information 203, and outputs the avoidance path. The drive unit 106 drives the own robot based on a final control input 205 that is the sum of the outputs from the control input correction unit 103 and the avoidance movement finite state transition system dynamic generation unit 105. The own lock position output unit calculates the own lock position based on the output result from the avoidance movement finite state transition system dynamic generation unit 105, and outputs it as own robot lock position information 207. The own position output unit 108 outputs the own position as own robot position information 206.

[0022] <Low level control> The low-level control implemented in each mobile robot will now be described. FIG. 5 is a diagram showing the structure of a robot (in this figure, a first mobile robot 3) in this embodiment. The structure of each mobile robot is the same. The mobile robot is represented by a general two-wheel model, and the rotation amount of each wheel can be controlled independently by a servo motor. That is, a left servo motor 303 is connected to a left wheel 301, and a right servo motor 304 is connected to a right wheel 302. The left servo motor 303 and the right servo motor 304 have internal rotary encoders, and the motors can be rotated by a certain angle by giving a target position from outside the servo motors. In addition, low-level control and high-level control, which will be described later, are calculated and processed by software, and the software is implemented in a control microcomputer 305 mounted on the mobile robot.

[0023] When the first high-level control 7 commands a translation of +0.2 meters, the first low-level control 8 sets the target position for the left servo motor 303 to (current left servo motor position) +0.2 / (wheel radius: meters) and the target position for the right servo motor 304 to (current right servo motor position) +0.2 / (wheel radius: meters). This causes the left servo motor 303 and the right servo motor 304 to rotate in the same direction to the target positions, and the first mobile robot 3 advances 0.2 meters. When the first high-level control 7 commands a rotation of +π / 2 radians, the first low-level control 8 sets the target position for the left servo motor 303 to (current left servo motor position) -(π / 2)*(tread length: meters) / (wheel diameter: meters) and the target position for the right servo motor to (current right servo motor position) +(π / 2)*(tread length: meters) / (wheel diameter: meters). As a result, the left servo motor 303 and the right servo motor 304 rotate in the reverse direction to the target position, and the first mobile robot 3 rotates counterclockwise by π / 2 radians. Translation and rotation in the negative direction can also be controlled in the same way. When an instruction to maintain the previous state is received from the first high-level control 7, the first low-level control 8 does not change the target position of each servo motor. As a result, the first mobile robot 3 does not translate or rotate, and maintains the same state as the previous time. The second low-level control 10 and the third low-level control 12 in the second mobile robot 4 and the third mobile robot 5 also operate in the same way as the first low-level control 8, following instructions from the second high-level control 9 or the third high-level control 11.

[0024] <Time step> In this embodiment, the first mobile robot 3, the second mobile robot 4, and the third mobile robot 5 are time-synchronized with each other. In this embodiment, the mobile robots can communicate with each other, so they can be time-synchronized by using a known time synchronization method, such as NTP (Network Time Protocol). A time synchronization signal may also be provided from outside the mobile control system. As described above, in low-level control, each servo motor rotates to a designated position, but servo positioning control is performed using a known control method such as PID control, and it takes some time from when a target position is specified for a servo motor until the servo motor actually reaches the target position. Therefore, the time step is defined as a time that is longer than the maximum time required for the servo motor to reach the target position after a target position is specified for the servo motor.

[0025] The length of time from when a target position is specified for a servo motor until the servo motor reaches the target position generally depends on the difference between the current servo motor position and the target position, so the time step may be set to a value equal to or greater than the time from when a target position is specified for the control input that requires the longest positioning time among the five control input patterns (+0.2 meter translation, -0.2 meter translation, +π / 2 radian rotation, -π / radian rotation, stop) selected in low-level control until the servo motor reaches the target position. The time step is the time interval during which each mobile robot performs one of the actions of translation, rotation, or stop, that is, the control period. In this embodiment, the first mobile robot 3, the second mobile robot 4, and the third mobile robot 5 all have the same hardware configuration, and the time step is the same for all mobile robots.

[0026] <Reception process> At the beginning of a time step, the first mobile robot 3 (or the second mobile robot 4 or the third mobile robot 5) receives a communication packet 19 and a lock position packet 20 sent by the other mobile robots. The communication packet 19 and the lock position packet 20 will be described later.

[0027] <Finite state transition model> Each mobile robot that implements low-level control can be regarded as a finite state transition model from the perspective of high-level control. FIG. 6 is a diagram showing a part of the first finite state transition model 13 as seen from the first high-level control 7 in this embodiment.

[0028] The first finite state transition model 13 is a model showing the dynamics of the first moving robot 3 equipped with the first low-level control 8, as seen from the first high-level control 7. For example, consider the case where the state of the first moving robot 3 is (0, 0, 0) in FIG. 6. Here, when the first high-level control 7 instructs U1 (translation +0.2 meters), the first moving robot 3 moves +0.2 meters in the X-axis direction, and the state transitions to (0.2, 0, 0) in the next time step. When the first high-level control 7 instructs U2 (translation -0.2 meters), the first moving robot 3 moves -0.2 meters in the X-axis direction, and the state transitions to (-0.2, 0, 0) in the next time step, but this transition is not generated because an obstacle exists at the coordinates (-0.2, 0). When the first high-level control 7 instructs U3 (+π / 2 radian rotation), the first mobile robot 3 rotates +π / 2 radians, so the state in the next time step becomes (0, 0, π / 2), and when it instructs U4 (-π / 2 radian rotation), the state in the next time step becomes (0, 0, -π / 2). When the first high-level control 7 instructs U5 (no movement), the state in the next time step becomes (0, 0, 0). By repeatedly applying this operation to each state such as (0, 0, π / 2), (0.2, 0, 0), etc., a first finite state transition model 13 for the first mobile robot 3 is obtained. In the same manner, a second finite state transition model 14 and a third finite state transition model 15 for the second mobile robot 4 and the third mobile robot 5, respectively, are obtained.

[0029] 7A to 7C are diagrams showing examples of source code for generating the above-mentioned first finite state transition model 13. Note that in this embodiment, the source code created according to C++ will be described, but the type of source code is not limited to this.

[0030] <Generation of finite state transition models> A description will now be given of source code for generating the first finite state transition model 13. The second finite state transition model 14 and the third finite state transition model 15 are generated in exactly the same manner.

[0031] First, as shown in FIG. 7A, structures and arrays are defined. The structure plantstate has the coordinates x, y, and θ of the robot, and a pointer array that stores which state (i.e., another plantstate structure) to transition to when each input is applied. The structure input has a value to be input, a Boolean variable for distinguishing whether the input indicates rotation or translation, and a weight value for Dijkstra's algorithm (described in detail later. Used for high-level control). In addition, variables for storing x and y coordinates are prepared in the position structure. In the finite state transition model in this embodiment, the X and Y coordinates have 10 points in 0.2 increments, and θ has 4 points in π / 2 increments, but these are managed by corresponding them to integer values ​​from 0 to 10 or 0 to 4. For example, the coordinates (-0.6, 0.4, π / 2) are corresponding to (2, 7, 1). This operation of associating is called encoding, and the operation of associating (2, 7, 1) with coordinates (-0.6, 0.4, π / 2) is called decoding. Using these defined structures and encoding / decoding operations, array data is defined. The array plant

[11]

[11] [4] is used to express the first finite state transition model 13 as an array of plantstate structures. At the stage of Figure 6-1, the array is only declared, and the first finite state transition model 13 has not yet been expressed. Next, the array input[] stores the grid size, i.e., 0.2 meters of translation in the forward / backward direction and the rotation of the θ axis in the plus / minus π / 2 radian direction as input. Finally, the array obstacle[] holds the obstacle position encoded and described as a position structure. Note that the contents of the obstacle array in Figure 7A correspond to the obstacle position in Figure 3.

[0032] Next, as shown in FIG. 7B, in the first finite state transition model 13, a function transition() is defined for calculating which state to transition to when a certain input is applied in a certain state. The calculation results of the transition destination are stored in integer variables i, j, and k, respectively, and finally the address is returned as &plant[i][j][k]. First, if the input is a rotation instruction, i and j do not change, and only k changes. The calculation is performed using the decoded raw radian angle, and finally encoded into an integer value. Although not shown in FIG. 7B, in reality, in order to correctly manage the angle, if the decoded value is -π / 2 or less, 2π is added, and if the decoded value is greater than π / 2, 2π is subtracted. On the other hand, if the input is a translation instruction, k does not change, and i and j change. A value obtained by replacing the translation distance with the number of grids is stored in the variable trans_step. In this case, the translation is plus or minus 0.2 meters, and the grid size is 0.2 meters, so either +1 or -1 is stored. The value of trans_step is then added or subtracted for i or j, but whether i or j changes and whether it is added or subtracted depends on the direction the robot is facing. Therefore, the calculation is divided into four cases (positive and negative directions of the X and Y axes).

[0033] In Figure 7B, the case where the robot is facing the positive direction of the X axis is specifically described, and this will also be explained in the main text. First, a boundary check is performed. If the current X coordinate is 9 and trans_step is +1, the X coordinate should be 10, which is inappropriate because it will collide with a boundary (obstacle). Similarly, if the current X coordinate is 1 and trans_step is -1, the X coordinate should be 0, which is also inappropriate because it will collide with a boundary (obstacle). For such an inappropriate transition, transition() returns NULL. After confirming that the transition does not hit a boundary, the transition is executed. To check whether the transition is passing through an obstacle position, the obstacle check() function is executed each time i is incremented or decremented. The definition of this function is not shown in Figure 6-2, but it returns true if the argument coordinates (i, j, k), especially the pair (i, j), are not included in the array obstacle[], and returns false otherwise. After confirming that the obstacle position is not passed, the incremented or decremented i is used to return &plant[i][j][k]. If it is determined that the obstacle position is passed, it is inappropriate and NULL is returned.

[0034] Finally, in FIG. 7C, a first finite state transition model 13 is generated. This is realized by calculating the next transition destination for each element (state) of plant

[11]

[11] [4] using the function transition() and adding it to the pointer array. In other words, a triple loop is used. The encoded values ​​i, j, and k are stored as they are in x, y, and θ of element plant[i][j][k]. After confirming that the state (i, j, k) is not on the boundary, outside the boundary, or on an obstacle, the state transition is calculated for each input. There are NUMBER_OF_INPUTS inputs (5 in this case), so the For loop is executed 5 times. Note that a std::vector<struct plantstate*> plant[i][j][k].next[u] represents the transition destination when input[u] is applied. This is calculated by the function transition() shown in Figure 7B and stored in the pointer next. If the pointer next is not NULL, it indicates that a transition is possible, so the pointer is used as the transition destination and a std::vector<struct plantstate*> In this embodiment, as described later, when communication is interrupted (i.e. when communication packet 19 from another robot is not received), the low-level control cancels the high-level control and stops the robot in place to avoid collisions between the robots, and the state is maintained. Therefore, std::vector<struct plantstate*> In plant[i][j][k].next[u], not only next but also &plant[i][j][k] itself is registered. By the above operations, the first finite state transition model 13 is generated as a structure array that holds the transition destination as a pointer.

[0035] <Control Microcomputer 305> The control microcomputer 305 mounted on the robot is a device including a CPU and memory, and performs calculation processing for low-level control and high-level control. In this embodiment, the array plant

[11]

[11] [4], which is an implementation of the finite state transition model, includes three variables x, y, and θ for expressing its own state in each element, and may include a maximum of NUMBER_OF_INPUTS * 2 pointers for expressing transitions. If the required amount of memory cannot be secured, the finite state transition model will not be implemented correctly, the control will be indefinite, and safety will not be guaranteed, so it is important to estimate the required amount of memory. If the three variables x, y, and θ are each expressed as a 1-byte variable and one pointer is expressed as 4 bytes, the required amount of memory to express one finite state transition model is calculated to be (1 * 3 + 4 * 5 * 2) * 11 * 11 * 4 = 20812 bytes, or about 20 kilobytes. Assuming that a finite state transition model for avoidance movement, which will be described later, consumes about 20 kilobytes, a memory capacity of about 100 kilobytes in the control microcomputer 305 will be sufficient.

[0036] However, in this embodiment, the destination cannot be specified with an accuracy smaller than 20 centimeters. This is because all positions within a 20-centimeter square grid are considered to be the same in the abstraction control system 6. On the other hand, if the grid size of the abstraction control system 6 in FIG. 3 is reduced to one tenth and set to 2 centimeters, the accuracy of destination specification improves to 2 centimeters, but the implementation array of the finite state transition model becomes plant

[0101]

[0101] [4], and the memory required to express one finite state transition model is calculated to be (1*3+4*5*2)*101*101*4=1754572 bytes, or about 1.7 megabytes. If it is estimated that about 1.7 megabytes is consumed for the finite state transition model for avoidance movement described later, the control microcomputer 305 will require a memory amount of about 4 megabytes.

[0037] Generally, a microcomputer with a large memory capacity is more expensive than a microcomputer with a small memory capacity. Therefore, by modeling as a finite state transition model, it is possible to estimate the memory capacity required for the required control performance (precision of destination specification) and select a microcomputer that is necessary and sufficient for the estimation as the control microcomputer 305. Conversely, by determining the smallest grid size that fits within the memory capacity of a given control microcomputer 305, it is possible to achieve the maximum control performance under a given cost constraint.

[0038] FIG. 8 is a diagram showing a part of a first directed graph 16 generated from a first finite state transition model 13 for generating a first high-level control 7 in this embodiment. FIG. 9 is a diagram showing the shortest path and input sequence determined by the first high-level control 7 in this embodiment.

[0039] <High-level control> The high-level control will be described below. The first high-level control 7 plans a route for moving the first mobile robot 3 to the destination, and has a role of instructing the first low-level control 8 to translate, rotate, or stop. This can be obtained by applying a shortest path search on the first finite state transition model 13. That is, for the first finite state transition model 13, a first directed graph 16 is constructed by replacing the control inputs U1 to U5 with a weight 1, and the shortest path is obtained by applying the Dijkstra algorithm on the first directed graph 16. The first high-level control 7 instructs the first low-level control 8 to translate, rotate, or stop by associating the control input before replacing the first finite state transition model 13 with the weight 1 to this shortest path.

[0040] FIG. 9 shows the shortest path and input sequence obtained by the first high-level control 7 after applying the Dijkstra algorithm when the initial position is (0, 0, 0) and the destination is (0, 0.2, π / 2). First, in the initial state (0, 0, 0), the first high-level control 7 selects the control input U3 (+π / 2 radian rotation). This control input is executed by the first low-level control 8 to drive the servo motor, so that in the next time step, the state of the first mobile robot 3 transitions to (0, 0, π / 2). The first high-level control 7 selects the control input U1 (+0.2 meter translation), and in the next time step, the state of the first mobile robot 3 transitions to (0, 0.2, π / 2), and the robot arrives at the destination. The second directed graph 17 and the third directed graph 18 are also generated in the same manner, and the second high-level control 9 and the third high-level control 11 are generated.

[0041] FIG. 10 is a diagram showing a communication packet 19 communicated between each mobile robot in this embodiment.

[0042] <Communication packets> In this embodiment, the first mobile robot 3, the second mobile robot 4, and the third mobile robot 5 can communicate with each other. At the beginning of each time step, the received data is confirmed, the high-level control and the low-level control operate, and the data is transmitted at the end. Each mobile robot transmits one communication packet 19 at each time step. Each communication packet 19 is broadcast to all other mobile robots. For example, the communication packet 19 transmitted by the first mobile robot 3 includes the state of the first mobile robot 3 at the time step, the position of the first mobile robot 3 at the next time step to which the first mobile robot 3 will transition depending on the result of the control input selected by the first high-level control 7, and the target state of the first mobile robot 3. In this embodiment, in addition to the communication packet 19, a lock position packet 20 (described later) is also communicated between each mobile robot.

[0043] <Processing for each time step> FIG. 11 is a diagram showing the processing that each mobile robot performs within each time step. As mentioned above, each mobile robot is synchronized in time, and the time step set for each mobile robot is the same. FIG. 10 shows the process each mobile robot performs in each time step. First, at the start time of the time step, a communication packet 19 received from another mobile robot is acquired. Next, in the first mobile robot 3, the first high level control 7 (the second mobile robot 4 and the third mobile robot 5 are the second high level control 9 and the third high level control 11, respectively) determines one control input based on the path to reach the target state. Next, a "control input safety check" is performed to check whether or not the control input is to be transmitted to the first low level control 8 (the second mobile robot 4 and the third mobile robot 5 are the second low level control 10 and the third low level control 12, respectively). The control input safety check will be described later. After that, the first low level control 8 (the second mobile robot 4 and the third mobile robot 5 are respectively the second low level control 10 and the third low level control 12) controls the left servo motor 303 and the right servo motor 304, so that the first mobile robot 3 (or the second mobile robot or the third mobile robot 5) translates, rotates or stops. When the low level control ends, the high level control obtains the current state of the mobile robot and performs a state transition according to the obtained result. This state transition will be described later. At the end of the time step, the first mobile robot 3 (or the second mobile robot or the third mobile robot 5) sends a communication packet 19 to the other mobile robot.

[0044] <Checking the safety of control inputs> FIG. 12 is a pseudo code showing a determination of transmission or non-transmission in a safety check of a control input for the first mobile robot 3 in this embodiment. The role of the control input safety check is to determine whether or not to transmit the control input by the high-level control to the low-level control in each mobile robot. If it is determined to transmit, the control input by the high-level control is passed to the low-level control as is, and if it is determined not to transmit, the control input by the high-level control is invalidated and the control input is replaced with stop and transmitted to the low-level control. Figure 12 shows pseudocode showing a specific method of determining the safety check of the control input. If there is a shortage of packets from other mobile bodies, it is determined that there is no safety and it is immediately determined not to transmit the control input to the low-level controller (replace with stop). If no packet interruption has occurred, the following process is performed. First, the state of the first mobile robot 3 in the next time step when the control input determined by the first high-level control 7 is input as is is calculated and stored in next_tmp_state.

[0045] Next, a safe state for the first moving robot 3 is calculated. A safe state is a subset of the entire state space, and is a set of states where safety is ensured. In this embodiment, a safe state refers to a position where other moving robots, i.e., the second moving robot 4 and the third moving robot 5, cannot exist. On the other hand, an unsafe state refers to a position where at least one of the other moving robots, i.e., the second moving robot 4 or the third moving robot 5, may exist. Since the safe state and the unsafe state are complementary, the unsafe state is considered this time. That is, an array unsafe_list is created, and the states included in the received communication packet 19 and the lock position packet 20 (described later) are registered.

[0046] In this embodiment, the current state, the next state, and the lock position of each of the second mobile robot 4 and the third mobile robot 5 are registered. Finally, each state registered in the unsafe_list is checked, and if a state in which the X coordinate and the Y coordinate of next_tmp_state match is included, the control input determined by the first high level control 7 is not safe, i.e., FALSE is returned, and the control input determined by the first high level control 7 is not used, but the control input is instructed to the first low level control 8 to stop. If at least one of the X coordinate or the Y coordinate of next_tmp_state is different from all states registered in the unsafe_list, the control input determined by the first high level control 7 is judged to be safe, and the control input determined by the first high level control 7 is transmitted to the first low level control 8 as it is. The safety of each of the second mobile robot 4 and the third mobile robot 5 is also confirmed in a similar manner.

[0047] FIG. 13 shows the state immediately before a deadlock occurs due to a safety check of the control input. FIG. 14 is a diagram showing a state at the time when a deadlock occurs due to a safety check of a control input.

[0048] <Avoidance trajectory generation when stopping repeatedly> Depending on the position and the target state of each robot, multiple robots may stop to prevent collisions with each other when checking the safety of the control input, resulting in a deadlock. In Fig. 13, the second mobile robot 4 attempts to move to a target state ahead of the third mobile robot 5, and the third mobile robot 5 attempts to move to a target state ahead of the second mobile robot 4. As a result, as shown in Fig. 14, the self-positions of the second mobile robot 4 and the third mobile robot 5 overlap with the next desired positions, resulting in a deadlock. Note that the first mobile robot 3 continues moving as usual.

[0049] In order to resolve the deadlock, at least one of the second mobile robot 4 and the third mobile robot 5 needs to temporarily generate an avoidance trajectory. Therefore, if a stop (unsafety) occurs in the safety confirmation of the control input consecutively for a certain threshold or more, an avoidance trajectory is generated. This threshold may be, for example, three times, or may be any other positive integer. Also, if a stop occurs due to packet loss, it may not be included in the count of the number of consecutive stops. This makes it possible to prevent unnecessary generation of an avoidance trajectory when the timing of packet loss is merely overlapped and no deadlock occurs between the mobile bodies. Also, even if an avoidance trajectory is not necessarily generated just because the threshold is exceeded, a mechanism may be included that probabilistically determines whether or not to generate an avoidance trajectory when the threshold is exceeded. This allows only one of the robots to generate an avoidance trajectory and the other robot to avoid deviating from the original destination.

[0050] FIG. 15 is a diagram showing a procedure for generating an avoidance trajectory. First, in step S1, all positions included in obstacle[] and the sum of the self positions and lock positions of other parties received by the moving object are stored in array obstacle2[].

[0051] Next, in step S2, a finite state transition model for avoidance movement is generated. The finite state transition model for avoidance movement is a model similar to the first finite state transition model 13. This model differs from the first finite state transition model 13 in that while the first finite state transition model 13 prohibits transitions to obstacle positions, the finite state transition model for avoidance movement prohibits transitions to positions where other mobile robots currently exist and positions currently locked (occupied) by other mobile robots in addition to transitions to obstacle positions. This can be generated by replacing the references to obstacle2[] in the transition() and plant_init() functions used when generating the first finite state transition model 13 with references to obstacle2[] generated in step S1. This replacement may be implemented by copying transition() and plant_init() to define alias functions transition2() and plant_init2(), and rewriting the references to obstacle[] in transition2() and plant_init2() to reference obstacle2().

[0052] In step S3, a temporary destination for generating an avoidance trajectory is set. When an avoidance trajectory is generated, there is a high possibility that the destination will move away from the original destination, so a location as close to the current position as possible may be set. In addition, probabilistic behavior such as random number generation may be included in setting this destination. By including probabilistic behavior, even if the generation of an avoidance trajectory fails once because the avoidance trajectory overlaps with that of another mobile robot, it is expected that the avoidance will be successful by generating a different avoidance trajectory the next time.

[0053] In step S4, a route to the destination determined in step S3 is generated on the finite state transition model for avoidance movement generated in step S2. This can be done by performing a shortest route search using the Dijkstra algorithm, as in the case of FIG.

[0054] Finally, in step S5, it is confirmed on the finite state transition model for avoidance movement whether the destination determined in step S3 can actually be reached from the current position of the moving body by the route generated in step S4. If the confirmation result shows that the destination can be reached, the generation of the avoidance trajectory is terminated. If the destination cannot be reached, it is found that the destination determined in step S3 is inappropriate, so the process returns to step S3 and steps S4 and S5 are carried out again.

[0055] In the time step in which the avoidance trajectory is generated, the mobile body does not enter the avoidance trajectory and remains stopped. This is because the avoidance trajectory generated by the other side that needs to avoid deadlock may overlap with the avoidance trajectory generated by the mobile body itself. Therefore, in the safety check of the control input in the time step next to the time step in which the avoidance trajectory is generated, the communication packet 19 and the lock position packet 20 (described later) received from the other mobile body are analyzed, and the avoidance trajectory generated by the mobile body itself is confirmed not to overlap with these, and then the mobile body enters the avoidance trajectory. If an overlap occurs, the avoidance trajectory is generated again. In this way, the avoidance trajectories are prevented from overlapping with each other, and the safety of the mobile robot is guaranteed while moving on the avoidance trajectory. Such generation and regeneration of the avoidance trajectory realizes negotiation between the mobile robots, so to speak, in which the mobile robots generate their own avoidance trajectories to pass each other, and judge whether the generated avoidance trajectory is adoptable or not by looking at the information of the other mobile robots. This allows each mobile robot to avoid collisions and deadlocks by itself without making an overall plan for all the mobile robots in the field.

[0056] <Lock Position Packet 20> FIG. 16 is a diagram showing the lock position packet 20. The locked position packet 20 is a packet indicating a position on the avoidance trajectory generated by the mobile robot transmitting the locked position packet 20 and that has not yet been passed, and is transmitted to all other mobile robots. For the explanation of the locked position packet 20, as an example, in FIG. 14, the third mobile robot 5 is in the state of (0.8, -0.2, -π / 2), and as a result of the avoidance trajectory generation procedure shown in FIG. 15, a temporary destination for avoidance (step S3) is specified as (0.6, 0, -π / 2), and in step S4 of FIG. 15, a control input U4 (-π / 2 radian rotation) is input at (0.8, -0.2, -π / 2) to transition to (0.8, -0.2, π), and then the temporary destination is specified as (0.8, -0.2, π). At (0.6, -0.2, -π / 2), a control input U1 (+0.2 meter translation) is input, causing a transition to (0.6, -0.2, π), at (0.6, -0.2, π), a control input U3 (+π / 2 radian rotation) is input, causing a transition to (0.6, -0.2, -π / 2), and at 0.6, -0.2, -π / 2), a control input U2 (-0.2 meter translation) is input, causing a transition to (0.6, 0, -π / 2), thereby generating an avoidance trajectory, and also considering a case in which the second mobile robot 4 does not generate an avoidance trajectory.

[0057] FIG. 17 is a diagram showing the avoidance movement of the third moving robot 5 after the third moving robot 5 generates an avoidance trajectory in FIG. 14, and the movements of the other moving robots during that time. FIG. 18 shows the lock position packet 20 transmitted by the third mobile robot 5 in the state shown in FIG.

[0058] In Fig. 17, the third moving robot 5 is in a state of (0.6, -0.2, π) following the avoidance trajectory, and next receives control input U3 to transition to (0.6, -0.2, -π / 2). While the moving robot is moving on the avoidance trajectory, the information in the communication packet 19 sent by the moving robot will be information following the avoidance trajectory. That is, the information contained in the communication packet 19 sent by the third moving robot 5 in the state of Fig. 17 is the own robot state (0.6, -0.2, π), own robot next state (0.6, -0.2, -π / 2), and own robot target state (0.6, 0, -π / 2).

[0059] Also, in this state, the third moving robot 5 has already reached the position (0.6, -0.2) and this information is recorded in the communication packet 19, so the first moving robot 3 and the second moving robot 4 will not take a control input that will cause them to reach (0.6, -0.2) at the next time due to the safety check of the control input. Therefore, there is no need to declare (0.6, -0.2) as the locked position. Therefore, the locked position packet 20 transmitted by the third moving robot 5 at this time includes only (0.6, 0), which is an unpassed position on the avoidance trajectory, as shown in FIG. 18. In this way, by sequentially releasing the positions that have been passed on the avoidance trajectory starting from the locked position, all locked positions are released when the third moving robot 5 reaches the end of the avoidance trajectory, (0.6, 0, -π / 2).

[0060] <High-level control state transition> The state transition of the high-level control is a process of updating the current state of the first mobile robot 3 (or the second mobile robot 4 or the third mobile robot 5) in the first finite state transition model 13 (or the second finite state transition model 14 or the third finite state transition model 15) shown in Fig. 6. In this embodiment, when a certain control input is applied in a certain state, the transition destination is not determined in advance, and if it is determined to be safe by a safety check of the control input, the robot transitions to the next state, and if not, the robot remains in the same state (stops).

[0061] For example, consider a time step when the state of the first mobile robot 3 is (0, 0, 0) in FIG. 6. At this time, as shown in FIG. 8, assume that the first high level control 7 instructs a control input U3 (+π / 2 radian rotation). Here, if the safety check of the control input at this time step determines that it is safe, the control input U3 is applied to the first mobile robot 3, so the state of the first finite state transition model 13 transitions to (0, 0, π / 2), and if not, it remains at (0, 0, 0). Note that the state transition of the first finite state transition model 13 may be determined based on the result of the safety check of the control input, that is, whether it is safe or not, as well as based on information on the current position and orientation acquired using a GPS for acquiring position information and an IMU (Inertial Measurement Unit) for acquiring orientation information for the first mobile robot 3. That is, in the previous example, the first finite state transition model 13 may transition to either (0, 0, π / 2) or (0, 0, 0), but if the position information detected by the GPS is (0.00123, 0.00311) and the IMU is a Z-axis angle of 0.0001 radians, it can be determined that the first finite state transition model 13 should transition to (0, 0, 0).

[0062] <Send process> The first mobile robot 3 (or the second mobile robot 4 or the third mobile robot 5) transmits a communication packet 19 and a lock position packet 20 at the end of the time step.

[0063] According to this embodiment, the state space of each mobile robot is divided into a finite number of states in the same manner and modeled as a finite state transition model, and a safety confirmation process is performed on the control input. If it is unsafe, the control input is overwritten with a stop signal. Therefore, a communication interruption can be treated as a non-deterministic transition on the finite state transition model, and control can be safely continued even if a communication interruption occurs. Furthermore, when an obstacle position / number of moving objects is changed, the finite state transition model can be automatically regenerated by updating the array obstacle[] indicating the obstacle position information in Figures 7A to 7C to a new obstacle position, and a change in the number of moving objects only changes the number of communication packets 19 and lock position packets 20 sent and received, and does not affect the control at all. This has the effect of making it possible to build a controller whose safety is theoretically guaranteed without requiring much labor.

[0064] According to the embodiment of the present invention described above, the following advantageous effects are obtained. (1) A mobile body control system according to one embodiment of the present invention is a mobile body control system having a plurality of mobile bodies each having a control input determination unit for movement to a destination that controls movement to a destination, a control input modification unit that modifies a value input to the control input determination unit for movement to a destination, a deadlock detection unit that detects deadlock, and a drive unit that drives the mobile body, wherein each of the mobile bodies has a dynamic generation unit for finite state transition model for evasive movement and an output unit, wherein the control input modification unit of the mobile body determines that evasive movement is necessary when the deadlock detection unit detects a deadlock and instructs the dynamic generation unit for finite state transition model for evasive movement, and when the dynamic generation unit for finite state transition model for evasive movement receives an instruction to make evasive movement from the control input modification unit, dynamically generates a finite state transition model for evasive movement based on self-position information, obstacle position information, other robot position information, and other robot lock position information to generate an avoidance trajectory, and the output unit outputs self robot position information indicating its current position and a position on the generated avoidance trajectory that the mobile body has not passed through as self robot lock position information to other mobile bodies.

[0065] With the above configuration, by treating communication outages as non-deterministic transitions on a finite state transition model, it is possible to control the movement of each mobile object sharing a field to its respective destination while ensuring safety and at low cost.

[0066] (2) When the control input correction unit detects a possibility of a collision with another robot, it instructs the drive unit to stop the moving body on which the control input correction unit is mounted, regardless of the control input determined by the destination movement control input determination unit. As a result, when there is a possibility of a collision with the other robot, stopping is given priority over moving along the path, making it possible to reliably avoid a collision with the other robot.

[0067] (3) The control input correction unit stops the self-moving body when it attempts to enter a position included in the other robot position information and the other robot lock position information, and the deadlock detection unit detects a deadlock when the moving body stops a certain number of times in succession. A single stop does not necessarily indicate a deadlock, so it is preferable to detect a deadlock when the moving body stops a certain number of times in this manner.

[0068] (4) When the control input correction unit fails to receive at least one of the other robot position information or the other robot lock position information, the control input correction unit instructs the drive unit to stop the moving body on which the control input correction unit is mounted, regardless of the control input determined by the destination movement control input determination unit. When the control input correction unit fails to receive information in this way, there is a possibility that the positional relationship between the self moving body and the other robot cannot be accurately grasped, and therefore such a stopping measure is taken to avoid a collision.

[0069] (5) An overwrite of a stop command by the control input correction unit due to a failure to receive other robot position information or other robot lock position information is not included in the count of the number of consecutive stops for the deadlock detection unit to determine whether a deadlock has been detected. This makes it possible to prevent erroneous detection of a deadlock.

[0070] (6) The control input correction unit compares the received other robot position information and other robot locked position information with the self-locked position generated by a dynamic generation unit of a finite state transition model for evasive movement included in the moving body on which the control input correction unit is mounted and transmitted to the other robot, and if the received position information and the self-locked position do not overlap, determines a control input in accordance with the finite state transition model for evasive movement generated by the dynamic generation unit of the finite state transition model for evasive movement and applies the control input to the drive unit, and if the received position information and the self-locked position overlap, discards the finite state transition model for evasive movement and applies a stop command to the drive unit and instructs the dynamic generation unit of the finite state transition model for evasive movement to regenerate the finite state transition model for evasive movement. This makes it possible to control the moving body while constantly monitoring whether the generated finite state transition model for evasive movement is one that avoids collision with the other robot.

[0071] (7) The destination movement control input determination unit and the control input correction unit each have a high-level controller, and each of the moving bodies has a low-level controller, and the high-level controller creates an abstract state space by dividing the state space of the moving body into a finite number of states, generates a finite state transition model based on which state a transition occurs when each control input is applied in each state in the abstract state space, plans a moving route to the destination on the generated finite state transition model, and determines whether the control input should be translation or rotation based on the plan, and the low-level controller controls a motor or engine or other device that generates a driving force to move the moving body included in the drive unit so that each moving body translates or rotates according to an instruction from the high-level controller. The moving body control of the present invention is specifically executed in this manner.

[0072] (8) The finite state transition model for avoidance movement generated by the dynamic generation unit for the finite state transition model for avoidance movement is obtained by prohibiting a state transition to a lock position received from another moving body in the configuration of the finite state transition model in the high-level controller. The finite state transition model for avoidance movement in the present invention is specifically generated in this manner.

[0073] (9) The high-level controller included in each of the moving bodies constructs an abstract state space in the same manner, and the other robot position information, own robot position information, other robot locked position information, and own robot locked position information transmitted and received by each moving body are described as position information in the abstract state space. This makes it possible to handle the position information simply as coordinate information.

[0074] (10) Each moving object has a control microcomputer, a high-level controller is implemented on the control microcomputer, and the control microcomputer has a memory area of ​​a size equal to or larger than that required for implementing the finite state transition model. A microcomputer with such specifications is required to generate the finite state transition model for avoidance movement.

[0075] (11) The high-level controller plans a route to the destination by considering the finite state transition model generated by the high-level controller as a weighted directed graph and applying dynamic programming. This allows weighting of each route, making it possible to plan a route while taking into account the priority of routes.

[0076] (12) The avoidance trajectory is generated by the dynamic generation unit of the finite state transition model for avoidance movement, and the destination to be set when applying dynamic programming is selected probabilistically by random number generation. By adopting such a method, it becomes possible to easily generate the avoidance trajectory.

[0077] (13) The finite state transition model is represented by a structure array, and each structure, which is an element of the structure array, stores the position and angle of at least one moving body, and a pointer to a structure corresponding to the state to which the moving body can transition when each control input is applied. This makes it possible to visually grasp the finite state transition model for avoidance movement, and allows for easy processing.

[0078] The present invention is not limited to the above-described embodiments, and various modifications are possible. For example, the above-described embodiments have been described in detail to clearly explain the present invention, and the present invention is not necessarily limited to an embodiment having all of the configurations described. It is also possible to replace a part of the configuration of one embodiment with the configuration of another embodiment. It is also possible to add the configuration of another embodiment to the configuration of one embodiment. It is also possible to delete a part of the configuration of each embodiment, or to add or replace another configuration. [Explanation of symbols]

[0079] 0 Control system, 3, 4, 5 Mobile robot, 100 Mobile body, 101 Self-position acquisition unit, 102 Control input determination unit for moving to a destination, 103 Control input correction unit, 104 Deadlock detection unit, 105 Dynamic generation unit of finite state transition system for avoidance movement, 106 Driving unit, 107 Self-lock position output unit, 108 Self-position output unit

Claims

1. A movement control system including a plurality of moving bodies having a control input determination unit for destination movement that controls movement to a destination, a control input correction unit that corrects a value input to the control input determination unit for destination movement, a deadlock detection unit that detects a deadlock, and a drive unit that drives itself, wherein each of the moving bodies has an avoidance movement finite state transition model dynamic generation unit and an output unit, wherein when the deadlock detection unit detects a deadlock, the control input correction unit in the moving body determines that avoidance movement is necessary and instructs the avoidance movement finite state transition model dynamic generation unit, wherein when the avoidance movement finite state transition model dynamic generation unit receives an instruction for avoidance movement from the control input correction unit, it dynamically generates an avoidance movement finite state transition model based on its own position information, obstacle position information, other robot position information, and other robot lock position information, and generates an avoidance trajectory, and the output unit outputs its own robot position information indicating its current position and a position on the generated avoidance trajectory that the moving body has not passed as its own robot lock position information to other moving bodies. A movement control system characterized by the above.

2. The movement control system according to claim 1, wherein when the control input correction unit detects a possibility of collision with another robot, regardless of the control input determined by the control input determination unit for destination movement, it instructs the drive unit to stop the moving body on which the control input correction unit is mounted. A movement control system characterized by the above.

3. The movement control system according to claim 2, wherein when the self-moving body attempts to enter a position included in the other robot position information and the other robot lock position information, the control input correction unit stops the moving body, and the deadlock detection unit detects a deadlock when the stop of the moving body occurs continuously a certain number of times or more. A movement control system characterized by the above.

4. The movement control system according to claim 3, wherein when the control input correction unit fails to receive at least one of the other robot position information or the other robot lock position information, regardless of the control input determined by the control input determination unit for destination movement, it instructs the drive unit to stop the moving body on which the control input correction unit is mounted. A movement control system characterized by the above.

5. The movement control system according to claim 4, Overwriting of the stop instruction by the control input correction unit due to failure to receive the other robot position information or the other robot lock position information is not included in the continuous stop count for deadlock detection determination by the deadlock detection unit. A movement control system characterized by the above.

6. The movement control system according to claim 5, The control input correction unit compares the received other robot position information and the other robot lock position information with the self-lock position generated by the avoidance movement finite state transition model dynamic generation unit included in the moving body on which the control input correction unit is mounted and transmitted to the other robot. If the received position information and the self-lock position do not overlap, the control input is determined according to the avoidance movement finite state transition model generated by the avoidance movement finite state transition model dynamic generation unit, and the control input is applied to the drive unit. If the received position information and the self-lock position overlap, the avoidance movement finite state transition model is discarded, a stop command is applied to the drive unit, and the avoidance movement finite state transition model dynamic generation unit is instructed to regenerate the avoidance movement finite state transition model. A movement control system characterized by the above.

7. The movement control system according to claim 6, The destination movement control input determination unit and the control input correction unit each have a high-level controller. Each of the moving bodies has a low-level controller. The high-level controller constructs an abstract state space obtained by dividing the state space of the moving body into a finite number of states, generates a finite state transition model based on which state the moving body will transition to when each control input is applied in each state in the abstract state space, plans a movement path to the destination on the generated finite state transition model, and determines whether to make the control input translational or rotational based on the plan. The low-level controller controls a motor or an engine included in the drive unit or other device that generates a driving force for moving the moving body so that each moving body translates or rotates according to the instructions of the high-level controller. A movement control system characterized by the above.

8. The movement control system according to claim 7, The finite state transition model for avoidance movement generated by the finite state transition model dynamic generation unit for avoidance movement is obtained by prohibiting a state transition to a locked position received from another moving object in the configuration of the finite state transition model in the high-level controller. A movement control system characterized by this.

9. The movement control system according to claim 8, wherein the high-level controllers included in each of the moving objects construct the abstract state space by the same method, and the other robot position information, the own robot position information, the other robot locked position information, and the own robot locked position information transmitted and received by each moving object are described as information on positions on the abstract state space. A movement control system characterized by this.

10. The movement control system according to claim 9, wherein each of the moving objects has a control microcomputer, the high-level controller is implemented on the control microcomputer, and the control microcomputer has a memory area larger than the size required for implementing the finite state transition model. A movement control system characterized by this.

11. The movement control system according to claim 10, wherein the planning of the movement route to the destination by the high-level controller is obtained by applying a dynamic programming method by regarding the finite state transition model generated by the high-level controller as a weighted directed graph. A movement control system characterized by this.

12. The movement control system according to claim 11, wherein the generation of the avoidance trajectory by the finite state transition model dynamic generation unit for avoidance movement probabilistically selects a destination set when applying the dynamic programming method by random number generation. A movement control system characterized by this.

13. The movement control system according to claim 12, wherein the finite state transition model is represented by a structure array, and each of the structures that are elements of the structure array stores at least the position and angle of one moving object and a pointer to the structure corresponding to the state of the moving object that can transition when each control input is applied. A movement control system characterized by this.

Images