Control device, mobile body, control system, and control method

Abstract

A calculation condition setting unit sets a first constraint condition on a mobile body on the basis of the operation characteristics of the mobile body (FC102), when another mobile body is present outside a prescribed range from the mobile body. When the other mobile body is present in the prescribed range from the mobile body and a planned trajectory of said other mobile body has been received (FC103: YES), the calculation condition setting unit sets a second constraint condition, wherein the distance between the position of the mobile body and the position of said other mobile body on the planned trajectory should be greater than a threshold value, on the mobile body (FC104). When the other mobile body is present in the prescribed range from the mobile body, the planned trajectory of said other mobile body is not received, and the speed at which said other mobile body moves has been acquired (FC105: YES), the calculation condition setting unit sets a third constraint condition, wherein the distance between the position of the mobile body and the position of said other mobile body based on the speed at which the other mobile body moves should be greater than a threshold value, on the mobile body (FC106). A prediction control calculation unit controls the mobile body so as to satisfy the set constraint conditions.

Description

Control device, mobile object, control system, and control method

[0001] The present invention relates to a control device, a mobile object, a control system, and a control method.

[0002] To alleviate labor shortages in transporting goods in logistics warehouses and between processes in factories, mobile robots and other moving objects (e.g., automated guided vehicles (AGVs) and autonomous mobile robots (AMRs)) are increasingly being introduced. Introducing such moving objects requires the creation of paths within warehouses and factories that the moving objects can navigate (creating a graph consisting of nodes and edges, for example). The more detailed this configuration, the greater the freedom of the paths that the moving objects can choose, enabling more efficient transportation. However, this configuration work requires a large number of man-hours. Furthermore, because the path configuration work is required every time the layout of a warehouse or factory changes, the work of configuring detailed paths places a heavy burden (engineering costs) on the operators who manage the warehouse or factory.

[0003] Furthermore, in warehouses and factories, items are sometimes temporarily placed in the aisles, making it difficult to obtain accurate map information. This issue can be shared by other industrial sectors as well. For example, when dealing with autonomous vehicles traveling on public roads, obtaining accurate map information is difficult due to environmental changes such as the installation of pylons and signs caused by temporary road closures due to road construction. Similarly, when dealing with automated construction machinery (hydraulic excavators, wheel loaders, dump trucks, etc.) used at construction sites and mines, map information changes constantly as excavation work is carried out.

[0004] In response to this issue, Patent Document 1 (paragraph

[0039] ) states, "Once the movement control device 10 acquires information on the constraint conditions, vehicle positions, and target positions, it solves a constrained optimization problem and calculates control inputs for M vehicles (step S16). The movement control device 10 calculates the movement of the vehicles that satisfies the constraint conditions using the number of look-ahead steps NPH. In other words, at each step of the movement routes of all vehicles, it evaluates the deviation between the vehicle and the target position, and calculates the movement route for all vehicles that minimizes the deviation."

[0005] Patent Publication No. 2021-77090

[0006] According to Patent Document 1, it is possible to calculate a route for a moving object to reach a target position without designing detailed passage information in a warehouse or factory in advance. In addition, because the dynamics of multiple moving objects are taken into consideration and a movement plan is calculated to prevent the moving objects from coming into contact with each other, it is expected that a route with superior movement efficiency will be generated compared to a method of individually optimizing the route of each moving object.

[0007] However, since Patent Document 1 requires solving the movement plans of multiple moving bodies simultaneously, the calculation time becomes enormous as the number of moving bodies increases, and there is a possibility that routes cannot be calculated in real time.

[0008] Furthermore, although Patent Document 1 assumes the target position of the moving object, it does not assume the target position of the worker. In other words, since it does not take into account the destinations of people who transport and collect items in warehouses and factories, there is a possibility that the moving object will impair the workability of the people.

[0009] The present invention has been made to solve the above-mentioned problems, and aims to provide a control device etc. that can perform control to efficiently avoid collisions.

[0010] In order to achieve the above object, a control device of one example of the present invention comprises a calculation condition setting unit that, when another moving body is present outside a predetermined range from the moving body, sets a first constraint condition on the moving body based on the operating characteristics of the moving body; when the other moving body is present within the predetermined range from the moving body and a planned trajectory of the other moving body is received, sets a second constraint condition on the moving body that the distance between the position of the moving body and the position of the other moving body on the planned trajectory is greater than a threshold; and when the other moving body is present within the predetermined range from the moving body, the planned trajectory of the other moving body is not received, and the speed at which the other moving body is moving is acquired, sets a third constraint condition on the moving body that the distance between the position of the moving body and the position of the other moving body based on the speed at which the other moving body is moving is greater than the threshold; and a predictive control calculation unit that controls the moving body to satisfy the set constraint conditions.

[0011] According to the present invention, it is possible to perform control for efficiently avoiding collisions. Problems, configurations, and effects other than those described above will become apparent from the following description of the embodiments.

[0012] 5a。 FIG. 5b is a functional block diagram related to a moving body according to an embodiment of the present invention. FIG. 5c is a diagram illustrating an embodiment (embodiment 1) in which the present invention is applied to a transport robot. FIG. 5d is a diagram illustrating a transport robot. FIG. 5e is a diagram illustrating a configuration for performing distributed processing. FIG. 5f is a diagram illustrating a state in which another moving body is outside the illumination range of a LiDAR. FIG. 5g is a diagram illustrating a state in which another moving body is within the illumination range of a LiDAR, but the movable ranges of each moving body do not overlap. FIG. 5g is a flowchart illustrating condition branching in setting calculation conditions. FIG. 5h is a diagram illustrating the positional relationship between the host vehicle and another vehicle under "Condition 2". FIG. 5h is a diagram illustrating the positional relationship between the host vehicle and another vehicle at time k=2. FIG. 5h is a diagram illustrating the positional relationship between the host vehicle and another vehicle under "Condition 3". FIG. 5j is a diagram superimposing a predicted trajectory and the trajectory shown in FIG. 5a. FIG. 5j is a diagram illustrating the trajectory of the host vehicle. FIG. 5k is a diagram illustrating a situation in which "Condition 4" can occur. FIG. 5k is a diagram illustrating a state in which a worker comes into contact with the host vehicle. FIG. 5k is a diagram illustrating a method for the host vehicle to avoid an obstacle under "Condition 4". FIG. 5j is a flowchart illustrating the calculation of a control device mounted on a moving body. FIG. 5k is a diagram illustrating an embodiment (embodiment 2) in which the present invention is applied to an air vehicle. FIG. 5k is a diagram illustrating contact determination between air vehicles.

