Control device, moving object, control system and control method

Abstract

To provide a control device and the like that can perform control to efficiently avoid a collision.SOLUTION: A calculation condition setting unit sets a first constraint condition for a moving object based on a dynamic characteristic of the moving object when another moving object exists outside a predetermined range from the moving object (FC102). When determining that another moving object exists within a predetermined range from the moving object and receiving a planned trajectory of the other moving object (FC103: YES), the calculation condition setting unit sets a second constraint condition for the moving object in which a distance between positions of the moving object and the other moving object on the planned trajectory is greater than a threshold (FC104). When determining that another moving object exists within a predetermined range from the moving object, not receiving a planned trajectory of the other moving object, and acquiring the velocity at which the other moving object is moving (FC105: YES), the calculation condition setting unit sets a third constraint condition for the moving object in which a distance between positions of the moving object and the other moving object based on the velocity at which the other moving object is moving is greater than a threshold (FC106). A predictive control calculation unit controls the moving object so as to satisfy the set constraint conditions.SELECTED DRAWING: Figure 4

Description

[Technical Field]

[0001] The present invention relates to a control device, a moving body, a control system, and a control method. [Background technology]

[0002] To alleviate labor shortages in transporting goods in logistics warehouses and between processes in factories, the introduction of mobile objects such as robots (e.g., Automated Guided Vehicles (AGVs) and Autonomous Mobile Robots (AMRs)) is progressing. Introducing such mobile objects requires the creation of paths within warehouses and factories that the objects can move along (creating a graph consisting of nodes and edges, for example). The more detailed this configuration, the greater the freedom of the paths that the objects can choose, enabling more efficient transport. However, the configuration work requires a large amount of man-hours. Furthermore, because the above-mentioned 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 businesses that 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, it is difficult to obtain accurate map information because of environmental changes such as the installation of pylons and signs due to temporary road closures caused by road construction. Furthermore, 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, the vehicle positions, and the 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 paths of all vehicles, it evaluates the deviation between the vehicle and the target position, and calculates the movement path for all vehicles that minimizes the deviation." [Prior art documents] [Patent documents]

[0005] [Patent Document 1] Patent Publication No. 2021-77090 Summary of the Invention [Problem to be solved by the invention]

[0006] According to Patent Document 1, it is possible to calculate a route for a moving body to reach a target position without designing detailed passage information in a warehouse or factory in advance. In addition, since the dynamics of multiple moving bodies are taken into consideration and a movement plan is calculated that prevents the moving bodies 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 body.

