MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL SYSTEM, CONTROLLED MOBILE BODY, AND MOBILE BODY CONTROL METHOD

Abstract

The object of the present invention is to provide a technology capable of suppressing deadlock. The mobile body control device includes an acquisition unit that acquires state quantities of one or more mobile bodies and a target travel route that is set in advance as a target for travel of the mobile bodies, and a target position trajectory setting unit that sets a target position trajectory in which a magnitude relationship between the relative positions of the mobile bodies with respect to the route intersections is maintained based on the state quantities of the multiple mobile bodies, the target travel route, and the route intersections when the one or more mobile bodies are multiple mobile bodies and the target travel route has a route intersection.

Description

[Technical field]

[0001] The present disclosure relates to a mobile object control device, a mobile object control system, a controlled mobile object, and a mobile object control method. [Background technology]

[0002] Conventionally, there is known a vehicle group control device that generates a virtual route with intersections for roads that have a width equivalent to the width of multiple lanes, such as a road at the exit of a toll gate, and that do not have markings separating the lanes, and smoothly guides vehicles based on the virtual route (for example, Patent Document 1). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2020-075558 A Summary of the Invention [Problem to be solved by the invention]

[0004] However, when a mobile object travels along a route that has an intersection, there is generally a problem that a deadlock occurs.

[0005] Therefore, the present disclosure has been made in consideration of the above-mentioned problems, and has an object to provide a technique capable of suppressing deadlock. [Means for solving the problem]

[0006] A mobile body control device according to the present disclosure includes an acquisition unit that acquires state quantities of one or more mobile bodies and a target travel route that is set in advance as a target for travel of the mobile bodies, and a target position trajectory setting unit that, when the one or more mobile bodies are a plurality of mobile bodies and the target travel route has a route intersection, indicates a target position of each of the mobile bodies for each time along the target travel route based on the state quantities of the plurality of mobile bodies, the target travel route, and the route intersection, and sets a target position trajectory according to a constraint condition that maintains a magnitude relationship of a relative position of each of the mobile bodies with respect to the route intersection, and the constraint condition includes a relative position of the position of the mobile body with respect to the route intersection. Another vehicle traveling on a different route that shares a route intersection with the route traveled by the vehicle. The distance between the relative position of the moving body to the route intersection is 、 More than a predetermined value away A set of the positions of the moving objects is The state quantities of the moving object on the target position trajectory are constrained by the first set. Effect of the Invention

[0007] According to the present disclosure, a target position trajectory is set in which the magnitude relationship between the relative positions of the moving bodies with respect to the route intersections is maintained. With this configuration, it is possible to prevent deadlock.

[0008] The objects, features, aspects and advantages of the present disclosure will become more apparent from the following detailed description and the accompanying drawings. [Brief description of the drawings]

[0009] [Figure 1] 1 is a block diagram showing an example of a configuration of a mobile object control system according to a first embodiment. [Diagram 2] 1 is a diagram for explaining the relationship between the position of a moving object and a target travel route. [Diagram 3] 1 is a diagram for explaining the relationship between the position of a moving object and a target travel route. [Figure 4] 6 is a flowchart showing an example of a setting procedure performed by a target position trajectory setting unit according to the first embodiment. [Diagram 5] 4 is a diagram for explaining a set used in a target position trajectory setting unit according to the first embodiment. FIG. [Figure 6] 4 is a diagram for explaining a set used in a target position trajectory setting unit according to the first embodiment. FIG. [Figure 7] 4 is a diagram for explaining a set used in a target position trajectory setting unit according to the first embodiment. FIG. [Figure 8] 4 is a diagram for explaining a set used in a target position trajectory setting unit according to the first embodiment. FIG. [Figure 9] 4 is a flowchart showing an example of a procedure of the mobile object control system according to the first embodiment. [Figure 10] 13A and 13B are diagrams illustrating travel of a moving object based on a target position trajectory in which the magnitude relationship of relative positions is not maintained; [Figure 11] 11A and 11B are diagrams illustrating travel of a moving body based on a target position trajectory in which the magnitude relationship of relative positions is maintained. [Figure 12] FIG. 11 is a block diagram showing an example of the configuration of a mobile object control system according to a second embodiment. [Figure 13] 13 is a flowchart showing an example of a setting procedure performed by a target position trajectory setting unit according to the second embodiment. [Figure 14] 11 is a flowchart showing an example of a procedure of a mobile object control system according to a second embodiment. [Figure 15] FIG. 13 is a block diagram showing an example of the configuration of a mobile object control system according to a fourth embodiment. [Figure 16] 13 is a flowchart showing an example of a procedure of a mobile object control system according to a fourth embodiment. [Figure 17] FIG. 13 is a block diagram showing an example of the configuration of a mobile object control system according to a fifth embodiment. [Figure 18] 13 is a flowchart showing an example of a procedure of a mobile object control system according to a fifth embodiment. [Figure 19] FIG. 23 is a block diagram showing an example of the configuration of a controlled mobile object according to a sixth embodiment. [Figure 20] 23 is a flowchart showing an example of a procedure of a controlled mobile object according to the sixth embodiment. [Figure 21]FIG. 23 is a block diagram showing an example of the configuration of a controlled mobile object according to a seventh embodiment. [Figure 22] 23 is a flowchart showing an example of a procedure of a controlled mobile object according to the seventh embodiment. [Figure 23] FIG. 13 is a block diagram showing a hardware configuration of a mobile object control device according to another modified example. [Figure 24] FIG. 13 is a block diagram showing a hardware configuration of a mobile object control device according to another modified example. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] <Embodiment 1> Fig. 1 is a block diagram showing an example of the configuration of a mobile object control system according to the embodiment 1. The mobile object control system in Fig. 1 includes a control device 101 and one or more k-th mobile objects 201k (k = 1 to N) capable of communicating with the control device 101. Hereinafter, when the k-th mobile object 201k is not to be distinguished, it may be referred to as the mobile object 201.

[0011] The control device 101 in FIG. 1 includes a mobile object control device 1, a route information acquisition unit 102, a mobile object information acquisition unit 103, a trajectory tracking control unit 104, and a transmission unit 105.

[0012] 1 acquires route information regarding a target travel route that is set in advance as a travel destination of the moving body 201. The target travel route may correspond to, for example, a road, a passageway in a building, or a virtual route obtained by the technology described in JP 2020-075558 A.

[0013] The route information regarding the target driving route may be information that allows the target driving route to be calculated, or may be information on the target driving route itself. For example, the route information acquisition unit 102 may acquire map information, information detected by a sensor, and information on the destination as route information. For example, the route information acquisition unit 102 may acquire information on a route generated based on map information, information detected by a sensor, and information on the destination as route information. For example, the route information acquisition unit 102 may acquire route information generated by the mobile object 201 or another control device. The sensor includes, for example, a LiDAR (Light Detection and Ranging), a radar, a sonar, a camera, a high-precision locator, a global navigation satellite system (GNSS), etc., set on the roadside or on the mobile object.

[0014] 1 acquires mobile object information related to a state quantity of the mobile object 201. The mobile object information includes, for example, information such as the number, attributes, size, travel route, position, speed, acceleration, steering angle, attitude, and angular velocity of the mobile object 201.

[0015] The number includes, for example, a number for distinguishing the moving body 201, such as a "vehicle registration plate" or a "vehicle number plate." The attribute may include, for example, information indicating the type of the moving body 201, such as a "vehicle" or a "legged robot," or information such as a "normal vehicle," an "emergency vehicle," or "running in a priority lane / not running." The size includes, for example, the length of the moving body 201 in the vertical, horizontal, and height directions. The travel route includes, for example, the number of the route on which the moving body 201 currently exists, point cloud data, and coordinate information of the intersection of the route with other routes.

[0016] Information such as the position, speed, acceleration, steering angle, attitude angle, and angular velocity of the moving body 201 may be measured by a roadside LiDAR, radar, sonar, camera, or the like, or may be measured by a high-precision locator of the moving body 201, a GNSS, a steering angle sensor, a steering torque sensor, an inertial measurement unit (IMU), a speed sensor, and an acceleration sensor, or may be estimated by applying a filtering process to these pieces of information. The filtering process includes, for example, a known Kalman filter. In addition, it is not necessary to measure all of the state quantities described above. For example, the position and speed may be estimated by integrating past speeds and accelerations, respectively, and the attitude angle may be estimated by integrating past angular accelerations. In this way, when the target position trajectory setting unit 12 sets the target position trajectory based on the state quantities of the moving body 201, any state quantity can be used as the state quantity of the moving body 201. If the moving body 201 is a legged robot, the state quantities such as the position of the moving body 201 may be measured by angle sensors, torque sensors, contact sensors at the tips of the legs, or the like of each joint instead of the steering angle sensor and steering torque sensor.

[0017] The moving body control device 1 in FIG.