[0013] An autonomous control system calculates a movement plan (time series of coordinates, posture, speed, etc.) for a moving object (a controllable moving object such as a robot or vehicle) within a specific area (for example, a warehouse or parking lot) and controls the moving object so that it follows this path plan. A movement plan is time-series information about the movement of a moving object. The movement plan includes, for example, the coordinates to which the moving object moves, the posture of the moving object as it moves, and the speed at which the moving object moves.

[0014] An embodiment of an autonomous control system according to the present invention will now be described with reference to the drawings. The autonomous control system according to the present embodiment executes a movement plan in real time that prevents multiple moving bodies from colliding with people or with each other, even in an environment where people are present.

[0015] Figure 1 is a simplified example of a functional block diagram of a mobile object that constitutes an autonomous control system according to an embodiment of the present invention. Note that Figure 1 summarizes the functions of one mobile object to be controlled, and that each mobile object has similar functions.

[0016] First Embodiment: In-Warehouse Transportation In the following, for ease of explanation, the target is a logistics warehouse shown in FIG. 2, and a transportation robot is assumed as the moving body.

[0017] <Warehouse Configuration> In this embodiment, it is assumed that the moving object B100 moves on a travel-permitted passage B101, which is a passage on which no shelves B102 are installed.

[0018] The mobile object B100 can load luggage and transport the luggage to a designated shelf, or assist in the task of retrieving the luggage from the shelf where it is stored. Loading and unloading of luggage can be performed by a worker B103 or by the mobile object B100 itself.

[0019] The destination location to which the mobile object B100 should move (corresponding to the coordinates of the shelf where the package is delivered or collected) is managed by a management server B104. The management server B104 may be installed in the logistics warehouse or in another location. The destination location determined by the management server B104 is transmitted to the mobile object B100 via a wireless communication device B105 installed in the warehouse.

[0020] An infrastructure sensor B106 is installed in the logistics warehouse and is capable of monitoring the position of the mobile object B100. The infrastructure sensor B106 may be a camera or a LiDAR (Light Detection and Ranging). Information acquired by the infrastructure sensor B106 can be provided to the mobile object B100 and the management server B104 via a wireless communication device B105. In this configuration, the infrastructure sensor B106 may be treated as the environment recognition device A002 in this embodiment.

[0021] The worker B103 also has a terminal (smartphone or tablet) for receiving the work details (the location of the baggage to be collected and the ID of the mobile object B100 carrying the baggage) from the management server B104.

[0022] <Configuration of Mobile Body> For simplicity of explanation, it is assumed that the mobile body B100 is a differential two-wheel type mobile body as shown in Fig. 3a. However, it should be noted that the mobile body B100 used in the present invention is not limited to a differential two-wheel type robot, and various types of mobile bodies such as an omni-wheel type robot, a forklift, a four-wheel vehicle, and a towing vehicle can be used.

[0023] The equation of motion for a differential two-wheel robot can be given by Equation 1. Here, x and y are the x and y coordinates of the robot, θ is the orientation (direction) of the robot, v is the robot's moving speed, and ω is the robot's angular velocity. Note that in the control design for a moving object, Equation 2 is used, which is obtained by discretizing Equation 1 with a sampling period Δt. Note that k in the equation means the processing step (time).

[0024]

[0025]

[0026] The moving object is equipped with sensors such as an encoder that detects the number of rotations of the wheels, an inertial sensor (IMU) that acquires the rotational speed and acceleration of the vehicle body, and LiDAR.

[0027] Encoders and IMUs measure the state of the moving object itself, so they correspond to the state recognition device A001, and LiDAR measures the state around the moving object, so they correspond to the environment recognition device A002. Note that when LiDAR is used to calculate the position and speed of a moving object using SLAM (Simultaneous Localization And Mapping), the LiDAR can also be treated as the state recognition device A001.

[0028] Furthermore, the mobile object is equipped with a communication device A003 that complies with communication standards such as Bluetooth (registered trademark) and Wi-Fi (registered trademark), and is able to communicate with surrounding mobile objects and a management server B104.

[0029] The mobile object uses information acquired by the state recognition device A001, the environment recognition device A002, and the communication device A003 to perform various calculations in the control device A100 and drive the actuator A004. The actuator A004 corresponds to a traction motor. The control device A100 is, for example, a microcomputer, and is composed of a storage device such as a memory, a processor such as a central processing unit (CPU), input / output circuits, etc.

[0030] <Configuration for Distributed Processing> In this embodiment, the mobile object generates its own movement path and performs tracking control along the generated path. For this purpose, each mobile object has the functions shown in FIG.

[0031] More specifically, as shown in Figure 3b, each of the M mobile bodies has the functions shown in Figure 1, and is configured to perform distributed processing in which it communicates via the management server B104, communicates directly between vehicles using the communication device A003, recognizes each other using the environment recognition device A002, treats other mobile bodies as obstacles, and controls itself.

[0032] This distributed processing configuration makes it possible to suppress the increase in the amount of calculations required by each control device A100 even if the number of mobile units increases, and also to suppress the impact on the entire system if a specific mobile unit fails.

[0033] Returning to Figure 1, the functional blocks of the mobile unit shown in Figure 2 for the logistics warehouse will now be explained in detail.

[0034] <Explanation of Functional Blocks of Mobile Objects> The autonomous control system is a system for safely and efficiently controlling multiple mobile objects B100. In order to control a large number of mobile objects B100 in real time, the processing is executed in an autonomous and distributed manner by the control device A100 installed in each mobile object B100. For this reason, it should be noted that in this embodiment, the travel routes of the mobile objects B100 are not centrally managed by the management server B104.

[0035] The control device A100 mounted on each moving body B100 comprises a trajectory prediction unit A101, a received trajectory correction unit A102, a calculation condition determination unit A103, a movement path calculation unit A104, and a planned trajectory provision unit A105.

[0036] The trajectory prediction unit A101 calculates a predicted trajectory of a moving object according to the positions and velocities of surrounding moving objects acquired by the environment recognition device A002, and the orientation calculated from the time-series data of these.

[0037] The predicted trajectory is calculated using the position coordinates (xdk, ydk), velocity vdk, and direction θdk at time k according to the prediction formula (Formula 3). Note that although Formula 3 predicts the trajectory of a specific moving object, it goes without saying that the same formula can be used to calculate the predicted trajectories of multiple moving objects.

[0038]