[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 it may become impossible to calculate routes in real time.

[0008] Furthermore, Patent Document 1 assumes the target position of the moving object, but does not assume the target position of the worker. In other words, since it does not take into consideration the destination of the person who transports and collects goods in a warehouse or factory, there is a possibility that the moving object will impair the workability of the person.

[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. [Means for solving the problem]

[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 sets a first constraint condition on a moving body based on the operating characteristics of the moving body when another moving body is present outside a predetermined range from the moving body, 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 value 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, and 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 value 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 not received and the speed at which the other moving body is moving is acquired, and a predictive control calculation unit that controls the moving body to satisfy the set constraint conditions; [Effects of the Invention]

[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 clear from the following description of the embodiments. [Brief explanation of the drawings]

[0012] [Figure 1] FIG. 2 is a functional block diagram relating to a moving body according to an embodiment of the present invention. [Figure 2] 1A and 1B are diagrams illustrating an embodiment (first embodiment) in which the present invention is applied to a transport robot. [Figure 3a] FIG. 1 is a diagram illustrating a transport robot. [Figure 3b] FIG. 1 is a diagram illustrating a configuration for performing distributed processing. [Figure 3c]FIG. 10 is a diagram showing a state in which another moving object is present outside the illumination range of the LiDAR. [Figure 3d] This is a diagram showing a state in which other moving bodies are present within the LiDAR's illumination range, but the movable ranges of each moving body do not overlap. [Figure 4] 10 is a flowchart illustrating a condition branch in setting a calculation condition. [Figure 5a] FIG. 10 is a diagram illustrating the positional relationship between the host vehicle and other vehicles under "Condition 2." [Figure 5b] FIG. 10 is a diagram showing the positional relationship between the subject vehicle and other vehicles at time k=2. [Figure 6a] FIG. 10 is a diagram illustrating the positional relationship between the host vehicle and other vehicles under "Condition 3." [Figure 6b] This is a diagram showing the predicted trajectory and the trajectory shown in Figure 5a superimposed on each other. [Figure 6c] FIG. 2 is a diagram showing the trajectory of the host vehicle; [Figure 7a] FIG. 10 is a diagram illustrating a situation in which "Condition 4" may occur. [Figure 7b] FIG. 10 is a diagram showing a state in which a worker comes into contact with the vehicle. [Figure 8] FIG. 10 is a diagram illustrating a method for the host vehicle to avoid an obstacle under "Condition 4." [Figure 9] 10 is a flowchart illustrating the calculation of a control device mounted on a moving body. [Figure 10] FIG. 1 is a diagram illustrating an embodiment (second embodiment) in which the present invention is applied to an aircraft. [Figure 11] FIG. 1 is a diagram illustrating an aircraft. [Figure 12] FIG. 10 is a diagram illustrating collision detection between flying objects. DETAILED DESCRIPTION OF THE INVENTION

[0013] An autonomous control system is a system that 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. A movement plan includes, for example, the coordinates to which the moving object has moved, the posture of the moving object as it moves, and the speed at which the moving object moves.

[0014] An embodiment of the autonomous control system of the present invention will be described below with reference to the drawings. The autonomous control system of this 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 mixed in.

[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 each mobile object has similar functions.

[0016] <Embodiment 1: Warehouse Transportation> For ease of explanation, the following will be based on the logistics warehouse shown in Figure 2, and the moving object will be a transport robot.

[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, carry the loaded luggage to a designated shelf, and assist in the task of retrieving the luggage from the shelf where the luggage is stored. The loading and unloading of luggage may 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 and 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 the 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 moving body> For ease of explanation, it is assumed that the moving body B100 is a differential two-wheel moving body as shown in Fig. 3a. However, it should be noted that the moving body B100 used in this invention is not limited to a differential two-wheel robot, and various types of moving bodies can be used, such as an omni-wheel robot, a forklift, a four-wheel vehicle, and a tow vehicle.

[0023] The equation of motion for a differential two-wheel type 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 body, 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]

number

[0025]

number

[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 (Inertia Measurement Unit)) that acquires the rotational speed and acceleration of the vehicle body, and LiDAR.

[0027] Encoders and IMUs measure the state of the moving body itself, so they correspond to the state recognition device A001, and LiDAR measures the state around the moving body, so they correspond to the environment recognition device A002. Note that when LiDAR is used to calculate the position and speed of a moving body by 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 conforms to communication standards such as Bluetooth (registered trademark) and Wi-Fi (registered trademark), and is capable of communicating with surrounding mobile objects and a management server B104.

[0029] The moving body 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 driving 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 CPU (Central Processing Unit), an input / output circuit, etc.

[0030] <Configuration for Distributed Processing> The mobile body of this embodiment generates its own movement path and performs tracking control along the generated path. For this purpose, each mobile body is provided with 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] By adopting such a distributed processing configuration, it is possible to suppress the increase in the amount of calculations in each control device A100 even if the number of mobile objects increases. Also, even if a specific mobile object fails, it is possible to suppress the impact on the entire system.

[0033] Returning to Figure 1, we will now provide a detailed explanation of the functional blocks of the mobile unit targeted at the logistics warehouse shown in Figure 2.

[0034] <Explanation of the functional blocks of the mobile unit> The autonomous control system is a system for safely and efficiently controlling multiple moving bodies B100. In order to control a large number of moving bodies B100 in real time, the processing is executed in an autonomous and distributed manner by the control device A100 mounted on each moving body B100. Therefore, it should be noted that in this embodiment, the travel routes of the moving bodies 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 reception 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 the predicted trajectory of the moving object according to the positions and velocities of the surrounding moving objects acquired by the environment recognition device A002, and the orientations 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 body, it goes without saying that the same formula can be used to calculate the predicted trajectories of multiple moving bodies.

[0038]

number

[0039] <Trajectory prediction by 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 to be used may be changed according to this classification. Since Equation 3 is the equation of motion 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 a 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]

number

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

[0042] The movement path includes the x and y coordinates, velocity v, and angular velocity ω that are reached at the time "ka+Δt×i (i=0,...,N)" every calculation period Δt, starting from the time ka when the other moving body calculated its movement path.

[0043] The reception trajectory correction unit corrects the received path information according to the time (timestamp) of the movement path received from another moving body and the time difference between 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 the correction using Equation 5 is complete, position information is calculated in units of control period Δt using Equation 2. Note that if the clocks of the other moving body and its own control device are completely synchronized, this correction process is not necessary.

[0044]

number

[0045] Also, if the moving body is a worker, this process is skipped because a predicted route is not provided.

[0046] The calculation condition determination unit A103 determines the calculation conditions of the travel 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 in 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 will be described later.

[0050] <Explanation of the flow chart for the calculation condition determination section> 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), it transitions to FC102, and if there are other moving objects (NO), it transitions 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 predetermined time when moving at maximum speed (Fig. 3d). Specifically, if there is another moving object outside the illumination range as shown in Fig. 3c, FC101 determines YES. Also, as shown in Fig. 3d, even if another moving object is within the illumination range of the sensor, FC101 also determines YES if the ranges that each moving object can move within the predetermined time T at its maximum moving speed vmax do not overlap.

[0052] In addition, the presence or absence of moving objects is confirmed by using LiDAR to observe obstacles, 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 is checked whether a planned trajectory is provided by the other moving body 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 confirmed whether the movement speed of the other moving body confirmed in FC101 can be acquired. If the other moving body 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] Even if the object is determined to be a moving object in FC101, if the position information acquired using the environment recognition device A002 is not updated, the moving speed cannot be calculated, and the process transitions to FC107.

[0059] "Condition 1" (FC102) to "Condition 4" (FC107) in the flowchart of Figure 4 correlate with how freely a person can decide their 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 travel that path are known, so the robot can plan its own movements to avoid coming into contact with it.

[0062] In condition 3, mainly only the movement speed of other moving bodies at that time is known, so there is a high degree of uncertainty compared to condition 2, and it becomes necessary to plan one's own movements 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 passed (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 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 (velocity, angular velocity) up to the prediction horizon can be calculated, so that 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 bodies in the vicinity, so only the moving body 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 moving body's own position and orientation vector Xk and the target position and orientation vector rk, and the control input uk. The target position and orientation vector rk is provided by the management server B104. Here, 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]