[0018] The acquisition unit 11 acquires a target driving route based on the route information acquired by the route information acquisition unit 102. When the route information indicates a target driving route, the acquisition unit 11 may acquire the target driving route from the route information, or when the route information does not indicate a target driving route, the acquisition unit 11 may acquire the target driving route by performing a calculation on the route information. The acquisition unit 11 also acquires state quantities of one or more moving bodies 201 based on the moving body information acquired by the moving body information acquisition unit 103. When the moving body information indicates a state quantity, the acquisition unit 11 may acquire the state quantity from the moving body information, or when the moving body information does not indicate a state quantity, the acquisition unit 11 may acquire the state quantity by performing a calculation on the moving body information.

[0019] The target position trajectory setting unit 12 sets a target position trajectory indicating the target position of each moving body 201 at each time along the target traveling path based on the state quantity and the target traveling path of one or more moving bodies 201 acquired by the acquisition unit 11. When one or more moving bodies are multiple moving bodies 201 and the target traveling path has a route intersection, the target position trajectory setting unit 12 sets a target position trajectory in which the magnitude relationship of the relative position of each moving body 201 with respect to the route intersection is maintained based on the state quantities of the multiple moving bodies 201, the target traveling path, and the route intersection. The route intersection includes, for example, at least one of a point where the routes intersect and a point where the routes join. In this specification, for example, at least one of A, B, C, ..., and Z means any one of all combinations of one or more extracted from the group of A, B, C, ..., and Z. The sign of the relative position of the moving body with respect to the route intersection before passing through the route intersection is positive, and the magnitude of the relative position is the distance between the route intersection and the position of the moving body. The sign of the relative position of the moving body with respect to the route intersection after passing through the route intersection is negative, and the magnitude of the relative position is the distance between the route intersection and the position of the moving body. Maintaining the magnitude relationship of the relative positions of each moving body 201 with respect to the route intersection corresponds to maintaining the order of the route intersection and each moving body 201 when the moving bodies 201 are arranged on a number line with the route intersection as the origin. As will be described later in detail, the target position trajectory setting unit 12 sets the target position trajectory by solving a constrained optimization problem and using a known model predictive control.

[0020] The trajectory tracking control unit 104 in FIG. 1 determines a control amount for driving each moving body 201 based on the route information acquired by the route information acquisition unit 102, the moving body information acquired by the moving body information acquisition unit 103, and the target position trajectory set by the moving body control device 1. The trajectory tracking control unit 104 may use a known PID (Proportional-Integral-Differential Controller) control, a known model predictive control, or a known data-driven control, etc., to determine the control amount. The content of the control amount may differ depending on the type of the moving body 201. For example, when the moving body 201 is a vehicle, the trajectory tracking control unit 104 may determine a target steering angle, a target speed, and a target acceleration as the control amount, and when the moving body 201 is a leg robot, the trajectory tracking control unit 104 may determine a torque command value, an angle command value, and an angular velocity command value of each joint as the control amount.

[0021] 1 transmits the control amount determined by the trajectory tracking control unit 104 to each moving object 201. For example, wireless communication using a Wi-Fi (registered trademark) router and a 5G router is used to transmit the control amount.

[0022] The k-th moving body 201k (k=1 to N) includes a receiving unit 202k and an actuator control unit 203k. The moving body 201 may be a real object such as an unmanned guided vehicle, an automobile, or a legged robot, or may be a model in a virtual space without a real object. The first moving body 2011 to the N-th moving body 201N may be of different types. For example, some of the first moving body 2011 to the N-th moving body 201N may be unmanned guided vehicles, and the rest may be legged robots. In the first embodiment, for the sake of simplicity, a case where the first moving body 2011 to the N-th moving body 201N are of the same type will be described as an example.

[0023] 1 receives a control amount transmitted from the control device 101. For example, wireless communication using a Wi-Fi (registered trademark) antenna and a 5G router is used to receive the control amount.

[0024] 1 performs control to drive the actuator of the k-th moving body 201k based on the control amount, using the control amount received by the receiving unit 202k as a command value. The actuator control unit 203k includes at least one of an EPS (Electric Power Steering - Electric Control Unit) motor of an automobile, a power train unit, a brake control unit, a wheel control device of an automatic guided vehicle, a motor control unit, and a motor control unit attached around the joint of a leg robot, for example.

[0025] <Relationship between the moving object's position and the target driving route> 2 and 3 are diagrams for explaining the relationship between the position of the moving body 201 and a target traveling route 301 acquired by the acquisition unit 11. In Fig. 2, a solid line represents the target traveling route 301, and a pentagon represents the moving body 201. The position p of the moving body 201 is represented by the coordinates (x, y) of a point defined with respect to an XY coordinate system fixed to the ground, for example.