[0039] <Trajectory prediction based on moving object classification> If the environment recognition device A002 is equipped with a function to determine the classification of a detected moving object (another robot or a worker), the prediction formula used may be changed according to this classification. Since Equation 3 is a motion equation for a general moving object, it cannot move sideways (at an orientation of 90 degrees relative to the robot's orientation θ), but a worker can also move sideways. For this reason, when the moving object is determined to be a worker, it is desirable to calculate the velocity vx in the X direction and the velocity vy in the y direction without calculating the orientation, and then use these to calculate the predicted trajectory using Equation 4.

[0040]

[0041] The reception trajectory correction unit A102 is a function that corrects the movement routes of other moving bodies received via the communication device A003. As will be described in more detail later, each moving body calculates its movement route up to an arbitrary future time N × Δt and provides this information to surrounding moving bodies, where N is the prediction step, as will be described later.

[0042] The path of travel begins at the time ka when the other moving body calculates its path, and includes the x and y coordinates, velocity v, and angular velocity ω of the object to be reached at the time ka + Δt × i (i = 0, ..., N) for each calculation cycle Δt.

[0043] The reception trajectory correction unit corrects the received path information according to the time difference between the time (timestamp) of the movement path received from other moving bodies and the clock built into its own control device. The specific correction method uses velocity and angular velocity information to apply Equation 5, which corrects the time difference Δtd in the timestamps to the equation of motion in Equation 1. Furthermore, after correction using Equation 5 is complete, position information is calculated in control period Δt units using Equation 2. Note that this correction process is not necessary if the clocks of the other moving bodies and its own control device are completely synchronized.

[0044]

[0045] Furthermore, if the moving entity is a worker, this process is skipped since a predicted route is not provided.

[0046] The calculation condition determination unit A103 determines the calculation conditions of the movement path calculation unit A104 (described later) according to the surrounding conditions acquired from the environment recognition device A002 and the communication device A003.

[0047] The branching of the calculation conditions will be explained according to the flowchart of FIG.

[0048] In addition, in order to simplify the explanation, FIG. 4 is limited to the explanation assuming that there is only one other moving body other than the own body.

[0049] The operation when multiple moving objects of different categories (robots, workers) are mixed together will be described later.

[0050] <Explanation of the flowchart of the calculation condition determination unit> First, in FC101, it is confirmed whether there are other moving objects (robots, workers) within a predetermined range. If there are no other moving objects (YES), the process moves to FC102, and if there are other moving objects (NO), the process moves to FC103.

[0051] The "predetermined range" refers to the illumination range of the LiDAR of the installed environment recognition device A002, or the distance that the host vehicle and other vehicles can reach within a specified time when moving at maximum speed (Figure 3d). Specifically, if there is another moving object outside the illumination range, as shown in Figure 3c, FC101 will determine YES. Also, as shown in Figure 3d, even if another moving object is within the illumination range of the sensor, if the ranges that each moving object can move within the specified time T at its maximum moving speed vmax do not overlap, FC101 will also determine YES.

[0052] In addition, the presence or absence of moving objects can be confirmed by observing obstacles with LiDAR, by establishing communication with each moving object, or by using the location information of each moving object provided by the management server.

[0053] Whether an obstacle detected by the environment recognition device A002 is a moving object or a stationary object is determined by comparing it with stationary objects (walls and shelves) registered in the management server B104.

[0054] When the process transitions to FC103, it checks whether a planned trajectory is provided by the other moving object confirmed in FC101. If a planned trajectory is provided (YES), the process transitions to FC104, and if a planned trajectory is not provided (NO), the process transitions to FC105.

[0055] If the other moving object is a worker, it does not have the control device A100 and therefore does not provide a planned trajectory. Therefore, if the environment recognition device A002 determines that the moving object is a worker, it may immediately transition to FC105.

[0056] When transitioning to FC105, it is checked whether it is possible to acquire the movement speed of the other moving object confirmed by FC101. If the other moving object is a robot, it is desirable to have a mechanism for providing the information acquired by the state recognition device A001 via the communication device A003.

[0057] Even if the speed cannot be obtained from the communication device A003, the speed can be calculated using the time series data of the acquired position information using the environment recognition device A002.

[0058] If the object is determined to be moving in FC101 but the acquired position information is not updated using the environment recognition device A002, the moving speed cannot be calculated, and the process proceeds to FC107.

[0059] "Condition 1" (FC102) to "Condition 4" (FC107) in the flowchart of Figure 4 correlate with how freely the robot can decide its own actions.

[0060] In condition 1, there are no other moving objects in the vicinity, so it is possible to implement a movement plan that takes into account only the robot's own movements and maximizes movement efficiency.

[0061] Condition 2 is that there are other moving objects in the vicinity, but the path that the moving object will take and the time it will take to pass through that path are known, so the robot can plan its own movements to avoid coming into contact with it.

[0062] In condition 3, only the speed of other moving objects at that time is known, which means there is a high degree of uncertainty compared to condition 2, and it becomes necessary to plan one's own actions while taking into account the movements of other vehicles.

[0063] In condition 4, since it is impossible to predict how other moving bodies will move, the moving body is forced to take conservative actions to avoid coming into contact with other moving bodies.

[0064] To summarize the above conditions, each time a condition branch is reached (the condition number increases), a movement plan that takes into account cooperation with other moving bodies is required.

[0065] The movement path calculation unit A104 is composed of a calculation condition setting unit A104a and a predictive control calculation unit A104b.

[0066] The calculation condition setting unit A104a changes the formulation and parameters used for the model predictive control (MPC) to be solved by the predictive control calculation unit A104b (described later) in accordance with conditions 1 to 4 determined by the calculation condition determination unit A103. With model predictive control, time-series data of control inputs (speed, angular velocity) up to the prediction horizon can be calculated, so the movement path can be calculated by integrating the control inputs starting from the current position.

[0067] The predictive control calculation unit A104b calculates the control input uk according to the MPC concept and under the formulation set by the calculation condition setting unit A104a.

[0068] The formulation of MPC for each condition will be explained below.

[0069] <Condition 1> When "Condition 1" is met, there are no other moving objects in the vicinity, so only the object needs to plan its own movement. To achieve this, the evaluation function J1 shown in Equation 6 is constructed using the deviation ek between the object's own position and orientation vector Xk and the target position and orientation vector rk, as well as the control input uk. The target position and orientation vector rk is provided by the management server B104. In Equation 6, Q1 and R1 are weighting parameters, and N1 is the prediction horizon. In general, in model predictive control, it is said that the longer the prediction horizon N1, the more appropriate the control input calculated.

[0070]

[0071] In MPC, in order to evaluate the evaluation function J up to the interval of the evaluation function (behavior N1 steps from the current time k), it is necessary to calculate the future behavior of the mobile unit B100 itself. Predictions of the behavior of the mobile unit B100 up to N steps ahead can be calculated recursively by inputting the control input uk calculated at each step into Equation 2.

[0072] MPC has the advantage of being able to easily handle constraints. For example, in the case of the differential two-wheel robot shown in Figure 3, the upper and lower limits of the robot's movement speed v and turning angular velocity ω depend on the rotational speeds ωl and ωr of the left and right wheels of the travel motor. By incorporating the operating characteristics of these actuators as constraints, it becomes possible to generate a path that the robot can actually follow. An MPC that takes such constraints into account can be expressed as in Equation 7. Note that u* represents the optimized control input, and st is an abbreviation for subject to, meaning that the optimization problem is solved under the conditions from st onwards. The subscripts max and min correspond to the upper and lower limits, respectively.

[0073]

[0074] In "Condition 1," only the behavior of the vehicle itself needs to be considered, so the calculation load is lighter than in the formulations from "Condition 2" onwards. For this reason, it is possible to take a longer prediction step N1.

[0075] <Condition 2> When "Condition 2" occurs, it is necessary to calculate the behavior of the mobile unit on the condition that it does not come into contact with other mobile units whose movement paths are known.

[0076] Regarding "Condition 2," the condition for not coming into contact with other moving objects will be explained using Figure 5.

[0077] Figure 5a shows an example of the movement trajectories of the subject vehicle (control target) and the other vehicle (obstacle) from time k=0 to k=3, three time periods in the future. Given the trajectory o01 of the other vehicle, the subject vehicle generates a trajectory o02 so as not to come into contact with the other vehicle. In Figure 5a, the other vehicle at time k=1 and the subject vehicle at time k=3 overlap, so it appears as if they are in contact, but please note that no contact occurs because the times do not match.

[0078] Figure 5b shows the relative positions of the subject vehicle and the other vehicle at time k=2. If the center position of the subject vehicle is (x2, y2), the radius of the circle surrounding the subject vehicle is r1, the center position of the other vehicle is (xd2, yd2), and the radius of the circle surrounding the other vehicle is r2, then the distance d2 between the vehicles at time k=2 satisfies the inequality in Equation 8, which is equivalent to the two robots not coming into contact. By incorporating such conditions into the constraints of the MPC, it is possible to achieve collision avoidance between robots. While Figure 5 only considers the conditions for collision avoidance between two robots, it goes without saying that similar conditions can be applied even if the number of robots is increased to three or more.

[0079]

[0080] In "Condition 2," a vehicle plans its own movements using route information provided by other vehicles. Therefore, the longest prediction horizon N2 to be considered is the step length Nd of the provided route. Therefore, as shown in Equation 9, the evaluation function uses the smaller of the pre-designed horizon length Np and the horizon Nd provided by the other vehicle. Note that the pre-designed prediction horizon Np is preferably shorter than N1 in Equation 6. This is because the calculation time required to avoid contact with other vehicles is longer. Furthermore, the weight parameters Q2 and R2 in Equation 9 may be the same as the weight parameters Q1 and R1 in Equation 6.

[0081]

[0082] Under the above preparations, the predictive control calculation unit A104b for "Condition 2" calculates the control input by performing the optimization calculation of Equation 10. The constraint condition of Equation 10 is an extension of Equation 8, assuming that there are m moving objects.

[0083]

[0084] <Condition 3> When "Condition 3" occurs, it is necessary to calculate the behavior of the robot itself, mainly on the condition that it does not come into contact with other moving bodies, the only information being known being their current moving speed.

[0085] Regarding "Condition 3," the condition for not coming into contact with other moving objects will be explained using FIG.

[0086] In "Condition 3," only the position (xd0, yd0), direction θd0, and speed vd0 of the other vehicle at time k=0 are known, so the other vehicle's position at time k≧1 can only be estimated using Equation 3. From the condition at time k=0, we can predict the behavior of the other vehicle moving in a uniform straight line, as shown in Figure 6a.

[0087] Figure 6b shows this predicted trajectory overlaid on the trajectory shown in Figure 5. As shown in this figure, the predicted trajectory o12, which uses only information from a specific time (k = 0), often does not match the actual trajectory o11. For this reason, using a long-term predicted trajectory to plan the vehicle's trajectory may result in an inappropriate trajectory. For example, if the vehicle's trajectory is calculated according to the predicted speed trajectory three steps ahead shown in Figure 6a, a trajectory like trajectory o13 in Figure 6c may be generated. If the other vehicle behaves in the same way as in Figure 5a from time k ≥ 1 onwards, the vehicle will not come into contact with the other vehicle even if it continues moving straight, so avoidance maneuvers downward on the page can be said to reduce movement efficiency.

[0088] As explained above, when the routes of other vehicles are unknown, long-term predictions may result in less efficient behavior. Taking this situation into consideration, "Condition 3" uses an evaluation function J3 that uses a prediction horizon N3 that is shorter than the prediction horizons N1 and Np used in "Condition 1" and "Condition 2," respectively.

[0089] Since the conditions for avoiding contact between the host vehicle and another vehicle are the same as those for "Condition 2," the predictive control calculation unit A104b for "Condition 3" calculates the control input by performing optimization calculations for the problem in which the evaluation function J2 in Equation 10 is replaced with J3 in Equation 11.

[0090]

[0091] <Condition 4> When "Condition 4" occurs, in a situation where only the position of the moving object is known, it is necessary to calculate its own behavior on the condition that it does not come into contact with other moving objects.

[0092] "Condition 4" corresponds to a situation where the vehicle is traveling near a worker who is storing items on a shelf, as shown in Figure 7. The environment recognition device A002 installed in the vehicle B100 allows the vehicle B100 to recognize the presence of a moving object (worker), but because the worker is working in front of the shelf, the moving speed is not detected.

[0093] Assuming that the moving speed is 0, and performing the same calculation as in "Condition 3," if the vehicle travels behind a worker as in Figure 7a, as in Figure 7b, if worker B103 backs up or turns around without noticing the approach of the vehicle B100, there is a possibility that the vehicle will come into contact with the vehicle B100.

[0094] To avoid such a situation, if the speed of a moving object cannot be detected, it is desirable to take action to avoid approaching the moving object.

[0095] To incorporate this type of behavior into the formulation of model predictive control, the inequality in Equation 12 can be considered to maintain the distance dk between the circle of radius r1 surrounding the worker, which includes a safety margin α in addition to the radius r2 surrounding the worker, as shown in Figure 8, and the circle of the vehicle with radius r1 so that they do not come into contact.

[0096]

[0097] As with Condition 3, Condition 4 is unlikely to generate appropriate behavior even if long-term predictions are made. For this reason, it is desirable that the prediction horizon N4 of the evaluation function J4 (Equation 13) used be equal to or shorter than the prediction horizon N3 of Condition 3.

[0098]

[0099] To summarize the above explanation, under "Condition 4", the control input is calculated by performing optimization calculations on Equation 14.

[0100]

[0101] <Processing when multiple moving bodies exist> Up to this point, for simplicity of explanation, we have assumed a situation where there is only one other moving body or one person. From now on, we will explain the processing when there are multiple moving bodies.

[0102] Even if there are multiple other moving objects, if there are no moving objects around the vehicle, the calculation of condition 1 will be performed. Therefore, the explanation will be given assuming that the transition to FC103 has occurred in the flowchart of Figure 4.

[0103] If there are multiple moving objects around the vehicle, the checks of FC103 and FC105 are performed on all moving objects to determine which of "Condition 2" to "Condition 4" applies to each moving object.

[0104] The prediction horizon N is set to the condition with the highest condition number among the conditions for each moving object. For example, if there are three moving objects, each determined to be in "Condition 2," "Condition 3," and "Condition 4," the prediction horizon is set to N4.

[0105] The predicted trajectory for collision avoidance conditions is calculated according to each condition, and the constraint conditions for collision avoidance are also adjusted according to each condition. However, the prediction step is limited to the common prediction horizon. Following the example above, N4 is used. Therefore, please note that even if a planned trajectory longer than N4 is received from a moving object that meets "Condition 2," only up to N4 steps will be used in the calculation.

[0106] The above explanation can be summarized in Equation 15. As an example, assume that the first moving object (i=1) is in "Condition 2," the second moving object (i=2) is in "Condition 3," and the third moving object (i=3) is in "Condition 4." Under these conditions, the prediction horizon of evaluation function 15a is the shortest, N4.

[0107]

[0108] The distance di,k between the vehicle and each moving object is calculated in 15b regardless of the conditions. However, the coordinates (xd1,k, yd1,k) of the moving object i=1 use the received planned trajectory, but the coordinates (xd2,k, yd2,k) of the moving object i=2 are predicted using Equation 3 or Equation 4. Because the moving object i=3 has no speed, the acquired position (xd3,k, yd3,k) is used without updating, even within the prediction step.

[0109] As the condition for preventing contact between the own vehicle and another vehicle, the same inequalities 15c and 15d are used for the moving bodies of i=1 and i=2, but for the moving body of i=3 that corresponds to "Condition 4", inequalities 15e that takes into account the safety margin α are used.

[0110] The same constraints 15f and 15g regarding the moving speed and angular velocity of the vehicle are always applied regardless of the conditions.

[0111] Returning to FIG. 1, the function of the control device A100 will be described.

[0112] When the predictive control calculation unit A104b completes calculation of the time series of the control input (velocity v, angular velocity w), it sends an instruction to the actuator A004 to realize the first time portion of this time series.

[0113] The planned trajectory providing unit A105 calculates the planned trajectory that the host vehicle will take from now on, using the time series of control inputs calculated by the predictive control calculation unit A104b. The planned trajectory is a set of coordinates that the host vehicle will pass through over the prediction horizon Ni (i = 1...4) determined by the calculation condition setting unit A104a at control period Δt intervals, that is, up to Δt × Ni ahead. By providing this set of coordinates to surrounding moving bodies, other moving bodies can use it for their own control.

[0114] <Flowchart of the Overall Control Device> The processing procedure of the autonomous control system explained above will be explained using the flowchart of FIG.

[0115] First, FC201 updates the information from various sensors mounted on the moving body B100. This process corresponds to the process of acquiring information from the state recognition device A001 and the environment recognition device A002. When FC201 is completed, the process transitions to FC202.

[0116] In FC202, it is confirmed whether inter-vehicle communication is possible with non-controllable mobile bodies B100 that are located around the controlled mobile body B100. This process corresponds to the process of acquiring information about non-controllable mobile bodies by the communication device A003. Note that if direct communication between vehicles is not possible, this process may be treated as this process if indirect communication is possible via the management server B104. When FC202 ends, the process transitions to FC203.

[0117] FC203 judges the calculation conditions according to the surrounding conditions acquired by FC201 and FC202. This process corresponds to the calculation condition judgment unit A103. The details of the processing of FC203 are as shown in the flowchart in Figure 4. When FC203 is completed, the process transitions to FC204.

[0118] In FC204, if the calculation condition decision unit A103 determines that "Condition 1" is met (YES), the process proceeds to FC205. If "Condition 1" is not met (NO), the process proceeds to FC206.

[0119] In FC205, calculation conditions are set in accordance with the conditions determined in the calculation condition determination unit A103. This process corresponds to the calculation condition setting unit A104a. When the process in FC205 is completed, the process transitions to FC210.

[0120] In FC206, if the calculation condition decision unit A103 determines that "Condition 2" is met (YES), the process proceeds to FC207. If "Condition 2" is not met (NO), the process proceeds to FC208.

[0121] When the process transitions to FC207, the planned trajectory of the moving object B100, which is not the object of control, has been received, so trajectory correction processing is performed on this planned trajectory. This processing corresponds to the received trajectory correction unit A102. When the processing of FC205 is completed, the process transitions to FC205.

[0122] In FC208, if the calculation condition decision unit A103 determines that "Condition 3" is met (YES), the process proceeds to FC209. If "Condition 3" is not met (NO), the process proceeds to FC205.

[0123] When the process transitions to FC209, the current position and speed of the moving body B100, which is not the object of control, have been acquired, so this information is used to predict the trajectory of the moving body B100, which is not the object of control. This process corresponds to the trajectory prediction unit A101. When the process of FC209 is completed, the process transitions to FC205.

[0124] FC210 determines the control input by solving the optimization problem according to the calculation conditions set in FC205. This process corresponds to the predictive control calculation unit A104b. When the processing of FC210 is completed, the process transitions to FC211.

[0125] In FC211, it is confirmed whether inter-vehicle communication has been established in FC202. If inter-vehicle communication has been established (YES), the process proceeds to FC212. If inter-vehicle communication has not been established (NO), the process for one control cycle is completed.

[0126] FC212 uses the time series of control inputs calculated by FC210 to calculate the trajectory that the vehicle will travel in the future and provides this trajectory to other vehicles via wireless communication. This process corresponds to the planned trajectory providing unit A105. When FC212 is completed, the processing for one unit of the control cycle is completed.

[0127] The above process is repeated for each control period.

[0128] The main features of the first embodiment can be summarized as follows.

[0129] The control device A100 includes a calculation condition setting unit A104a and a predictive control calculation unit A104b (Figure 1). As shown in Figure 4, when another moving object (e.g., a robot, a person, etc.) is present outside a predetermined range from the moving object B100 (Figure 1) (FC101: YES), the calculation condition setting unit A104a sets a first constraint condition (Condition 1, st in Equation 7) on the moving object based on the operating characteristics (e.g., the performance of the actuator A004) of the moving object (FC102). When the calculation condition setting unit A104a detects that another moving object is present within a predetermined range from the moving object and receives a planned trajectory of the other moving object (FC103: YES), the calculation condition setting unit A104a sets a second constraint condition (Condition 2) on the moving object, in which the distance dk between the position (xk, yk) of the moving object and the position (xdk, ydk) of the other moving object on the planned trajectory is greater than a threshold value (r1 + r2) (FC104). If another moving body is present within a predetermined range from the moving body, the calculation condition setting unit A104a has not received the planned trajectory of the other moving body, and has acquired the moving speed (speed during movement) of the other moving body (FC105: YES), the calculation condition setting unit A104a sets a third constraint condition (condition 3) for the moving body, which states that the distance between the position of the moving body and the position of the other moving body based on the moving speed of the other moving body (Figure 6c) is greater than a threshold value (FC106). The predictive control calculation unit A104b (Figure 1) controls the moving body so as to satisfy the set constraint condition.

[0130] By setting constraints on a moving object according to available information about other moving objects (e.g., planned trajectory, speed, etc.), it is possible to perform control to efficiently avoid collisions. The first constraint (condition 1) does not require avoiding collisions with other moving objects, so the computational load is low. The second constraint (condition 2) uses the positions of other moving objects on the planned trajectory, so the accuracy of the positions of other moving objects is high. The third constraint (condition 3) makes it easy to calculate the positions of other moving objects because the positions are based on the speeds at which the other moving objects are moving.

[0131] As shown in Figure 1, the predictive control calculation unit A104b calculates a control input for the moving body B100 so as to satisfy the constraint conditions. The control device A100 includes a planned trajectory providing unit A105. The planned trajectory providing unit A105 calculates a planned trajectory for the moving body based on the control input. When communication is established between the moving body and another moving body (e.g., a robot), the planned trajectory providing unit A105 provides the planned trajectory of the moving body to the other moving body via the communication device A003.

[0132] This allows other moving bodies to use the planned trajectory of the moving body.

[0133] As shown in Figure 8, if there is another moving body (e.g., a person) within a predetermined range of the moving body B100, the calculation condition setting unit A104a has not received the planned trajectory of the other moving body, and the other moving body is stopped (FC105: NO, Figure 4), the calculation condition setting unit A104a sets a fourth constraint condition (condition 4) for the moving body, which states that the distance dk between the position of the moving body and the position of the other moving body (position when stopped) is greater than the value obtained by adding a predetermined margin α to a threshold value (r1 + r2) (FC107).

[0134] This makes it possible to prevent collisions even if the positions of other moving objects cannot be predicted.

[0135] In this embodiment, the predictive control calculation unit A104b (FIG. 1) calculates the control input for the moving body B100 using model predictive control. The calculation condition setting unit A104a (FIG. 1) lengthens the prediction horizon (N4<N3<N2<N1) as the constraint condition (conditions 1 to 4) number becomes smaller.

[0136] This allows for efficient control to avoid collisions even if the accuracy of the position of other moving objects changes.

[0137] As shown in Figure 3d, the calculation condition setting unit A104a (Figure 1) sets a first constraint condition (condition 1) for the moving body when the ranges in which the moving body and another moving body can move at their maximum speed within a predetermined time T are mutually prime (do not overlap).

[0138] The calculation load can be reduced by setting the first constraint (condition 1) when the operating characteristics of the moving body and other moving bodies indicate that there is no possibility of collision.

[0139] In this embodiment, the control device A100 includes a trajectory prediction unit A101 (FIG. 1). If the other moving body is a person (e.g., a worker), the trajectory prediction unit A101 calculates the predicted trajectory of the other moving body using an equation of motion corresponding to a person (Equation 4), and if the other moving body is a machine (e.g., a robot), the trajectory prediction unit A101 calculates the predicted trajectory of the other moving body using an equation of motion corresponding to the machine (Equation 3).

[0140] This makes it possible to calculate the predicted trajectory of another moving object according to the type (classification) of that moving object. The trajectory prediction unit A101 determines the type (classification) of the other moving object from, for example, tag information output from the environment recognition device A002.

[0141] There are multiple other moving bodies (e.g., robots, people, etc.). A calculation condition setting unit A104a (Figure 1) sets constraint conditions corresponding to each of the other moving bodies to a moving body B100. A predictive control calculation unit A104b (Figure 1) calculates the control input for the moving body by model predictive control using a prediction horizon (e.g., N4 in Equation 15) corresponding to the constraint condition with the highest number among the constraint conditions.

[0142] This allows for efficient control to avoid collisions with multiple other moving bodies.

[0143] When the calculation condition setting unit A104a (Figure 1) sets the second constraint condition (condition 2), it acquires the position of another moving body (e.g., a robot) up to the prediction horizon from the planned trajectory of the other moving body.When the calculation condition setting unit A104a sets the third constraint condition (condition 3), it acquires the position of another moving body up to the prediction horizon from the predicted trajectory based on the prediction that the other moving body will perform uniform linear motion.

[0144] This makes it possible to obtain the positions of other moving objects up to the common prediction horizon in a manner that is suited to the constraints.

[0145] In this embodiment, the control device A100 includes a reception trajectory correction unit A102 (FIG. 1) that corrects the planned trajectory of another moving body (e.g., a robot) based on the difference between the time used by the moving body B100 and the time used by the other moving body (e.g., a robot) (e.g., a timestamp attached to the planned trajectory).

[0146] This makes it possible to suppress deviations in the planned trajectories of other moving bodies corresponding to the time difference.

[0147] As shown in Figure 1, the control device A100 includes an environment recognition device A002 that recognizes the environment around a moving object B100, a state recognition device A001 that recognizes the state of the moving object (e.g., rotational speed, acceleration, etc.), a communication device A003 that communicates with devices (e.g., robots) around the moving object, and a calculation condition determination unit A103. The calculation condition determination unit A103 determines whether other moving objects (e.g., robots) are present within a predetermined range based on the environmental information recognized by the environment recognition device A002. The calculation condition determination unit A103 receives the planned trajectory of the other moving object from the other moving object via the communication device A003. The predictive control calculation unit A104b controls the moving object based on the state of the moving object recognized by the state recognition device A001.

[0148] The environment recognition device A002 can determine whether other moving objects exist within a predetermined range. The state recognition device A001 can acquire the state of the moving object to be used for controlling the moving object. The communication device A003 can acquire the planned trajectory of other moving objects.

[0149] The control system (autonomous control system) may include a control device A100 and two or more mobile bodies. The control device A100 outputs a control signal to the mobile body. The mobile body receives a control signal from the control device A100 and operates in accordance with the control signal. By mutually utilizing planned trajectories, collisions can be efficiently avoided.

[0150] <Embodiment 2: Air Mobility> The above explanation has focused on mobile objects such as robots in logistics warehouses. However, the application of the present invention is not limited to robots in logistics warehouses. Here, as another embodiment of the present invention, an example will be described in which an object is a goods delivery center using an air vehicle, which is a mobile object capable of flight, as shown in Figure 10.

[0151] Unlike the robot of embodiment 1, the flying object of embodiment 2 can move in three-dimensional space. Therefore, instead of the travel-permitted passage B101 of embodiment 1 shown in Figure 1, an area for retrieving items, taking off, and landing is designated as a controlled space B101-1, and the operation of the flying object within this area is controlled.

[0152] Within the managed space B101-1 (managed area), there are a mixture of an automatically controlled flying object B100-1, a manually operated flying object B100-2, and a worker B100-3 who loads cargo onto the flying object. To simplify the notation, Figure 10 shows only one vehicle (or one person) of each object, but there may be more moving objects.

[0153] To acquire its position and orientation in three-dimensional space, an aircraft is equipped with various sensors such as a Global Navigation Satellite System (GNSS) and an IMU. These sensors are equivalent to environmental recognition devices.

[0154] In addition, in order to reduce the aircraft weight in order to secure the payload, it may not be possible to install an external recognition device such as LiDAR. In such a case, it is desirable to treat the surrounding information acquired by the infrastructure sensor B106 as the environment recognition device A002.

[0155] In this embodiment, model predictive control is used, and therefore it is possible to easily extend the system to robots with different movement modes by changing the model used for control calculations, that is, the mathematical formula that describes the motion.

[0156] Robots in logistics warehouses perform various calculations using Equation 1, but for flying objects, for example, to simplify the problem, we can use the equation of motion (Equation 16) focusing on the Y- and Z-axis directions, as shown in Figure 11. Note that py and pz are coordinates on the Y and Z axes, vy and vz are velocities on the Y and Z axes, θ is the rotation angle, ω is the rotational angular velocity, g is the gravitational acceleration, m is the robot mass, L is the distance from the center of the body to the propeller rotation axis, I is the moment of inertia, T1 is the propeller thrust on the left side of the page, and T2 is the propeller thrust on the right side of the page. Equation 16 can be rewritten as Equation 17 using the sampling period ΔT. Because Equation 17 has a similar form to Equation 2, model predictive control formulations (such as Equation 7) can be used.

[0157]

[0158]

[0159] If we further expand Equation 16 to include equations of motion that handle all of the X, Y, and Z axes, we can easily calculate the trajectory of the aircraft in three-dimensional space.

[0160] In the first embodiment, a robot moving on a plane was considered, and therefore the constraint condition was that the circles surrounding the moving object do not come into contact, as shown in Figure 5b and Equation 8. In the case of an aircraft moving in three-dimensional space, this constraint condition is expanded to the constraint condition that the spheres surrounding the aircraft do not come into contact, as shown in Figure 12. In this case, the constraint condition can be given by Equation 18. By expanding the inequality in Equation 18 into Equation 10 and Equation 12, it is possible to formulate model predictive control that takes into account collision avoidance in three-dimensional space.

[0161]

[0162] As described above, model predictive control makes it possible to control robots with different locomotion patterns using a unified control method. Therefore, the present invention can be applied not only to robots but also to a wide range of moving objects such as passenger cars and motorcycles.

[0163] Furthermore, although the embodiments of the present invention have been described in detail using examples of robots in logistics warehouses and flying objects within managed areas, it goes without saying that the application of the present invention is not limited to these cases. For example, the present invention can also be used to generate routes for transport vehicles in ports and routes for robots moving within theme parks.

[0164] The present invention is not limited to the above-described embodiments and includes various modifications. For example, the above-described embodiments have been described in detail to clearly explain the present invention, and are not necessarily limited to those including all of the described configurations. Furthermore, it is possible to replace part of the configuration of one embodiment with the configuration of another embodiment, or to add the configuration of another embodiment to the configuration of one embodiment. Furthermore, it is possible to add, delete, or replace part of the configuration of each embodiment with other configurations.

[0165] Furthermore, some or all of the above configurations, functions, etc. may be implemented in hardware, for example, by designing them as integrated circuits. Furthermore, the above configurations, functions, etc. may be implemented in software by a processor interpreting and executing programs that implement the respective functions. Information such as programs, tables, and files that implement the respective functions may be stored in memory, a recording device such as a hard disk or solid-state drive (SSD), or a recording medium such as an IC card, SD card, or DVD.

[0166] The embodiment of the present invention may be in the following form.

[0167] (C1) In a mobile body having an environment recognition device that detects the environment around the mobile body, a state recognition device that detects the state of the mobile body itself, a communication device that communicates with devices around the mobile body, and a control device that controls the operation of the mobile body, the control device comprises: a trajectory prediction unit that, when the environment recognition device detects another mobile body in the environment around the mobile body, predicts the trajectory of the other mobile body based on speed information of the other mobile body detected by the environment recognition device; a reception trajectory correction unit that, when a planned trajectory is provided from the other mobile body by the communication device, calculates the trajectory of the other mobile body based on the mobile body; and a reception trajectory correction unit that checks whether there are other mobile bodies around the mobile body within a predetermined range from the detection result of the environment recognition device, and detects other mobile bodies within the predetermined range. a second calculation condition under which the moving body does not come into contact with the planned trajectory of the other moving body calculated by the received trajectory correction unit when there are other moving bodies within a predetermined range and the communication device has received a planned trajectory from the other moving body; and a third calculation condition under which the moving body does not come into contact with the predicted trajectory of the other moving body calculated by the trajectory prediction unit when there are other moving bodies within a predetermined range and the communication device has not received a planned trajectory from the other moving body; and a movement path calculation unit that calculates the movement path of the moving body under the calculation conditions determined by the calculation condition calculation unit.

[0168] (C2) In a control system for integrated control of the operation of a moving body equipped with the control device of (C1), the movement path calculation unit of the control device includes a predictive control calculation unit that calculates a control input for the moving body to move to a predetermined step ahead in accordance with the calculation conditions determined by the calculation condition calculation unit, and the control device is characterized in that it includes a planned trajectory providing unit that calculates a planned trajectory, which is the trajectory the moving body will follow to the predetermined step ahead, based on the control input calculated by the predictive control calculation unit, and provides the planned trajectory to the other moving body via the communication device.

[0169] (C3) In the control device of (C1), the calculation setting unit is characterized in that, when there is another moving body within a predetermined range, the communication device does not receive a planned trajectory from the other moving body, and there is no change in the predicted trajectory calculated by the trajectory prediction unit, it sets a fourth calculation condition that prevents contact between the moving body and the other moving body while providing a safety margin of a predetermined distance from the current positions of the moving body and the other moving body.

[0170] (C4) In the control device of (C1), the movement path calculation unit is realized by model predictive control, and the smaller the condition number set in the calculation condition setting unit, the longer the prediction horizon used for the calculation of the model predictive control.

[0171] (C5) In the control device of (C1), the calculation condition setting unit is characterized in that, even if the environment recognition device detects another moving body in the environment around the moving body, if the range in which the moving body and the other moving body can move at maximum speed do not overlap within the predetermined step set in the first calculation condition, the first calculation condition is set.

[0172] (C6) In the control device of (C1), the environment recognition device has a function of classifying attributes related to the dynamics of other moving bodies, and the trajectory prediction unit changes the trajectory prediction method of the other moving bodies according to the attributes classified by the environment recognition device.

[0173] (C7) The control device of (C1) is characterized in that it comprises a predictive control calculation unit that calculates a control input for the moving body to move a predetermined number of steps ahead in accordance with the calculation conditions determined by the calculation condition calculation unit, and when the environment recognition device detects multiple other moving bodies in the environment around the moving body, the calculation condition setting unit judges the calculation conditions for each of the other detected moving bodies, and the movement path calculation unit performs calculation using the predictive step that is set under the condition with the largest condition number among the calculation conditions judged for each of the other moving bodies.

[0174] According to (C1)-(C7), it is possible to generate efficient movement routes for multiple moving objects in real time, even in warehouses and factories where workers and other people are present.

[0175] A001...State recognition device A002...Environment recognition device A003...Communication device A004...Actuator A100...Control device A101...Trajectory prediction unit A102...Received trajectory correction unit A103...Calculation condition determination unit A104...Movement path calculation unit A104a...Calculation condition setting unit A104b...Predictive control calculation unit A105...Planned trajectory provision unit B100...Mobile body B100-1...Air vehicle B100-2...Air vehicle B100-3...Worker B101...Permitted travel passage B101-1...Managed space B102...Shelf B103...Worker B104...Management server B105...Wireless communication device B106...Infrastructure sensor

Claims

1. A control device comprising: a calculation condition setting unit that, when another moving body exists outside a predetermined range from the moving body, sets a first constraint condition on the moving body based on the operating characteristics of the moving body; when the other moving body exists within the predetermined range from the moving body and a planned trajectory of the other moving body is received, sets a second constraint condition on the moving body that the distance between the position of the moving body and the position of the other moving body on the planned trajectory is greater than a threshold; and when the other moving body exists within the predetermined range from the moving body, the planned trajectory of the other moving body is not received, and the moving speed of the other moving body is acquired, sets a third constraint condition on the moving body that the distance between the position of the moving body and the position of the other moving body based on the moving speed of the other moving body is greater than the threshold; and a predictive control calculation unit that controls the moving body so as to satisfy the set constraint conditions.

2. A control device according to claim 1, characterized in that the predictive control calculation unit calculates a control input for the moving body so as to satisfy the constraint conditions, the control device calculates a planned trajectory for the moving body based on the control input, and has a planned trajectory providing unit that provides the planned trajectory of the moving body to the other moving body when communication between the moving body and the other moving body is established.

3. A control device as described in claim 1, characterized in that the calculation condition setting unit sets a fourth constraint condition on the moving body that, when another moving body is present within the specified range from the moving body, the planned trajectory of the other moving body is not received, and the other moving body is stopped, the distance between the position of the moving body and the position of the other moving body is greater than a value obtained by adding a specified margin to the threshold value.

4. A control device according to claim 1, characterized in that the predictive control calculation unit calculates the control input of the moving body using model predictive control, and the calculation condition setting unit makes the prediction horizon longer as the number of the constraint condition becomes smaller.

5. A control device according to claim 1, characterized in that the calculation condition setting unit sets the first constraint condition for the moving body when the ranges in which the moving body and the other moving body can move at their maximum speed within a predetermined time are relatively prime.

6. A control device according to claim 1, characterized in that it comprises a trajectory prediction unit that, if the other moving body is a person, calculates a predicted trajectory of the other moving body using an equation of motion corresponding to the person, and, if the other moving body is a machine, calculates a predicted trajectory of the other moving body using an equation of motion corresponding to the machine.

7. A control device according to claim 1, wherein the other moving bodies are multiple, the calculation condition setting unit sets the constraint conditions corresponding to each of the other moving bodies for the moving bodies, and the predictive control calculation unit calculates the control input for the moving body using model predictive control, using a prediction horizon corresponding to the constraint condition with the highest number among the constraint conditions.

8. A control device as described in claim 7, characterized in that the calculation condition setting unit, when the second constraint condition is set, obtains the position of the other moving body up to the prediction horizon from the planned trajectory of the other moving body, and when the third constraint condition is set, obtains the position of the other moving body up to the prediction horizon from a predicted trajectory based on a prediction that the other moving body will perform uniform linear motion.

9. A control device according to claim 1, characterized in that it comprises a reception trajectory correction unit that corrects the planned trajectory of the other moving body based on the difference between the time used by the moving body and the time used by the other moving body.

10. A mobile body equipped with the control device described in claim 1, comprising: an environment recognition device that recognizes the environment around the mobile body; a state recognition device that recognizes the state of the mobile body; a communication device that communicates with equipment around the mobile body; and a calculation condition judgment unit that determines whether another mobile body is present within the specified range from environmental information recognized by the environment recognition device and receives the planned trajectory of the other mobile body from the other mobile body via the communication device, wherein the predictive control calculation unit controls the mobile body based on the state of the mobile body recognized by the state recognition device.

11. A control system comprising the control device according to claim 1 and two or more mobile bodies, wherein the control device outputs a control signal to the mobile bodies, and the mobile bodies receive a control signal from the control device and operate in accordance with the control signal.

12. A control method that causes a control device to execute the following steps: when another moving body is present outside a predetermined range from the moving body, setting a first constraint condition on the moving body based on the operating characteristics of the moving body; when the other moving body is present within the predetermined range from the moving body and a planned trajectory of the other moving body is received, setting a second constraint condition on the moving body such that the distance between the position of the moving body and the position of the other moving body on the planned trajectory is greater than a threshold; when the other moving body is present within the predetermined range from the moving body, the planned trajectory of the other moving body is not received, and the speed at which the other moving body is moving is acquired, setting a third constraint condition on the moving body such that the distance between the position of the moving body and the position of the other moving body based on the speed at which the other moving body is moving is greater than the threshold; and controlling the moving body so that the set constraint conditions are satisfied.

Images