number

[0071] In MPC, in order to evaluate the evaluation function J up to the interval of the evaluation function (behavior from the current time k to the next N1 steps), it is necessary to calculate the future behavior of the moving body B100 itself. The behavior prediction of the moving body B100 up to the next N steps 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 constraint conditions. For example, in the case of the differential two-wheel robot shown in Figure 3, the upper and lower limits of the robot's moving 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 such actuators as constraint conditions, it becomes possible to generate a path that the robot can actually follow. An MPC that takes such constraint conditions 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]

number

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

[0075] <Condition 2> If "Condition 2" occurs, it is necessary to calculate the behavior of the robot, provided that it does not come into contact with other moving objects whose movement paths are known.

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

[0077] Figure 5a is an example showing the movement trajectories of the subject vehicle (control target) and another 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 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 satisfying the inequality in equation 8 is equivalent to the two robots not coming into contact. By incorporating such conditions into the constraint conditions of the MPC, it is possible to achieve contact avoidance between robots. Note that while Figure 5 only considers the conditions for contact 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]

number

[0080] In "Condition 2," the vehicle utilizes route information provided by other moving bodies to plan its own movements, so the longest prediction horizon N2 to be considered is the step length Nd of the provided route. For this reason, 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, as the prediction horizon N2 is a variable value. Note that it is desirable for the pre-designed prediction horizon Np to be shorter than N1 in Equation 6. This is because the calculation time required to avoid contact with other vehicles is long. 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]

number

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

[0083]

number

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

[0085] Regarding "Condition 3," the condition for not coming into contact with other moving bodies 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, it is possible to predict the behavior of the other vehicle as moving at a constant speed in a straight line, as shown in Figure 6a.

[0087] Figure 6b shows this predicted trajectory superimposed 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, if a long-term predicted trajectory is used to plan the trajectory of the own vehicle, an inappropriate trajectory may be generated. For example, if the trajectory of the own vehicle 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 own vehicle will not come into contact with the other vehicle even if it continues moving straight, so an avoidance maneuver downward on the page can be said to be an action that reduces 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 of "Condition 1" and "Condition 2," respectively.

[0089] Since the conditions for avoiding contact between the vehicle and other vehicles 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]

number