[0026] In the following description, when there are multiple moving bodies 201, the i-th moving body 201i and the j-th moving body 201j are abbreviated as moving bodies i, j, etc., and the coordinates of the moving body i are expressed as (x [i] ,y [i] The yaw angle Ψ of the moving body 201 is the angle of the moving body with respect to the X-axis of the XY coordinate system. The target travel path 301 is a set of points (x, y). In order to distinguish from the coordinates of the moving body 201, in the following description, QQ is a set of target travel path numbers, and when q∈QQ, each coordinate of the q-th target travel path is expressed as ( [q] x path , [q] y path Due to restrictions on the characters that can be used in the specification, the outlined Q will be written as QQ in the text of this specification.

[0027] In the first embodiment, the coordinate system of the moving body 201 is defined by a longitudinal position and a lateral position deviation with respect to the target traveling path 301. The longitudinal position represents the position of the moving body 201 on the target traveling path 301, with the start point of the target traveling path 301 being the origin 301a and the traveling direction of the moving body 201 being positive. The longitudinal position s of the moving body 201 means the longitudinal position of a point p' on the target traveling path 301 that is closest to the position p of the moving body 201.

[0028] Vertical position s and coordinate ( [q] x path , [q] y path ) and ( [q] x path , [q] y path )=ξ q (s) The function ξ q may be a function obtained by polynomial approximation from the group of points on the target travel path 301, may be a function obtained by linear interpolation processing, or may be a spline function. The lateral position deviation is defined as the distance between the position p and the point p'. In the following description, the lateral position deviation is not taken into account, but may be taken into account. In addition, the method of setting the coordinate system of the moving body 201 may be any other method as long as it associates the position of the moving body 201 with the target travel path 301.

[0029] As shown in Figure 3, [i] conf,j is the position of the route intersection 302 where the target travel route 301 of the moving object i and the target travel route 301 of the moving object j intersect, with the origin 301a of the target travel route 301 of the moving object i as the reference. [i] conf,j is the longitudinal position s of the moving object i on the target travel route 301, which indicates the position of the route intersection 302. [j,k] conf,i is the position of the route intersection 302 where the target travel route 301 of the moving bodies j and k intersects with the target travel route 301 of the moving body i, with the origin 301b of the target travel route 301 of the moving bodies j and k as the reference. [j,k] conf,iis the longitudinal position s of the moving objects j and k on the target traveling route 301, which indicates the position of the route intersection 302. The origins 301a and 301b may be set, for example, to a stop line on the target traveling route 301 just before the route intersection 302, or may be set at any point on the target traveling route 301.

[0030] <Procedure for setting target position trajectory> Next, an example of a procedure for setting a target position trajectory at a certain time by the target position trajectory setting unit 12 according to the present embodiment 1 will be described. Fig. 4 is a flowchart showing an example of the setting procedure by the target position trajectory setting unit 12 according to the present embodiment 1.

[0031] In step S1, the target position trajectory setting unit 12 counts the number of moving bodies for which the target position trajectory has not been set, and judges whether the target position trajectory has been set for all moving bodies. If it is judged that the target position trajectory has been set for all moving bodies, the procedure in Fig. 4 ends, and if it is judged that the target position trajectory has not been set for all moving bodies, the process proceeds to step S2.

[0032] In step S2, the target position trajectory setting unit 12 determines the order of the preceding moving bodies according to rules 1 to 3 below, and selects a moving body for which a target position trajectory is to be set, starting with the preceding moving body that is the most advanced. Note that, if all the moving bodies 201 are traveling only on one path having a path intersection 302 among the target traveling path 301, it is sufficient to set the distance between the moving bodies 201 and travel, so it is not essential to determine the preceding moving body according to the following rules. This is true not only for the first embodiment, but also for the second and subsequent embodiments.

[0033] (Rule 1) When two moving objects 201 are traveling on one route having a route intersection 302 among a target traveling route 301, the moving object that is ahead in the traveling direction is determined as the preceding moving object.

[0034] (Rule 2) When two moving objects 201 are traveling on two routes that share a route intersection 302 among the target traveling routes 301, the moving object with the smaller relative position distance to the route intersection 302 is determined as the preceding moving object.

[0035] (Rule 3) When three or more moving bodies 201 exist, a unique order of the preceding moving body is determined that satisfies all of the precedence-successor relationships determined by rules 1 and 2. When the precedence-successor relationships are inconsistent and a unique order of the preceding moving body cannot be determined, a unique order of the preceding moving body is determined based on, for example, attribute information of each moving body 201.

[0036] Rule 1 is a rule for setting a safe target position trajectory. In the first embodiment, the target position trajectory setting unit 12 selects moving bodies for which a target position trajectory is to be set in order from the preceding moving body that is ahead in position, and sets the target position trajectory. In this case, the target position trajectory of the moving body 201 that is ahead in position is set ignoring the following moving body 201. Here, it is assumed that, of two moving bodies 201 traveling on the same route, the rear moving body 201 is set as the leading moving body. In this case, the front moving body 201 travels so as to move away from the rear moving body 201, but if an obstacle such as another moving body exists further ahead of the front moving body 201, a safe target position trajectory cannot be set for the front moving body 201. In order to suppress such a problem, rule 1 is defined in the first embodiment.

[0037] Rule 2 is also a rule for moving bodies 201 to pass each other or join together safely, and is a rule for setting a safe target position trajectory, similar to rule 1. For example, assume that moving body i is located exactly at a route intersection 302, and moving body j is heading toward the route intersection 302 on a route different from the route of moving body i. In this case, the relationship between moving bodies i and j is similar to the relationship of rule 1, that is, the relationship between two moving bodies 201 traveling on the same route. Therefore, if the target position trajectory of moving body j is set ignoring moving body i, it is not possible to set a safe target position trajectory for moving body i, similar to the state described in rule 1. In order to suppress such a problem, rule 2 is defined in the first embodiment.

[0038] Rule 3 is a rule for cases where there are three or more moving bodies. First, for safety, the order of the preceding moving body is determined so as to satisfy the relationship obtained by rules 1 and 2. If the precedence-successor relationship is inconsistent and a unique order of the preceding moving body cannot be determined, the target position trajectory setting unit 12 determines a unique order of the preceding moving body based on the attribute information of the moving body 201. For example, the target position trajectory setting unit 12 determines a moving body whose attribute information indicates an emergency vehicle such as an ambulance as the preceding moving body, as far as possible within a range where the relationship obtained by rules 1 and 2 is satisfied.

[0039] Each time the process of step S2 is performed, the target position trajectory setting unit 12 may determine the order of the preceding moving body according to rules 1 and 2. Alternatively, in the second or subsequent processes of step S2, the target position trajectory setting unit 12 may use the order of the preceding moving body determined in the initial process of step S2. For example, in a configuration in which the order determined in the initial process of step S2 is used, when a new moving body enters the target travel path, the target position trajectory setting unit 12 may determine the preceding moving body according to rules 1 and 2 only for another moving body related to the new moving body, and then re-determine the order of the preceding moving body according to rule 3.

[0040] In step S3 of FIG. 4, the target position trajectory setting unit 12 sets a target position trajectory for the moving body 201 selected in step S2 based on the state quantity and the target travel route of the moving body 201. When there are multiple moving bodies 201 and the target travel route has a route intersection, the target position trajectory setting unit 12 sets a target position trajectory based on the state quantity, the target travel route, and the route intersection of the selected moving body 201 and the moving body 201 preceding it. Here, in the target position trajectory, the magnitude relationship of the relative positions of the selected moving body 201 and the moving body 201 preceding it with respect to the route intersection is maintained. As a result, a trajectory that is ahead of one moving body 201 in position and does not collide with the target position trajectory of another moving body 201 set before the one moving body 201 is set as the target position trajectory of the one moving body 201. The target position trajectory setting unit 12 sets the target position trajectory of the moving body 201, for example, by solving an optimization problem that will be described in detail later.

[0041] In step S4, the target position trajectory setting unit 12 stores the set target position trajectory of the moving body 201 in memory. After that, the process returns to step S1. The target position trajectory of the moving body 201 stored in memory in step S4 is used as the target position trajectory of the other moving bodies 201 thereafter. The target position trajectory setting unit 12 may refer to the target position trajectory of the other moving bodies from the result of one step before. In this case, the target position trajectory setting unit 12 may set the target position trajectories of all the moving bodies 201 in parallel.

[0042] Next, a description will be given of an example of the process of step S3, that is, an example in which the target position trajectory setting unit 12 solves an optimization problem to set a target position trajectory of the moving body 201. The target position trajectory setting unit 12 solves an optimization problem expressed by the following equations (1) and (2) to set a target position trajectory at time t0.

[0043]

number

[0044]

number

[0045] Equation (1) is the state quantity x such that the value of the evaluation function J, which is a real-valued function, is minimized in the time interval t0 to t0+T. → (t) and input u → This means that (t0≦t≦t0+T) is obtained. Note that due to restrictions on the character notation that can be used in the specification, for example, x with "→" on the upper side will be written as x → It is written as x → (t) is an n-dimensional real vector, and u → (t) is an m-dimensional real vector. Equation (2) represents the constraints of the optimization problem.

[0046] When setting the target position trajectory of the moving object i, for example, x → (t) is the vertical position s of the moving object i [i] (t) and the longitudinal velocity v [i] (t) is set to a vector of u → (t) is the vertical acceleration a [i] (t). By solving the optimization problem, the target position trajectory setting unit 12 can obtain the optimal u →* (t) (t0≦t≦t0+T) can be obtained, and u →* x when (t) is controlled as the input →* (t) (t0≦t≦t0+T). Then, the target position trajectory setting unit 12 calculates the calculated x →* Vertical position s included in (t) [i] (t), or vertical position s [i] The coordinates of the XY coordinate system converted from (t) can be set as the target position trajectory.

[0047] The evaluation function J in the formula (1) is defined, for example, as the following formula (3).

[0048]

number

[0049] The first term on the right side of equation (3) represents the cost at time t0+T, the second term on the right side represents the cost over the entire prediction interval, and the third term on the right side corresponds to the barrier function of the constraint described later. → (t), t) is expressed as the following equation (4), and the function L(x → (t),u → (t), t) are expressed as follows:

[0050]

number

[0051]

number

[0052] x in Equation (4) and Equation (5) →- (t) is the reference value of the state quantity, and W T,x and W x (t) is an n × n positive semidefinite matrix, and W u (t) is an m × m positive semidefinite matrix. Due to restrictions on the character notation that can be used in the specification, for example, x with "→" above it and "-" above it will be written as x →- By the condition of positive semidefiniteness, the function φ(x → (t), t) and the function L(x → (t),u → Since the minimum value of (t), t is 0, minimizing equations (4) and (5) means bringing the state quantities closer to the reference values ​​and reducing the inputs.

[0053] The barrier function h(x → The equations relating to (t) and (t) are expressed as the following equations (6) and (7).

[0054]

number

[0055]

number

[0056] Due to restrictions on characters that can be used in the specification, in the text of this specification, the blank I in formula (6) and formula (7) will be written as II, and the blank J will be written as JJ. Also, in the text of this specification, for example, s and e with "^" above them in formula (6) and formula (7) will be written as s^ and e^.

[0057] II is a set of mobile unit numbers of the controlled mobile units, and ρ 0,1 , a0, b0 are positive constants, and γ(i) is a function that returns the route number of the mobile unit i. [j] is a predicted trajectory of the moving object j, which may be obtained by solving an optimization problem for the moving object j, or may be predicted based on the current state quantities.

[0058] Equation (6) expresses the vertical position s of the moving object i at time t. [i] (t) Gathering JJ s All the set S [i] s,j of It means that it is included in the intersection of the set S. [i] s,j is the vertical position s that is smaller than the vertical position of the moving object j by a0 or more as shown in FIG. [i] (t) represents the set of JJ s represents the set of all other moving objects that are greater than the target moving object i in the vertical direction by a0 or more at time t0 and travel on the same route as the target moving object i. In other words, equation (6) requires a solution that "maintains the distance between the target moving object i and other moving objects on the same route as the target moving object i at a constant value or more."

[0059] Equation (7) expresses the vertical position s of the moving object i at time t. [i] (t) Gathering JJ x All the set S [i] x,j of It means that it is included in the intersection of the set S.[i] x,j is the relative position e of the moving object i with respect to the route intersection 302 as shown in FIG. [i] j (t) is the relative position e of the moving object j with respect to the route intersection 302 [j] i (t) is greater than b0 by a vertical position s [i] (t) represents the set of JJ x represents a set of all other moving bodies that, at time t0, have a relative position distance to the route intersection 302 that is smaller than b0 or more than that of the moving body i to be set, and that travel on another route that shares the route intersection 302 with the route traveled by the moving body i. In other words, equation (7) requires a solution that "maintains a magnitude relationship between the relative position of the moving body i to be set and the relative positions of other moving bodies that exist on another route that shares the route intersection 302 with the route on which the moving body i exists."

[0060] The barrier function h(x → (t), t) are expressed by the following equation (8).

[0061]

number

[0062] The first term including a0 in equation (8) corresponds to equation (6), and the second term including b0 corresponds to equation (7). Minimizing the time integral of equation (8) or the value of the third term in equation (3) is equivalent to finding a solution that satisfies equations (6) and (7).

[0063] X and U in the first line of equation (2) represent a set of allowable values ​​of the state quantities and inputs that are invariant in the time interval from t0 to t0 + T. This set is set, for example, as shown in the following equation (9) depending on the domains of the longitudinal position, longitudinal velocity, and longitudinal acceleration.

[0064]

number

[0065] x in equation (9) → The symbols on either side of ≦ mean that the inequality ≦ relationship applies to each element. min and V max represents the minimum and maximum allowable speeds, and A min and A max represents the minimum and maximum allowable accelerations.

[0066] The second line of equation (2) represents the equality constraint related to the dynamic characteristics of the moving object i. The function f(x → (t),u → (t), t) are expressed, for example, as in the following equation (10).

[0067]

number

[0068] The third line of equation (2) is a constraint to be considered at each time in the time interval. C(t) representing this constraint is expressed, for example, as in the following equation (11).

[0069]

number

[0070] Here, a1 and b1 are positive constants. Since a1 is a constraint on the distance between vehicles to prevent collisions, it is desirable that the value be large enough to prevent contact between the moving bodies 201, and unless there is a special reason, it may be the same value as a0. Furthermore, since b1 is a constraint on the distance between vehicles to prevent collisions, it is desirable that the value be large enough to prevent contact between the moving bodies 201, and unless there is a special reason, it may be the same value as b0.

[0071] C(t) in equation (11) is expressed as the product of two sets of state quantities. The first part of C(t) (before the product symbol) is the vertical position of a certain moving object 201 and the JJ sThis represents a set of all the moving objects 201 included in the set whose vertical position is at least a1 away, as shown in FIG. 7. For example, after setting the value of a1 to a sufficiently large value, x → If (t) is included in this set, then mobile unit i is JJ s This means that there is no collision with the moving object 201.

[0072] The second part of C(t) (after the intersection symbol), the relative position of a certain mobile unit 201 with respect to a route intersection 302, and JJ x The distance between the relative position of all other moving objects 201 included in x and the route intersection 302 is b1 or more as shown in FIG. 8. For example, after setting the value of b1 to a sufficiently large value, → If (t) is included in this set, then mobile unit i is JJ x This means that there is no collision with the moving object 201.

[0073] As a result, the target position trajectory setting unit 12 can set a target position trajectory that maintains the magnitude relationship of the relative positions between one moving body 201 and another moving body 201 traveling on another route that shares the route intersection 302 with the route traveled by the one moving body 201. This allows the moving body control device 1 to suppress deadlock and smoothly and safely guide the moving body 201 passing through the route intersection 302.

[0074] <Procedures for mobile control system> 9 is a flowchart showing an example of a procedure of the mobile object control system (that is, the control device 101 and the mobile object 201) according to the embodiment 1. Steps S11 to S15 are the procedure of the control device 101, and steps S21 and S22 are the procedure of the mobile object 201.

[0075] In step S11, the route information acquisition unit 102 acquires route information, and the acquisition unit 11 acquires the target driving route 301. [q] x path , [q] y path ),q∈QQ.

[0076] In step S12, the moving object information acquisition unit 103 acquires moving object information, and the acquisition unit 11 acquires the state quantity of the moving object 201. The moving object information includes, for example, the number, attribute, size, travel route, position, speed, acceleration, vertical position, vertical speed, vertical acceleration, steering angle, attitude, and angular velocity of the moving object 201.

[0077] In step S13, the target position trajectory setting unit 12 of the moving body control device 1 calculates the target position trajectory (ξ q (s * (t)), t0≦t≦t0+T).

[0078] In step S14, the trajectory tracking control unit 104 determines a control amount for causing each moving body 201 to follow the target position trajectory based on the route information in step S11, the moving body information in step S12, and the target position trajectory in step S13. The control amount is, for example, a target steering angle δ of the moving body (i∈II). [i] and the target longitudinal acceleration a [i] It is.

[0079] In step S15, the transmitter 105 of the control device 101 transmits the control amount of step S14 to the k-th moving object 201k.

[0080] In step S21, the receiver 202k of the k-th moving object 201k receives the control amount transmitted in step S15.

[0081] In step S22, the actuator control unit 203k calculates the steering angle δ [k] The actuator control unit 203k controls the EPS motor of the k-th moving body 201k based on the received target vertical acceleration a [k]The brake control unit and the power train unit of the k-th moving body 201k are controlled based on the above. After step S22, the process returns to step S11. Note that, when there are two or more moving bodies 201, the processes of steps S21 and S22 for each moving body 201 may be performed in parallel.

[0082] <Summary of the first embodiment> FIG. 10 is a diagram showing the traveling of the moving body 201 when the magnitude relationship of the relative positions is not maintained in the target position trajectory. FIG. 10 shows state transitions at times t, t+1, and t+2. The upper half of the diagram at each time shows the positional relationship between the path and the moving body on the XY plane, and the lower half of the diagram shows the relative position of the moving body 201 with respect to the path intersection 302 on a number line. If the magnitude relationship of the relative positions is not maintained, the moving body i may accelerate just before the path intersection 302 and reach the path intersection 302 before the moving body j. In this case, if the target position trajectory of the moving body j is planned while ignoring the moving body i, there is a risk that the moving body j may collide with the moving body i.

[0083] 11 is a diagram showing the traveling of the moving object 201 when the magnitude relationship of the relative positions is maintained on the target position trajectory. Maintaining the magnitude relationship of the relative positions means that the moving objects 201 pass the route intersection 302 in order starting from the moving object 201 closest to the route intersection 302. In other words, the moving object i will not arrive at the route intersection 302 before the moving object j.

[0084] According to the mobile object control device 1 of the first embodiment, a target position trajectory is set in which the magnitude relationship between the relative positions is maintained. With this configuration, the mobile object 201 can pass through the route intersection 302 in order from the mobile object 201 closest to the route intersection 302 as shown in FIG. 11, so that deadlock can be suppressed and the mobile object 201 passing through the route intersection 302 can be guided smoothly and safely.

[0085] <Embodiment 2> In the first embodiment, it was possible to suppress deadlock in a situation where a route intersection 302 exists by setting a target position trajectory so as to maintain the magnitude relationship of the relative position of the moving body 201 with respect to the route intersection 302. In contrast to this, in the second embodiment, it is possible to set a target position trajectory that flexibly meets requests such as traffic manners and rules by setting a target position trajectory in consideration of moving body information, that is, the state quantity of each moving body 201.

[0086] 12 is a block diagram showing an example of the configuration of a mobile object control system according to the present embodiment 2. In the following, among the components according to the present embodiment 2, components that are the same as or similar to the components described above are given the same or similar reference numerals, and different components will be mainly described.

[0087] The configuration of Fig. 12 is the same as the configuration of the mobile object control device 1 of Fig. 1 with the addition of a priority determination unit 13. The priority determination unit 13 of Fig. 12 determines the priority p of each mobile object 201 based on the state quantity of each mobile object 201. For example, the priority determination unit 13 may determine the priority p according to a preset rule based on the state quantity of each mobile object 201, or may determine the priority p by searching a preset scenario based on the state quantity of each mobile object 201. Also, for example, the priority determination unit 13 may determine the priority p by solving an optimization problem based on the state quantity of each mobile object 201, or may determine the priority p according to a heuristic method such as a neural network based on the state quantity of each mobile object 201.

[0088] <How to determine priority> Hereinafter, a description will be given of an example in which the priority determination unit 13 determines the priority p in accordance with a preset rule based on the state quantity of each moving object 201. The priority rule according to the second embodiment is defined as follows.

[0089] (Priority Rules) The priority determination unit 13 gives a higher priority to the moving object 201 moving on the main line in accordance with traffic regulations and traffic manners.

[0090] <Setting target position trajectory considering priority> The target position trajectory setting unit 12 sets the target position trajectory taking into consideration the priority. The target position trajectory setting unit 12 sets the set JJ s and the set JJ of Eq. (7) x Instead of this, the set JJ of the following formula (12) s and the set JJ of the following equation (13) x is used.

[0091]

number

[0092]

number

[0093] p [i] (t0),p [j] (t0) represents the priority of moving objects i and j at time t0. The set JJ in Eq. (12) s represents a set of all other moving objects that have a higher priority than the moving object i and travel along the same route as the moving object i at time t0. x represents a set of all other moving objects that have a higher priority than the target moving object i at time t0 and travel on other routes that share a route intersection 302 with the route traveled by the moving object i.

[0094] The set JJ of Eq. (6) s and the set JJ of Eq. (7) x is determined by the positional relationship of the moving object 201, whereas the set JJ s and the set JJ of Eq. (13) xis determined by the priority of the moving body 201. For this reason, in the second embodiment, the target position trajectory setting unit 12 is configured to set the target position trajectory of one moving body 201 by considering only the priority of the other moving body 201 that is higher than the one moving body 201. In other words, the target position trajectory setting unit 12 is configured to set the target position trajectory of one moving body 201 by ignoring the other moving bodies 201 that are lower in priority than the one moving body 201.

[0095] Fig. 13 is a flowchart showing an example of a setting procedure of the target position trajectory setting unit 12 according to the present embodiment 2. The setting procedure in Fig. 13 is similar to the setting procedure in Fig. 4 except that step S2 is changed to step S2a, so step S2a will be mainly described here.

[0096] In the first embodiment, in step S2 in Fig. 4, the target position trajectory setting unit 12 selected a moving body for which a target position trajectory is to be set, starting with the moving body with the most advanced position. In contrast, in the second embodiment, in step S2a in Fig. 13, the target position trajectory setting unit 12 determines the order of priority, and selects a moving body for which a target position trajectory is to be set, starting with the moving body with the highest priority. Note that a high priority does not necessarily have to coincide with a moving body being advanced in position.

[0097] <Procedures for mobile control system> Fig. 14 is a flowchart showing an example of a procedure of the mobile object control system according to the embodiment 2. The procedure in Fig. 14 is similar to the procedure in Fig. 9 with step S17 added between step S12 and step S13, so step S17 will be mainly described here.

[0098] In step S17, the priority determination unit 13 determines the priority p of each moving object 201 based on the state quantity of each moving object 201. [i] The priority determination unit 13 determines the priority p (i∈II) based on the state quantity of each moving object 201 and the target traveling route 301. [i] may be determined.

[0099] In step S13, the target position trajectory setting unit 12 calculates the target position trajectory (ξ q (s * (t)), t0≦t≦t0+T).

[0100] <Summary of the second embodiment> According to the mobile object control device 1 of the second embodiment as described above, a target position trajectory is set in consideration of priority. With such a configuration, for example, when a plurality of mobile objects 201 travel through the route intersection 302, it is possible to set a target position trajectory that takes into consideration safety, urgency, and requests, and thus a target position trajectory that meets more complicated travel needs.

[0101] Furthermore, in the second embodiment, a target position trajectory of one moving body 201 is set while ignoring other moving bodies 201 that have a lower priority than one moving body 201. With this configuration, the constraints on the optimization problem can be reduced, and a reduction in the calculation load can be expected.

[0102] <Modification> In the second embodiment, the priority determination unit 13 sets the target position trajectory of one moving body 201 while ignoring other moving bodies 201 having a lower priority than the one moving body 201, but this is not limited to the above. For example, the priority determination unit 13 may relax the constraints for collision avoidance as the priority decreases, or may set the target position trajectory for mobility less frequently. With this configuration, the constraints of the optimization problem can be reduced, and a reduction in the calculation load can be expected. In addition, the target position trajectory setting unit 12 sets the set JJ in equation (6) s and the set JJ of Eq. (7) x and the set JJ of Eq. (12) s and the set JJ in equation (13). x The target position trajectory may be set by appropriately switching between the set of the first and second positions.

[0103] <Embodiment 3> A block diagram showing an example of the configuration of a mobile object control system according to the third embodiment is similar to the block diagram of Fig. 12 in the second embodiment. Hereinafter, among the components according to the third embodiment, the components that are the same as or similar to the components described above are denoted by the same or similar reference numerals, and different components will be mainly described.

[0104] In the second embodiment, the priority rule used by the priority determination unit 13 was a rule of obeying traffic laws or manners. In contrast, in the third embodiment, the priority rule is different from that in the second embodiment and is defined as follows.

[0105] <How to determine priority> (Priority Rule 1) When two moving objects 201 are traveling on one route having a route intersection 302 among the target traveling route 301, the priority of the moving object that is ahead in the traveling direction is determined to be higher.

[0106] (Priority Rule 2) When two moving objects 201 are traveling on two routes that share a route intersection 302 among the target traveling routes 301, the priority of the moving object having a smaller relative position distance to the route intersection 302 is determined to be higher.

[0107] (Priority rule 3) When three or more moving bodies 201 exist, a unique priority order is determined that satisfies all of the priority magnitude relationships determined by rules 1 and 2. When the magnitude relationships are inconsistent and a unique priority order cannot be determined, a unique priority order is determined, for example, based on attribute information of each moving body 201.

[0108] The priority rule 1 is a rule for setting a safe target position trajectory. The target position trajectory setting unit 12 sets the target position trajectory in order from the moving body with the highest priority, and sets the target position trajectory of one moving body 201 while ignoring other moving bodies 201 with a lower priority than the one moving body 201. Here, it is assumed that, of two moving bodies 201 traveling on the same route, the priority of the rear moving body 201 is determined to be higher. In this case, if an obstacle such as another moving body exists further forward than the forward moving body 201, a safe target position trajectory cannot be set for the forward moving body 201. In order to suppress such a problem, the priority rule 1 is specified in the third embodiment.

[0109] Priority rule 2 is also a rule for moving bodies 201 to pass each other or join together safely, and is a rule for setting a safe target position trajectory, similar to priority rule 1. For example, assume that moving body i is located exactly at route intersection 302, and moving body j is heading toward the route intersection 302 on a route different from the route of moving body i. In this case, the relationship between moving bodies i and j is similar to the relationship of priority rule 1, that is, the relationship between two moving bodies 201 traveling on the same route. Therefore, if the target position trajectory of moving body j is set ignoring moving body i, it is not possible to set a safe target position trajectory for moving body i, similar to the state described in priority rule 1. In order to suppress such a problem, rule 2 is defined in this embodiment 3.

[0110] Here, an example will be described in which a unique order of priority is determined from the magnitude relationship of multiple priorities obtained by priority rules 1 and 2. For example, assume that the following relationships are obtained from priority rules 1 and 2: priority of first moving body<priority of second moving body, priority of second moving body<priority of third moving body, and priority of first moving body<priority of third moving body. This can be expressed mathematically as p [1] <p [2] ,p [2] <p [3] ,p [1] <p [3] It is expressed as p [1] ,p [2] ,p [3]are all real numbers greater than or equal to 0. This formula is equivalent to the following formula (14):

[0111]

number

[0112] Therefore, satisfying a unique priority ordering is [1] ,p [2] ,p [3] satisfies all the constraints of formula (14). Therefore, the priority determination unit 13 may determine a unique order of priority by solving the optimization problems expressed by the following formulas (15) and (16). Note that a function other than min(d1+d2+d3) may be used as the evaluation function.

[0113]

number

[0114]

number

[0115] The priority rule 3 is a rule for suppressing deadlock when the priorities of the priority rules 1 and 2 are combined to produce a contradiction and a unique priority order cannot be determined. For example, assume that a plurality of priority relationships are obtained among the first moving body 2011 to the third moving body 2013, such as the priority of the first moving body<the priority of the second moving body, the priority of the second moving body<the priority of the third moving body, and the priority of the third moving body<the priority of the first moving body. Since the first moving body cannot pass the route intersection 302 after the second moving body, the second moving body cannot pass after the third moving body, and the third moving body cannot pass after the first moving body, the target position trajectory setting unit 12 cannot set the target position trajectory based on the plurality of priority order relationships. Therefore, the priority determination unit 13 may determine a unique priority order based on attribute information of each moving body 201, or may determine a unique priority order by randomly adjusting the priority of the moving body 201. For example, the priority determination unit 13 increases the priority of a moving object whose attribute information indicates an emergency vehicle such as an ambulance, as far as possible within the scope in which the relationships obtained by the priority rules 1 and 2 are satisfied.

[0116] Furthermore, the priority determination unit 13 may adjust the priority of the moving object 201 in accordance with the following priority rule 4.

[0117] (Rule 4 of Priority) When the first moving body 2011 and the second moving body 2012 satisfy a preset priority exchange condition, the priority of the first moving body 2011 and the priority of the second moving body 2012 are exchanged.

[0118] The priority exchange conditions set forth in priority rule 4 are defined as follows:

[0119] (Priority exchange condition 1) A first moving object 2011 and a second moving object 2012 are respectively traveling on two routes that share a route intersection 302 of a target traveling route 301 .

[0120] (Priority exchange condition 2) The priority of the first moving body 2011 and the priority of the second moving body 2012 are consecutive.

[0121] (Priority exchange condition 3) When one of the first moving body 2011 and the second moving body 2012, which has the higher priority, stops, there are no other moving bodies, including the other of the first moving body 2011 and the second moving body 2012, on its stopping position trajectory.

[0122] (Priority exchange condition 4) The distance between one of the stop positions having a higher priority on the stop position trajectory and the route intersection 302 is equal to or greater than a preset distance.

[0123] According to the priority exchange condition 1, when the first moving body 2011 and the second moving body 2012 are traveling on one route having a route intersection 302 of the target traveling route 301, the priority of the first moving body 2011 and the priority of the second moving body 2012 cannot be exchanged. The reason for this is to prevent a dangerous case from occurring in which, when a higher priority is determined for a moving body behind, it is necessary to increase the speed of the moving body ahead to match the moving body behind.

[0124] The reason why the priority exchange condition 2 requires that the priorities be consecutive is that the exchange of non-consecutive priorities can be realized by combining the exchange of consecutive priorities. For example, when exchanging p1 and p3 in the order of priorities p1>p2>p3, first exchange p1 and p3, and then exchange p1 and p3.

[0125] Priority exchange conditions 3 and 4 are conditions for ensuring prevention of accidents due to priority exchange. When the priority of the moving body 201 becomes lower due to priority exchange, the constraints for determining the target position trajectory of the moving body 201 become stricter. For example, the moving body 201 with a lower priority is constrained to wait for a time that allows the moving body 201 with a higher priority to pass the route intersection 302 after decelerating and stopping. Therefore, if the priority of the moving body 201 becomes inappropriately lower due to priority exchange, the moving body 201 may be rear-ended by the moving body 201 behind it due to a sudden stop, or may be rear-ended by another moving body 201 that enters from the side at the route intersection 302. Therefore, priority exchange condition 3 is stipulated to leave a sufficient space between the moving body 201 with a lower priority and the moving body 201 behind the moving body 201 with a lower priority so that the moving body 201 behind the moving body 201 with a lower priority can react even if the moving body 201 with a lower priority safely decelerates. In addition, a priority exchange condition 4 is defined so as not to impede the passage of the moving object 201, whose priority has been increased, through the route intersection 302.

[0126] Each time the process of step S2a is performed, the priority determination unit 13 may determine the priority of the moving object according to priority rules 1 and 2. Alternatively, in the second or subsequent processes of step S2a, the priority determination unit 13 may use the priority order determined in the initial process of step S2a. For example, in a configuration in which the order determined in the initial process of step S2a is used, when a new moving object enters the target travel route, the priority determination unit 13 may determine the priority only for another moving object related to the new moving object according to priority rules 1 and 2, and then re-determine the priority order according to priority rule 3.

[0127] <Summary of the Third Embodiment> According to the mobile object control device 1 of the third embodiment as described above, the priority determination unit 13 assigns a higher priority to each mobile object 201 as the distance of the relative position of the mobile object 201 to the route intersection 302 decreases. With this configuration, a safe target position trajectory can be set.

[0128] In addition, in the third embodiment, when a first moving body and a second moving body included in the plurality of moving bodies satisfy a preset priority exchange condition, the priority of the first moving body and the priority of the second moving body can be exchanged. According to such a configuration, when the moving body 201 passes or joins at the route intersection 302, the target position trajectory for guiding the moving body 201 can be corrected according to various requests under certain conditions, so that the moving body 201 passing through the route intersection 302 can be safely and flexibly guided.

[0129] <Fourth embodiment> 15 is a block diagram showing an example of the configuration of a mobile object control system according to the present embodiment 4. In the following, among the components according to the present embodiment 4, components that are the same as or similar to the components described above are given the same or similar reference numerals, and different components will be mainly described.

[0130] In embodiments 1 to 3, the trajectory tracking control unit 104 of the control device 101 determines the control amount for each moving body 201 based on the target position trajectory set by the mobile body control device 1 of the control device 101, and the transmission unit 105 of the control device 101 transmits the control amount to each moving body 201.

[0131] In contrast to this, in the fourth embodiment, the mobile body control device 1 includes a trajectory tracking control unit 14 that determines a control amount for driving each mobile body 201 based on the target position trajectory, similar to the trajectory tracking control unit 104 in Fig. 1. That is, the mobile body control device 1 determines a control amount for each mobile body 201 based on the target position trajectory. The transmission unit 105 of the control device 101 transmits the control amount determined by the mobile body control device 1 to each mobile body 201.

[0132] <Procedures for mobile control system> Fig. 16 is a flowchart showing an example of a procedure of the mobile object control system according to the embodiment 4. The procedure in Fig. 16 is similar to the setting procedure in Fig. 9 except that step S14 is changed to step S14a, so step S14a will be mainly described here.

[0133] In step S14a, the trajectory tracking control unit 14 of the mobile object control device 1 determines a control amount for causing each mobile object 201 to track the target position trajectory based on the route information in step S11, the mobile object information in step S12, and the target position trajectory in step S13. In step S15, the transmission unit 105 of the control device 101 transmits the control amount of step S14a to the k-th mobile object 201k.

[0134] <Summary of the fourth embodiment> According to the mobile body control device 1 of the fourth embodiment as described above, a control amount for driving each mobile body 201 is determined based on a target position trajectory in which the magnitude relationship of the relative positions of the mobile bodies 201 with respect to the route intersections 302 is maintained, as in the first embodiment. Therefore, as in the first embodiment, deadlock can be suppressed, and the mobile body 201 can be guided smoothly and safely through the route intersections 302.

[0135] <Embodiment 5> 17 is a block diagram showing an example of the configuration of a mobile object control system according to the present embodiment 5. In the following, among the components according to the present embodiment 5, components that are the same as or similar to the components described above are given the same or similar reference numerals, and different components will be mainly described.

[0136] In embodiments 1 to 3, the trajectory tracking control unit 104 of the control device 101 determines the control amount for each moving body 201 based on the target position trajectory set by the mobile body control device 1 of the control device 101, and the transmission unit 105 of the control device 101 transmits the control amount to each moving body 201.

[0137] In contrast to this, in the fifth embodiment, the transmitting unit 105 of the control device 101 is configured to transmit the target position trajectory set by the mobile object control device 1 to each mobile object 201, and each mobile object 201 determines the control amount for each mobile object 201 based on the target position trajectory. The configurations of the control device 101 and the mobile object 201 will be described below.

[0138] 17 does not include a trajectory tracking control unit 104, but includes a route information acquisition unit 102, a mobile body information acquisition unit 103, a mobile body control device 1, and a transmission unit 105. The transmission unit 105 transmits the target position trajectory set by the mobile body control device 1 to each mobile body 201. For example, wireless communication using a Wi-Fi (registered trademark) router and a 5G router is used to transmit the target position trajectory.

[0139] The k-th moving object 201k (k=1 to N) in FIG. 17 includes not only a receiving unit 202k and an actuator control unit 203k, but also a route information acquiring unit 204k, a moving object information acquiring unit 205k, and a trajectory tracking control unit 206k.

[0140] The receiving unit 202 k receives the target position trajectory from the control device 101 .

[0141] The route information acquisition unit 204k acquires route information regarding a target travel route that is set in advance as a travel destination of the k-th moving object 201k. The route information acquired by the route information acquisition unit 204k is the same as the route information acquired by the route information acquisition unit 102.

[0142] The mobile object information acquisition unit 205k acquires mobile object information related to the state quantity of the k-th mobile object 201k. The mobile object information acquired by the mobile object information acquisition unit 205k is similar to the mobile object information acquired by the mobile object information acquisition unit 103.

[0143] The trajectory tracking control unit 206k determines a control amount for driving the k-th moving body 201k based on the route information acquired by the route information acquisition unit 204k, the moving body information acquired by the moving body information acquisition unit 205k, and the target position trajectory received by the receiving unit 202k. The actuator control unit 203k performs control to drive the actuator of the k-th moving body 201k based on the control amount, using the control amount determined by the trajectory tracking control unit 206k as a command value.

[0144] <Procedures for mobile control system> Fig. 18 is a flowchart showing an example of a procedure of the mobile object control system according to the embodiment 5. In steps S11 to S13 in Fig. 18, the same processes as those in steps S11 to S13 in Fig. 9 are performed.

[0145] In step S15a after step S13, the transmission unit 105 of the control device 101 receives the target position trajectory (ξ q (s * (t)), t0≦t≦t0+T) to the kth mobile station 201k.

[0146] In step S21a, the receiving unit 202k of the kth moving object 201k receives the target position trajectory transmitted in step S15a.

[0147] In step S21b, the route information acquisition unit 204k acquires route information. [q] x path , [q] y path ),q∈QQ.

[0148] In step S21c, the moving object information acquisition unit 205k acquires moving object information, which includes, for example, the number, attribute, size, travel route, position, speed, acceleration, vertical position, vertical speed, vertical acceleration, steering angle, attitude, and angular velocity of the kth moving object 201k.

[0149] In step S21d, the trajectory tracking control unit 206k determines a control amount for making the k-th moving body 201k follow the target position trajectory based on the route information in step S21b, the moving body information in step S21c, and the target position trajectory in step S21a. The control amount is, for example, a target steering angle δ of the moving body (i∈II). [i] and the target longitudinal acceleration a [i] It is.

[0150] In step S22, the actuator control unit 203k adjusts the steering angle δ [k]The actuator control unit 203k controls the EPS motor of the k-th moving body 201k based on the determined target vertical acceleration a [k] The brake control unit and the power train unit of the kth moving body 201k are controlled based on the above.

[0151] <Summary of the fifth embodiment> According to the mobile body control device 1 of the fifth embodiment as described above, a control amount for driving each mobile body 201 is determined based on a target position trajectory in which the magnitude relationship of the relative positions of the mobile bodies 201 with respect to the route intersections 302 is maintained, as in the first embodiment. Therefore, as in the first embodiment, deadlock can be suppressed, and the mobile body 201 can be guided smoothly and safely through the route intersections 302.

[0152] In addition, since the control device 101 transmits the target position trajectory, it becomes unnecessary to calculate the control amount and manage the detailed control parameters of the moving object, which were performed by the control device 101, and it is expected that the calculation load and memory load of the control device 101 will be reduced. Furthermore, since the moving object 201 calculates the control amount, the control period can be set to be short, and it is expected that the safety will be improved.

[0153] <Sixth embodiment> 19 is a block diagram showing an example of the configuration of a controlled mobile body according to the present embodiment 6. In the following, among the components according to the present embodiment 6, the components that are the same as or similar to the components described above are given the same or similar reference numerals, and different components will be mainly described.

[0154] In the fifth embodiment, the control device 101 includes the moving body control device 1 and transmits the target position trajectory of each moving body 201 to each moving body 201 .

[0155] In contrast to this, in the sixth embodiment, a controlled mobile object 211 included in the mobile object 201 has a mobile object control device 1 and sets a target position trajectory. The configuration of the controlled mobile object 211 will be described below.

[0156] The controlled mobile object 211 includes a mobile object control device 1 , a route information acquisition unit 212 , a mobile object information acquisition unit 213 , a trajectory tracking control unit 214 , and an actuator control unit 215 .

[0157] The route information acquisition unit 212 acquires route information related to a target travel route set in advance as a travel target of the moving body 201. The route information acquired by the route information acquisition unit 212 is similar to the route information acquired by the route information acquisition unit 102. Note that the route information acquisition unit 212 may acquire route information from another moving body 201 using V2V (Vehicle-to-Vehicle) communication. Using 5G communication for V2V communication can improve the control performance of each moving body.

[0158] The mobile object information acquisition unit 213 acquires mobile object information related to state quantities of one or more mobile objects 201 including the controlled mobile object 211. The mobile object information acquired by the mobile object information acquisition unit 213 is similar to the mobile object information acquired by the mobile object information acquisition unit 103. Note that the mobile object information acquisition unit 213 may acquire mobile object information from another mobile object 201 using V2V communication. Using 5G communication for V2V communication can improve the control performance of each mobile object.

[0159] The mobile body control device 1 in FIG. 19 includes an acquisition unit 11 and a target position trajectory setting unit 12 similar to the acquisition unit 11 and the target position trajectory setting unit 12 described in embodiment 1, and sets a target position trajectory for the controlled mobile body 211.

[0160] The trajectory tracking control unit 214 determines a control amount for driving the controlled mobile object 211 based on the route information acquired by the route information acquisition unit 212, the mobile object information acquired by the mobile object information acquisition unit 213, and the target position trajectory set by the mobile object control device 1. The actuator control unit 215 uses the control amount determined by the trajectory tracking control unit 214 as a command value and performs control to drive the actuator of the controlled mobile object 211 based on the control amount.

[0161] <Procedures for controlled vehicles> FIG. 20 is a flowchart showing an example of a procedure of the controlled mobile object 211 according to the sixth embodiment.

[0162] In step S31, similarly to step S11 in FIG. 9, the route information acquisition unit 212 acquires route information, and the acquisition unit 11 acquires the target travel route 301.

[0163] In step S32, similarly to step S12 in FIG. 9, the mobile object information acquisition unit 213 acquires mobile object information, and the acquisition unit 11 acquires state quantities of the mobile objects 201 including the controlled mobile object 211.

[0164] In step S33, the target position trajectory setting unit 12 of the mobile object control device 1 calculates the target position trajectory (ξ q (s * (t)), t0≦t≦t0+T).

[0165] In step S34, the trajectory tracking control unit 214 determines a control amount for causing the controlled mobile object 211 to track the target position trajectory based on the route information in step S31, the mobile object information in step S32, and the target position trajectory in step S33.

[0166] In step S35, the actuator control section 215 controls the EPS motor, the brake control unit, and the power train unit of the controlled movable object 211 based on the determined control amount.

[0167] <Summary of the sixth embodiment> According to the mobile object control device 1 of the sixth embodiment as described above, the control amount for driving the controlled mobile object 211 is determined based on the target position trajectory in which the magnitude relationship of the relative position of the mobile object 201 with respect to the route intersection 302 is maintained, as in the first embodiment. Therefore, as in the first embodiment, deadlock can be suppressed, and the controlled mobile object 211 can pass through the route intersection 302 smoothly and safely.

[0168] <Embodiment 7> 21 is a block diagram showing an example of the configuration of a controlled mobile object according to the present embodiment 7. In the following, among the components according to the present embodiment 7, the components that are the same as or similar to the components described above are given the same or similar reference numerals, and different components will be mainly described.

[0169] In the sixth embodiment, the trajectory tracking control unit 214 of the controlled mobile object 211 determined the control amount of the controlled mobile object 211 based on the target position trajectory set by the mobile object control device 1. In contrast to this, in the seventh embodiment, the mobile object control device 1 is provided with a trajectory tracking control unit 14 that determines the control amount for driving the controlled mobile object 211 based on the target position trajectory, similar to the trajectory tracking control unit 214 in Fig. 19. In other words, the mobile object control device 1 sets the target position trajectory for the controlled mobile object 211, and determines the control amount for the controlled mobile object 211. The actuator control unit 215 performs control to drive the actuator of the controlled mobile object 211 based on the control amount, using the control amount determined by the mobile object control device 1 as a command value.

[0170] <Procedures for controlled vehicles> Fig. 22 is a flowchart showing an example of a procedure of the controlled object 211 according to the embodiment 7. The procedure in Fig. 22 is similar to the setting procedure in Fig. 20 except that step S34 is changed to step S34a, so step S34a will be mainly described here.

[0171] In step S34a, the trajectory tracking control unit 14 of the mobile body control device 1 determines a control amount for causing the controlled mobile body 211 to track the target position trajectory based on the route information of step S31, the mobile body information of step S32, and the target position trajectory of step S33.

[0172] <Summary of the seventh embodiment> According to the mobile object control device 1 of the seventh embodiment as described above, the control amount for driving the controlled mobile object 211 is determined based on the target position trajectory in which the magnitude relationship of the relative position of the mobile object 201 with respect to the route intersection 302 is maintained, as in the first embodiment. Therefore, as in the first embodiment, deadlock can be suppressed, and the controlled mobile object 211 can pass through the route intersection 302 smoothly and safely.

[0173] <Other Modifications> The acquisition unit 11 and the target position trajectory setting unit 12 in FIG. 1 described above will be hereinafter referred to as the "acquisition unit 11, etc." The acquisition unit 11, etc. are realized by a processing circuit 81 shown in FIG. 23. That is, the processing circuit 81 includes the acquisition unit 11 that acquires a state quantity of one or more moving bodies 201 and a target running route that is set in advance as a target for the running of the moving bodies 201, and a target position trajectory setting unit 12 that indicates the target position of each moving body 201 at each time along the target running route 301 based on the state quantities of the multiple moving bodies 201, the target running route 301, and the route intersection 302 when the one or more moving bodies 201 are multiple moving bodies 201 and the target running route 301 has a route intersection 302, and sets a target position trajectory in which the magnitude relationship of the relative positions of each moving body 201 with respect to the route intersection 302 is maintained. The processing circuit 81 may be implemented by dedicated hardware, or may be implemented by a processor that executes a program stored in a memory. The processor may be, for example, a central processing unit, a processing unit, an arithmetic unit, a microprocessor, a microcomputer, or a DSP (Digital Signal Processor).

[0174] When the processing circuit 81 is a dedicated hardware, the processing circuit 81 corresponds to, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or a combination of these. Each function of the acquisition unit 11 and the like may be realized by a circuit in which the processing circuits are distributed, or the functions of each unit may be realized by a single processing circuit.

[0175] When the processing circuit 81 is a processor, the functions of the acquisition unit 11 and the like are realized by a combination with software and the like. Note that the software and the like includes, for example, software, firmware, or software and firmware. The software and the like are described as a program and stored in a memory. As shown in FIG. 24, a processor 82 applied to the processing circuit 81 realizes the functions of each unit by reading and executing a program stored in a memory 83. That is, the mobile body control device 1 includes a memory 83 for storing a program which, when executed by the processing circuit 81, results in the execution of the following steps: acquiring state quantities of one or more mobile bodies 201 and a target travel route set in advance as a travel target for the mobile bodies 201; and, when the one or more mobile bodies 201 are multiple mobile bodies 201 and the target travel route 301 has a route intersection 302, indicating the target positions of each mobile body 201 at each time along the target travel route 301 based on the state quantities of the multiple mobile bodies 201, the target travel route 301, and the route intersection 302, and setting a target position trajectory in which the magnitude relationship of the relative positions of each mobile body 201 with respect to the route intersection 302 is maintained. In other words, this program can be said to cause a computer to execute the procedures and methods of the acquisition unit 11, etc. Here, the memory 83 may be, for example, a non-volatile or volatile semiconductor memory such as a Random Access Memory (RAM), a Read Only Memory (ROM), a flash memory, an Erasable Programmable Read Only Memory (EPROM), an Electrically Erasable Programmable Read Only Memory (EEPROM), an HDD (Hard Disk Drive), a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, a DVD (Digital Versatile Disc), a drive device for any of these, or any storage medium to be used in the future.

[0176] The above describes a configuration in which each function of the acquisition unit 11, etc. is realized by either hardware or software, etc. However, the present invention is not limited to this, and a configuration in which a part of the acquisition unit 11, etc. is realized by dedicated hardware and another part is realized by software, etc. For example, the function of the acquisition unit 11 can be realized by a processing circuit 81 as dedicated hardware, and the other functions can be realized by the processing circuit 81 as a processor 82 reading and executing a program stored in a memory 83.

[0177] As described above, the processing circuitry 81 can realize each of the above-mentioned functions by hardware, software, or a combination of these.

[0178] It should be noted that the embodiments and modifications may be freely combined, and the embodiments and modifications may be modified or omitted as appropriate.

[0179] The above description is illustrative in all respects and is not restrictive. It is understood that countless variations not illustrated can be envisioned. [Explanation of symbols]

[0180] 1 Mobile object control device, 11 Acquisition unit, 12 Target position trajectory setting unit, 13 Priority determination unit, 101 Control device, 102, 204k, 212 Route information acquisition unit, 103, 205k, 213 Mobile object information acquisition unit, 14, 104, 206k, 214 Trajectory tracking control unit, 105 Transmission unit, 201, 201k Mobile object, 202k Receiving unit, 203k, 215 Actuator control unit, 211 Controlled mobile object, 301 Target driving route, 302 Route intersection.

Claims

1. An acquisition unit that acquires state quantities of one or more moving objects and a target travel route that is set in advance as a travel target of the moving object; a target position trajectory setting unit that indicates a target position of each of the moving bodies at each time along the target traveling route based on the state quantities of the moving bodies, the target traveling route, and the route intersection, when the one or more moving bodies are a plurality of moving bodies and the target traveling route has a route intersection, and sets a target position trajectory according to a constraint condition that maintains a magnitude relationship of a relative position of each of the moving bodies with respect to the route intersection; A mobile control device comprising:

2. An acquisition unit that acquires state quantities of one or more moving objects and a target travel route that is set in advance as a travel target of the moving object; a priority determination unit that determines a priority of each of the moving bodies based on the state quantity of each of the moving bodies; a target position trajectory setting unit that indicates a target position of each of the moving bodies at each time along the target traveling route based on the state quantities of the moving bodies, the target traveling route, and the route intersection, when the one or more moving bodies are a plurality of moving bodies and the target traveling route has a route intersection, and sets a target position trajectory in which a magnitude relationship between the relative positions of each of the moving bodies with respect to the route intersection is maintained; Equipped with The target position trajectory setting unit sets the target position trajectory of one of the moving bodies while ignoring other moving bodies that have a lower priority than one of the moving bodies.

3. The mobile object control device according to claim 1, A mobile object control device, wherein the state quantity of the mobile object includes a position of the mobile object.

4. The mobile object control device according to claim 1, a priority determination unit that determines a priority of each of the moving bodies based on the state quantity of each of the moving bodies; The target position trajectory setting unit sets the target position trajectory taking into consideration the priority.

5. The mobile object control device according to claim 4, The target position trajectory setting unit sets the target position trajectory of one of the moving bodies while ignoring other moving bodies that have a lower priority than one of the moving bodies.

6. The mobile object control device according to claim 4 or 5, The priority determination unit is configured to set the priority of the moving body to be higher as the distance of the relative position of the moving body to the route intersection is smaller.

7. The mobile object control device according to claim 4 or 5, The priority determination unit A mobile body control device capable of exchanging the priority of a first moving body and the priority of a second moving body included in the plurality of moving bodies when the first moving body and the second moving body satisfy a predetermined priority exchange condition.

8. The mobile object control device according to claim 7, The priority tradeoff condition is: the first moving body and the second moving body are respectively traveling on two routes that share the route intersection point among the target traveling routes; the priority of the first moving body and the priority of the second moving body are consecutive; When one of the first moving body and the second moving body having the higher priority stops, no other moving body including the other of the first moving body and the second moving body is present on the stop position trajectory; and The distance between the one of the stop positions having the higher priority and the route intersection on the stop position trajectory is equal to or greater than a preset distance. A mobile control device comprising:

9. The mobile object control device according to claim 1 or 2, The moving body control device further includes a trajectory tracking control unit that determines a control amount for driving the moving body based on the target position trajectory.

10. A control device comprising the mobile object control device according to claim 1 or 2; The one or more mobile objects capable of communicating with the control device; Equipped with The control device includes: A route information acquisition unit that acquires route information regarding the target travel route; a mobile object information acquisition unit that acquires mobile object information related to the state quantity of the one or more mobile objects; a trajectory tracking control unit that determines a control amount for driving the moving body based on the target position trajectory set by the moving body control device; a transmitter that transmits the control amount to the moving body; Further equipped with The moving body is A receiving unit that receives the control amount from the control device; an actuator control unit that controls the actuator based on the control amount; A mobile control system comprising:

11. A control device comprising the mobile object control device according to claim 9; The one or more mobile objects capable of communicating with the control device; Equipped with The control device includes: A route information acquisition unit that acquires route information regarding the target travel route; a mobile object information acquisition unit that acquires mobile object information related to the state quantity of the one or more mobile objects; a transmission unit that transmits the control amount determined by the mobile object control device; Further equipped with The moving body is A receiving unit that receives the control amount from the control device; an actuator control unit that controls the actuator based on the control amount; A mobile control system comprising:

12. A control device comprising the mobile object control device according to claim 1 or 2; The one or more mobile objects capable of communicating with the control device; Equipped with The control device includes: A route information acquisition unit that acquires route information regarding the target travel route; a mobile object information acquisition unit that acquires mobile object information related to the state quantity of the one or more mobile objects; a transmission unit that transmits the target position trajectory set by the mobile body control device to the mobile body; Further equipped with The moving body is a receiving unit for receiving the target position trajectory from the control device; a trajectory tracking control unit that determines a control amount for driving the moving body based on the target position trajectory; an actuator control unit that controls the actuator based on the control amount; A mobile control system comprising:

13. A control moving body, 3. A mobile object control device according to claim 1, which sets the target position trajectory for the controlled mobile object; A route information acquisition unit that acquires route information regarding the target travel route; a mobile object information acquisition unit that acquires mobile object information related to the state quantity of the one or more mobile objects including the controlled mobile object; a trajectory tracking control unit that determines a control amount for driving the controlled mobile object based on the target position trajectory set by the mobile object control device; an actuator control unit that controls the actuator based on the control amount; The control mobile object further comprises:

14. A control moving body, A mobile object control device according to claim 9, which sets the target position trajectory for the controlled mobile object and determines the control amount for the controlled mobile object; A route information acquisition unit that acquires route information regarding the target travel route; a mobile object information acquisition unit that acquires mobile object information related to the state quantity of the one or more mobile objects including the controlled mobile object; an actuator control unit that controls an actuator based on the control amount determined by the mobile object control device; The control mobile object further comprises:

15. Acquire state quantities of one or more moving objects and a target travel route that is preset as a travel target of the moving object; A mobile body control method, when the one or more moving bodies are multiple moving bodies and the target travel route has a route intersection, which indicates a target position of each of the moving bodies at each time along the target travel route based on the state quantities of the multiple moving bodies, the target travel route, and the route intersection, and sets a target position trajectory according to a constraint condition that maintains a magnitude relationship between the relative positions of each of the moving bodies with respect to the route intersection.

16. Acquire state quantities of one or more moving objects and a target travel route that is preset as a travel target of the moving object; determining a priority of each of the moving bodies based on the state quantity of each of the moving bodies; When the one or more moving bodies are a plurality of moving bodies and the target travel route has a route intersection, a target position trajectory is set that indicates a target position of each of the moving bodies at each time along the target travel route based on the state quantities of the plurality of moving bodies, the target travel route, and the route intersection, and maintains a magnitude relationship between the relative positions of each of the moving bodies with respect to the route intersection; A moving body control method, wherein the target position trajectory of one of the moving bodies is set while ignoring other moving bodies having a lower priority than one of the moving bodies.

Images