[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 B100 is traveling near a worker who is storing luggage 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 since the worker is working in front of the shelf, the moving speed is not detected.

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

[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 such behavior into the formulation of model predictive control, the inequality in Equation 12 can be considered so that the distance dk between the circle of radius r2 surrounding the worker plus a safety margin α and the circle of radius r1 of the vehicle itself is maintained so that they do not come into contact, as shown in Figure 8.

[0096]

number

[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]

number

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

[0100]

number

[0101] <Processing when multiple moving objects exist> Up to this point, for simplicity, 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 bodies, if there are no moving bodies around the vehicle, the calculation of condition 1 will be performed, so the explanation will be given assuming that the transition has been made to FC103 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 check which of "Condition 2" to "Condition 4" they fall under.

[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 and they are judged to be "Condition 2," "Condition 3," and "Condition 4," respectively, the prediction horizon is set to N4.

[0105] For collision avoidance conditions, the predicted trajectory 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, let us assume a situation where the first moving body (i=1) is in "Condition 2," the second moving body (i=2) is in "Condition 3," and the third moving body (i=3) is in "Condition 4." Under these conditions, the prediction horizon of evaluation function 15a becomes the shortest N4.

[0107]

number

[0108] The distances di,k between the vehicle and each moving object are 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. Since 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] The conditions for preventing contact between the vehicle and other vehicles are the same as those in inequalities 15c and 15d for the moving bodies i=1 and i=2, but for the moving body i=3, which corresponds to "Condition 4," inequalities 15e are used, taking into account the safety margin α.

[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 vehicle will take in the future 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 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, the other moving bodies can use it for their own control.

[0114] <Overall flow chart of the control device> The processing procedure of the autonomous control system explained above will be explained using the flowchart in FIG.

[0115] First, FC201 updates the information of 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 on 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. Details of the processing contents 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 "condition 1" is met (YES) in the calculation condition decision unit A103, the process transitions to FC205. If the "condition 1" is not met (NO), the process transitions to FC206.

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

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

[0121] When the process transitions to FC207, the planned trajectory of the moving body 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 transitions to FC209. If "Condition 3" is not met (NO), the process transitions to FC205.

[0123] When the process transitions to FC209, the current position and speed of the mobile body B100 that is not the control target can be acquired, so this information is used to predict the trajectory of the mobile body B100 that is not the control target. This process corresponds to the trajectory prediction unit A101. When the process of FC209 is completed, the process transitions to FC205.

[0124] In FC210, the control input is determined 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 transitions 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 embodiment 1 can be summarized as follows.

[0129] The control device A100 includes a calculation condition setting unit A104a and a predictive control calculation unit A104b (FIG. 1). As shown in FIG. 4, when another moving body (e.g., a robot, a person, etc.) is present outside a predetermined range from the moving body B100 (FIG. 1) (FC101: YES), the calculation condition setting unit A104a sets a first constraint condition (condition 1, st in Equation 7) on the moving body based on the operating characteristics (e.g., the performance of the actuator A004) of the moving body (FC102). When the calculation condition setting unit A104a detects that another moving body is present within a predetermined range from the moving body and receives a planned trajectory of the other moving body (FC103: YES), the calculation condition setting unit A104a sets a second constraint condition (condition 2) on the moving body, in which the distance dk between the position (xk, yk) of the moving body and the position (xdk, ydk) of the other moving body 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 does not receive the planned trajectory of the other moving body, and acquires the moving speed (speed during movement) of the other moving body (FC105: YES), it sets a third constraint condition (condition 3) to the moving body, that is, 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 (Fig. 6c) is greater than a threshold value (FC106).The predictive control calculation unit A104b (Fig. 1) controls the moving body so as to satisfy the set constraint condition.

[0130] By setting constraint conditions for 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. In the first constraint condition (condition 1), there is no need to avoid collisions with other moving objects, so the calculation load is low. In the second constraint condition (condition 2), the positions of other moving objects on the planned trajectory are used, so the accuracy of the positions of other moving objects is high. In the third constraint condition (condition 3), the positions of other moving objects are based on the speed at which they are moving, so it is easy to calculate the positions of other moving objects.

[0131] As shown in Fig. 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), it sets a fourth constraint condition (condition 4) on the moving body (FC107) 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).

[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 of the moving body B100 by model predictive control. The calculation condition setting unit A104a (FIG. 1) makes the prediction horizon (N4 < N3 < N2 < N1) longer as the number of the constraint conditions (conditions 1 to 4) is smaller.

[0136] Thereby, even if the accuracy of the position of other moving bodies changes, it is possible to perform control for efficiently avoiding collisions.

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

[0138] By setting the first constraint condition (condition 1) when there is no possibility of collision based on the motion characteristics of the moving body and other moving bodies, the calculation load is reduced.

[0139] In this embodiment, the control device A100 includes a trajectory prediction unit A101 (FIG. 1). When another moving body is a person (for example, a worker), the trajectory prediction unit A101 calculates the predicted trajectory of the other moving body using the equation of motion (mathematical formula 4) corresponding to the person. When another moving body is a machine (for example, a robot), the trajectory prediction unit A101 calculates the predicted trajectory of the other moving body using the equation of motion (mathematical formula 3) corresponding to the machine.

[0140] Thereby, the predicted trajectory of another moving body can be calculated according to the type (classification) of the other moving body. The trajectory prediction unit A101 determines the type (classification) of the other moving body from, for example, the tag information output from the environment recognition device A002.

[0141] There are a plurality of other moving bodies (for example, robots, people, etc.). The calculation condition setting unit A104a (FIG. 1) sets the constraint conditions corresponding to each of the other moving bodies for the moving body B100. The predictive control calculation unit A104b (FIG. 1) calculates the control input of the moving body by model predictive control using the prediction horizon (for example, N4 in mathematical formula 15) corresponding to the constraint condition with the largest constraint condition 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 (FIG. 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 allows the positions of other moving objects up to the common prediction horizon to be obtained in a manner that is appropriate for the constraints.

[0145] In this embodiment, the control device A100 includes a reception trajectory correction unit A102 (FIG. 1). The reception trajectory correction unit A102 (FIG. 1) 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 FIG. 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., rotation speed, acceleration, etc.), a communication device A003 that communicates with peripheral devices (e.g., robots) of the moving object, and a calculation condition determination unit A103. The calculation condition determination unit A103 determines whether other moving objects (e.g., robots) exist 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 moving bodies. The control device A100 outputs a control signal to the moving body. The moving 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 explanation so far 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 base that uses an air vehicle, which is a mobile object capable of flight, as shown in FIG.

[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 Fig. 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 is a mixture of an automatically controlled flying object B100-1, a manually operated flying object B100-2, and a worker B100-3 who loads the object into the flying object and carries the cargo. To simplify the notation, Fig. 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 correspond to environment recognition devices.

[0154] In addition, in order to reduce the aircraft weight in order to ensure 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, so by changing the model used for control calculation, that is, the mathematical formula that describes the motion, it is possible to easily expand to robots with different movement forms.

[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 written 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]

number

[0158]

number

[0159] If we further expand Equation 16 to include an equation of motion that handles all of the X, Y, and Z axes, we can easily calculate the trajectory of the flying object 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 body do not come into contact, as shown in FIG. 5b and Equation 8. In the case of a flying body moving in three-dimensional space, this constraint condition is expanded, and the constraint condition is that the spheres surrounding the flying body do not come into contact, as shown in FIG. 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 contact avoidance in three-dimensional space.

[0161]

number

[0162] As described above, by using model predictive control, robots with different movement patterns can be controlled 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, but 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, and it is also possible 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, the above-mentioned configurations, functions, etc. may be realized in part or in whole by hardware, for example, by designing them as integrated circuits. Furthermore, the above-mentioned configurations, functions, etc. may be realized in software by a processor interpreting and executing a program that realizes each function. Information such as the program, table, and file that realizes each function can be stored in a memory, a recording device such as a hard disk or SSD (Solid State Drive), or a recording medium such as an IC card, SD card, or DVD.

[0166] The present invention may be embodied in the following manner.

[0167] (C1) 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 having 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 is another mobile body around the mobile body within a predetermined range from the detection result of the environment recognition device and corrects the trajectory of the other mobile body based on the speed information of the other mobile body detected by the environment recognition device. a calculation condition setting unit that sets a first calculation condition that takes into account only the movement of the moving body when there are no other moving bodies within a specified range; a second calculation condition that the moving body does not come into contact with the planned trajectory of the other moving body calculated by the receiving trajectory correction unit when there are other moving bodies within a specified range and the communication device has received a planned trajectory from the other moving body; and a third calculation condition that 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 specified range and the communication device has not received a planned trajectory from the other moving body, and the control device is characterized by having a calculation condition setting unit that sets a first calculation condition that takes into account only the movement of the moving body when there are no other moving bodies within a specified range and the communication device has received a planned trajectory from the other moving body,

[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 that 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, and the communication device has not been provided with 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 position of the 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 model predictive control calculation.

[0171] (C5) In the control device of (C1), the calculation condition setting unit is characterized in that, even if the environmental 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 specified step set in the first calculation condition, the first calculation condition is set.

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

[0173] (C7) The control device of (C1) is characterized in that it is provided with 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 environmental 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 calculations using the predictive step that is set under the condition with the highest 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. [Explanation of symbols]

[0175] A001...Status recognition device A002…Environment recognition device A003...Communication equipment A004...Actuator A100...Control device A101...Trajectory prediction section A102...Reception trajectory correction unit A103…Calculation condition judgment unit A104...Movement path calculation unit A104a...Calculation condition setting section A104b...Predictive control calculation unit A105...Scheduled trajectory provider B100…mobile object B100-1...Flying object B100-2...Flying object B100-3…Worker B101: Permitted passage B101-1…Management space B102…Shelf B103...Worker B104...Management server B105...Wireless communication equipment B106...Infrastructure sensor

Claims

1. If another moving object is present outside a predetermined range from the moving object, a first constraint condition is set for the moving object based on a motion characteristic of the moving object; 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, a second constraint condition is set on the moving body that a distance between a position of the moving body and a position of the other moving body on the planned trajectory is greater than a threshold value; a calculation condition setting unit that, when the other moving body is present within the predetermined range from the moving body, a planned trajectory of the other moving body is not received, and a moving speed of the other moving body is acquired, sets a third constraint condition on the moving body that a distance between a position of the moving body and a position of the other moving body based on the moving speed of the other moving body is greater than the threshold value; a predictive control calculation unit that controls the moving object so as to satisfy a set constraint condition; A control device comprising:

2. 2. The control device according to claim 1, the predictive control calculation unit calculates a control input for the moving body so as to satisfy the constraint condition; The control device includes a planned trajectory providing unit that calculates a planned trajectory of the moving body based on the control input and 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. A control device characterized by:

3. 2. The control device according to claim 1, The calculation condition setting unit sets a fourth constraint condition on the moving body that, when the other moving body is present within the predetermined range from the moving body, a 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 predetermined margin to the threshold value. A control device characterized by:

4. 2. The control device according to claim 1, the predictive control calculation unit calculates a control input for the moving body by model predictive control; The calculation condition setting unit sets a longer prediction horizon as the number of the constraint condition becomes smaller. A control device characterized by:

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

6. 2. The control device according to claim 1, If the other moving object is a person, a predicted trajectory of the other moving object is calculated using an equation of motion corresponding to the person; If the other moving body is a machine, a trajectory prediction unit is provided that calculates a predicted trajectory of the other moving body using a motion equation corresponding to the machine. A control device characterized by:

7. 2. The control device according to claim 1, the other moving bodies are plural, the calculation condition setting unit sets the constraint conditions corresponding to each of the other moving bodies on the moving body; The predictive control calculation unit calculates a control input for the moving body by model predictive control using a prediction horizon corresponding to the constraint condition with the largest number among the constraint conditions. A control device characterized by:

8. The control device according to claim 7, The calculation condition setting unit When the second constraint condition is set, a position of the other moving object up to the prediction horizon is obtained from a planned trajectory of the other moving object; When the third constraint condition is set, the position of the other moving body up to the prediction horizon is acquired from a predicted trajectory based on a prediction that the other moving body will perform uniform linear motion. A control device characterized by:

9. 2. The control device according to claim 1, and 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. A control device characterized by:

10. A moving body equipped with the control device according to claim 1, an environment recognition device that recognizes the environment around the moving object; a state recognition device that recognizes the state of the moving object; a communication device that communicates with peripheral devices of the mobile object; a calculation condition determination unit that determines whether the other moving body is present within the predetermined range based on information about the environment recognized by the environment recognition device, and receives a planned trajectory of the other moving body from the other moving body via the communication device, The predictive control calculation unit controls the moving body based on the state of the moving body recognized by the state recognition device. A moving object characterized by:

11. The control device according to claim 1; two or more moving bodies; the control device outputs a control signal to the moving body, The mobile body has a control system that receives a control signal from the control device and operates in accordance with the control signal.

12. 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 a motion characteristic of the moving body; a step of setting, 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, a second constraint condition on the moving body that a distance between a position of the moving body and a position of the other moving body on the planned trajectory is greater than a threshold value; a step of setting a third constraint condition on the moving body that, 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 moving speed of the other moving body is acquired, 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 value; a step of controlling the moving body so as to satisfy a set constraint condition; A control method for causing a control device to execute the above.

Images