Moving body control device, moving body control system, control moving body, and moving body control method
The mobile body control device addresses deadlock issues by setting target position trajectories that maintain relative position magnitudes, enabling safe and smooth navigation through intersections by using model predictive control.
Patent Information
- Application Number
- PCT/JP2023/020422
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- Filing Date
- 2023-06-01
- Publication Date
- 2024-12-05
AI Technical Summary
Conventional systems face deadlock issues when multiple moving objects navigate routes with intersections, particularly on roads without lane markings, where vehicles struggle to smoothly guide and maintain relative positions.
A mobile body control device that acquires state quantities and sets target position trajectories, maintaining the magnitude relationship of relative positions with respect to route intersections, using model predictive control to prevent collisions and ensure safe passage.
The solution effectively suppresses deadlock and enables smooth, safe navigation of moving objects through intersections by maintaining the relative position magnitude, ensuring orderly passage and reducing collision risks.
Smart Images

Figure JP2023020422_05122024_PF_FP_ABST
Abstract
Description
Mobile object control device, mobile object control system, controlled mobile object, and mobile object control method
[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.
[0002] Conventionally, a vehicle group control device is known that generates a virtual route with intersections for a road that has a width equivalent to the width of multiple lanes and does not have markings separating the lanes, such as a road at the exit of a toll booth, and smoothly guides vehicles based on that route (for example, Patent Document 1).
[0003] Japanese Patent Application Laid-Open No. 2020-075558
[0004] However, when a mobile object travels on 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 aims to provide a technology that can suppress deadlock.
[0006] The mobile body control device of the present disclosure includes an acquisition unit that acquires state quantities of one or more mobile bodies and a target driving route that is set in advance as a target for the driving of the mobile bodies, and a target position trajectory setting unit that, when the one or more mobile bodies are multiple mobile bodies and the target driving route has a route intersection, indicates the target position of each of the mobile bodies at each time along the target driving route based on the state quantities of the multiple mobile bodies, the target driving route, and the route intersection, and sets a target position trajectory that maintains the magnitude relationship of the relative positions of each of the mobile bodies with respect to the route intersection.
[0007] According to the present disclosure, a target position trajectory is set that maintains the magnitude relationship between the relative positions of the moving bodies with respect to the route intersections. 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.
[0009] 1 is a block diagram showing an example of the configuration of a mobile body control system according to a first embodiment. FIG. 1 is a diagram for explaining the relationship between the position of a mobile body and a target traveling path. FIG. 1 is a diagram for explaining the relationship between the position of a mobile body and a target traveling path. FIG. 2 is a flowchart showing an example of a setting procedure by a target position trajectory setting unit according to the first embodiment. FIG. 2 is a diagram for explaining a set used in the target position trajectory setting unit according to the first embodiment. FIG. 3 is a diagram for explaining a set used in the target position trajectory setting unit according to the first embodiment. FIG. 4 is a diagram for explaining a set used in the target position trajectory setting unit according to the first embodiment. FIG. 5 is a flowchart showing an example of the procedure of the mobile body control system according to the first embodiment. FIG. 6 is a diagram showing traveling of a mobile body based on a target position trajectory in which the magnitude relationship of relative positions is not maintained. FIG. 7 is a diagram showing traveling of a mobile body based on a target position trajectory in which the magnitude relationship of relative positions is maintained. FIG. 8 is a block diagram showing an example of the configuration of a mobile body control system according to a second embodiment. FIG. 9 is a flowchart showing an example of a setting procedure by a target position trajectory setting unit according to the second embodiment. FIG. 10 is a flowchart showing an example of the procedure of the mobile body control system according to the second embodiment. FIG. 11 is a block diagram showing an example of the configuration of a mobile body control system according to a fourth embodiment. FIG. 12 is a flowchart showing an example of the procedure of the mobile body control system according to the fourth embodiment. Fig. 10 is a flowchart showing an example of the procedure of a mobile object control system according to embodiment 5. Fig. 11 is a block diagram showing an example of the configuration of a controlled mobile object according to embodiment 6. Fig. 12 is a flowchart showing an example of the procedure of a controlled mobile object according to embodiment 6. Fig. 13 is a block diagram showing an example of the configuration of a controlled mobile object according to embodiment 7. Fig. 14 is a flowchart showing an example of the procedure of a controlled mobile object according to embodiment 7. Fig. 15 is a block diagram showing the hardware configuration of a mobile object control device according to another modified example. Fig. 16 is a block diagram showing the hardware configuration of a mobile object control device according to another modified example.
[0010] <Embodiment 1> Fig. 1 is a block diagram showing an example of the configuration of a mobile object control system according to 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) that can communicate with the control device 101. Hereinafter, when the k-th mobile object 201k does not need to be distinguished, it may be referred to as 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 mobile object 201. The target travel route may correspond to, for example, a road, a passageway within a building, or a virtual route obtained by the technology described in Japanese Patent Laid-Open No. 2020-075558.
[0013] The route information regarding the target driving route may be information from which the target driving route can be calculated, or may be information about the target driving route itself. For example, the route information acquisition unit 102 may acquire map information, information detected by a sensor, and destination information as route information. Alternatively, the route information acquisition unit 102 may acquire information about a route generated based on map information, information detected by a sensor, and destination information as route information. Alternatively, the route information acquisition unit 102 may acquire route information generated by the mobile object 201 or another control device. The sensor may include, for example, a light detection and ranging (LiDAR), radar, sonar, camera, high-precision locator, or global navigation satellite system (GNSS) installed on the roadside or the mobile object.
[0014] 1 acquires mobile object information related to the state quantities of the mobile object 201. The mobile object information includes, for example, 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 attributes 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 "standard vehicle," an "emergency vehicle," or whether the moving body 201 is traveling in a priority lane. 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 is currently located, point cloud data, and coordinate information of intersections between the route and other routes.
[0016] Information such as the position, velocity, acceleration, steering angle, attitude angle, and angular velocity of the moving object 201 may be measured by roadside LiDAR, radar, sonar, or camera, or by a high-precision locator, GNSS, steering angle sensor, steering torque sensor, inertial measurement unit (IMU), speed sensor, or acceleration sensor of the moving object 201, or may be estimated by applying filtering processing to this information. Examples of filtering processing include a well-known Kalman filter. Furthermore, it is not necessary to measure all of the above-described state quantities. For example, the position and velocity may be estimated by integrating past velocities and accelerations, respectively, and the attitude angle may be estimated by integrating past angular accelerations. This allows the target position trajectory setting unit 12 to use any state quantity as the state quantity of the moving object 201 when setting the target position trajectory based on the state quantities of the moving object 201. If the moving body 201 is a legged robot, state quantities such as the position of the moving body 201 may be measured using angle sensors, torque sensors, contact sensors at the tips of the legs, etc., at each joint instead of a steering angle sensor and a steering torque sensor.
[0017] The mobile object control device 1 in FIG. 1 includes an acquisition unit 11 and a target position trajectory setting unit 12 .
[0018] The acquisition unit 11 acquires a target driving route based on the route information acquired by the route information acquisition unit 102. If the route information indicates a target driving route, the acquisition unit 11 may acquire the target driving route from the route information, or if the route information does not indicate a target driving route, the acquisition unit 11 may acquire the target driving route by performing calculations 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. If the moving body information indicates a state quantity, the acquisition unit 11 may acquire the state quantity from the moving body information, or if the moving body information does not indicate a state quantity, the acquisition unit 11 may acquire the state quantity by performing calculations 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 quantities of one or more moving bodies 201 and the target traveling path acquired by the acquisition unit 11. When the one or more moving bodies 201 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 that maintains the magnitude relationship of the relative positions of each moving body 201 with respect to the route intersection, based on the state quantities of the multiple moving bodies 201, the target traveling path, and the route intersection. A route intersection includes, for example, at least one of a point where routes intersect and a point where routes converge. Note that in this specification, for example, "at least one of A, B, C, ..., and Z" means any one of all combinations of one or more values extracted from the group A, B, C, ..., and Z. The sign of the relative position of a 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 path intersection after passing through the path intersection is negative, and the magnitude of the relative position is the distance between the path 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 path intersection corresponds to maintaining the order of the path intersection and each moving body 201 when the moving bodies 201 are arranged on a number line with the path intersection as the origin. As will be described in detail later, the target position trajectory setting unit 12 sets the target position trajectory by solving a constrained optimization problem and using known model predictive control.
[0020] The trajectory tracking control unit 104 in FIG. 1 determines control variables for driving each moving body 201 based on the path information acquired by the path 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 proportional-integral-differential controller (PID) control, a known model predictive control, or a known data-driven control to determine the control variables. The content of the control variables may vary depending on the type of moving body 201. For example, if 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 variables. If the moving body 201 is a legged 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 variables.
[0021] 1 transmits the control amount determined by the trajectory tracking control unit 104 to each moving object 201. The control amount is transmitted using wireless communication using, for example, a Wi-Fi (registered trademark) router and a 5G router.
[0022] The kth 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 physical object such as an automated guided vehicle, an automobile, or a legged robot, or may be a virtual space model without a physical body. The first moving body 2011 to the Nth moving body 201N may be of different types. For example, some of the first moving body 2011 to the Nth moving body 201N may be automated guided vehicles, and the rest may be legged robots. In this first embodiment, for simplicity of explanation, an example will be described in which the first moving body 2011 to the Nth moving body 201N are of the same type.
[0023] 1 receives the control amount transmitted from the control device 101. To receive the control amount, for example, wireless communication using a Wi-Fi (registered trademark) antenna and a 5G router is used.
[0024] 1 controls 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, for example, at least one of an EPS (Electric Power Steering - Electric Control Unit) motor of an automobile, a powertrain 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 joints of a leg robot.
[0025] 2 and 3 are diagrams for explaining the relationship between the position of the moving body 201 and the target traveling path 301 acquired by the acquisition unit 11. In Fig. 2, a solid line represents the target traveling path 301, and a pentagon represents the moving body 201. The position p of the moving body 201 is expressed 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] ) to distinguish them. The yaw angle Ψ of the moving body 201 is the angle of the moving body relative 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 it from the coordinates of the moving body 201, in the following explanation, when QQ is a set of numbers of the target travel path and q∈QQ, each coordinate of the q-th target travel path is expressed as ( [q] x path , [q] y path Due to limitations 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 travel path 301. The longitudinal position represents the position of the moving body 201 on the target travel path 301, with the starting point of the target travel 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 point p' on the target travel 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 Let us assume that the relationship is as follows: (s) Function ξ q may be a function obtained by polynomial approximation from the point cloud of the target travel path 301, a function obtained by linear interpolation processing, or a spline function. The lateral position deviation is defined as the distance between the position p and the point p'. Although the lateral position deviation is not taken into account in the following description, it may be taken into account. Furthermore, 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] The s shown in Figure 3 [i] conf,j is the position of the route intersection 302 where the target travel route 301 of the moving body i and the target travel route 301 of the moving body j intersect, with the origin 301a of the target travel route 301 of the moving body i as the reference. [i] conf,j is the longitudinal position s of the moving object i on the target travel path 301, which indicates the position of the path 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 travel 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 travel route 301 just before the route intersection 302, or may be set at any point on the target travel 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 trajectories have not been set, and determines whether the target position trajectories have been set for all moving bodies. If it is determined that the target position trajectories have been set for all moving bodies, the procedure in Fig. 4 ends. If it is determined that the target position trajectories have 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 ranking 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 moving bodies 201 are traveling on only one route of the target traveling route 301 that has a route intersection 302, 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 applies not only to the first embodiment but also to the second and subsequent embodiments.
[0033] (Rule 1) When two moving bodies 201 are traveling on one route having a route intersection 302 among the target traveling routes 301, the moving body whose position in the traveling direction is ahead is determined as the leading moving body.
[0034] (Rule 2) When two moving bodies 201 are traveling along two routes that share a route intersection 302 among the target traveling routes 301, the moving body with the smaller relative position distance to the route intersection 302 is determined to be the preceding moving body.
[0035] (Rule 3) When three or more moving bodies 201 exist, determine the unique order of the preceding moving body 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, determine the unique order of the preceding moving body based on, for example, the 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, starting with the preceding moving body that is ahead in position, and sets the target position trajectory. In this case, the target position trajectory of the leading moving body 201 is set while ignoring the following moving body 201. Here, suppose that of two moving bodies 201 traveling on the same route, the rear moving body 201 is the leading moving body. In this case, the leading moving body 201 travels so as to move away from the rear moving body 201. However, if an obstacle such as another moving body exists further ahead of the leading moving body 201, a safe target position trajectory cannot be set for the leading moving body 201. In order to prevent such problems, rule 1 is defined in the first embodiment.
[0037] Rule 2 is also a rule for allowing moving bodies 201 to safely pass or merge with each other, and like rule 1, is a rule for setting a safe target position trajectory. For example, assume that moving body i is located exactly at route intersection 302, and moving body j is heading toward 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 defined in 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 while ignoring moving body i, a safe target position trajectory cannot be set for moving body i, as in the state described in rule 1. In order to prevent such problems, rule 2 is defined in embodiment 1.
[0038] Rule 3 is a rule used when there are three or more moving bodies. First, for safety reasons, the order of the preceding moving body is determined so as to satisfy the relationships obtained by rules 1 and 2. If the precedence-successor relationships are 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, as the preceding moving body, a moving body whose attribute information indicates an emergency vehicle such as an ambulance, as long as the relationships obtained by rules 1 and 2 are 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 in accordance with rules 1 and 2. Alternatively, in the second or subsequent process 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 traveling path, the target position trajectory setting unit 12 may determine the preceding moving body in accordance with rules 1 and 2 only for other moving bodies related to the new moving body, and then re-determine the order of the preceding moving body in accordance with 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 quantities and the target travel path of the moving body 201. When multiple moving bodies 201 exist and the target travel path has a path intersection, the target position trajectory setting unit 12 sets a target position trajectory based on the state quantities of the selected moving body 201 and the preceding moving body 201, the target travel path, and the path intersection. Here, the magnitude relationship between the relative positions of the selected moving body 201 and the preceding moving body 201 with respect to the path intersection is maintained in the target position trajectory. As a result, a trajectory that is ahead of one moving body 201 and does not collide with the target position trajectories of other moving bodies 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, which 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 trajectories of the other moving bodies from the result of the previous step. In this case, the target position trajectory setting unit 12 may set the target position trajectories of all moving bodies 201 in parallel.
[0042] Next, an example of the process of step S3 will be described, that is, an example in which the target position trajectory setting unit 12 solves the optimization problem and sets the target position trajectory of the moving body 201. The target position trajectory setting unit 12 solves the optimization problem expressed by the following equations (1) and (2) to set the target position trajectory at time t 0 Set the target position trajectory.
[0043]
[0044]
[0045] Equation (1) is a time interval t 0 From t 0 +T, the state quantity x such that the value of the evaluation function J, which is a real-valued function, is minimized. → (t), and input u → (t 0 ≦t≦t 0 Due to restrictions on the character notation that can be used in the specification, in the text of this specification, for example, x with "→" above it 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) vector, and u → (t) is the vertical acceleration a [i]By solving the optimization problem, the target position trajectory setting unit 12 can determine the optimal u →* (t) (t 0 ≦t≦t 0 +T) can be obtained, and u →* x when controlled with (t) as input →* (t) (t 0 ≦t≦t 0 +T). Then, the target position trajectory setting unit 12 calculates the calculated x →* The 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 equation (1) is defined, for example, as in the following equation (3).
[0048]
[0049] The first term on the right side of equation (3) is the time t 0 +T, the second term on the right side represents the cost of the entire prediction interval, and the third term on the right side corresponds to the barrier function of the constraint, which will be described later. → (t), t) is expressed as the following equation (4), and the function L(x → (t), u → (t), t) are expressed as in the following equation (5).
[0050]
[0051]
[0052] x in formula (4) and formula (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, in the text of this specification, for example, x with "→" above it and "-" above it will be referred to 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) of the constraint corresponding to the third term of the above equation (3) → The equations relating to (t), t) are expressed as the following equations (6) and (7).
[0054]
[0055]
[0056] Due to restrictions on the characters that can be used in the specification, in the text of this specification, the blank I in formulas (6) and (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 formulas (6) and (7) will be written as s^ and e^.
[0057] II is a set of mobile unit numbers of controlled mobile units, and ρ 0,1 , a 0 , b 0 is a positive constant, and γ(i) is a function that returns the route number of the mobile unit i. [j] is the 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) is the vertical position s of the moving object i at time t. [i] (t) is the set S [i] s,j and gathering JJ s It means that the set S is included in the intersection of [i] s,j is a distance from the vertical position of the moving object j as shown in FIG. 0 The vertical position s [i] (t) represents the set of JJ s is the time t 0 In this case, the vertical position of the moving object i is a 0It represents the set of all other moving bodies that are greater than or equal to ∑ i ...
[0059] Equation (7) expresses the vertical position s of the moving object i at time t. [i] (t) is the set S [i] x,j and gathering JJ x It means that the set S is included in the intersection of [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) than b 0 The vertical position s [i] (t) represents the set of JJ x is the time t 0 In this case, the distance of the relative position of the target moving body i to the route intersection 302 is b 0 represents a set of all other moving bodies traveling on other routes that are smaller than or equal to the route traveled by moving body i and share the route intersection 302. In other words, equation (7) requires a solution that "maintains the magnitude relationship between the relative position of the target moving body i and the relative positions of other moving bodies that exist on other routes that share the route intersection 302 with the route on which moving body i exists."
[0060] The barrier function h(x) of the constraint corresponding to the third term of the above equation (3) → (t), t) are expressed by the following equation (8).
[0061]
[0062] a in formula (8) 0 The first term including corresponds to equation (6), and b 0 The second term including corresponds to equation (7). Minimizing the time integral of equation (8) or the value of the third term of 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) are the time interval t0 From t 0 +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]
[0065] x in equation (9) → The symbols similar to ≦ written on the left and right of ≦ mean that the inequality symbol ≦ holds for 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 constraints 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]
[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 equation (11) below.
[0069]
[0070] However, a 1 , b 1 It is a positive constant. 1 is a constraint on the distance between vehicles to prevent collisions, so it is desirable that the value be large enough to prevent the moving bodies 201 from coming into contact with each other. 0 It may be the same value as b 1 is a constraint on the distance between vehicles to prevent collisions, so it is desirable that this be a large value so that the moving bodies 201 do not come into contact with each other. 0 It may be the same value as
[0071] C(t) in equation (11) is expressed as the product of two sets related to state quantities. The first part of C(t) (before the product symbol) is the vertical position of a certain moving object 201 and JJ s The distance between the vertical position of all other moving bodies 201 included in 1 For example, a 1 After setting the value of x to a sufficiently large value, → If (t) is included in this set, then mobile i is JJ at time t. s This means that there is no collision with the moving object 201.
[0072] The second part of C(t) (after the intersection symbol) is the relative position of a certain mobile unit 201 with respect to the route intersection 302, and x The distance between the relative positions of all other moving bodies 201 included in the route intersection 302 and the 1 For example, b 1 After setting the value of x to a sufficiently large value, → If (t) is included in this set, then mobile i is JJ at time t. x This means that there is no collision with the moving object 201.
[0073] As described above, the target position trajectory setting unit 12 can set a target position trajectory that maintains the magnitude relationship of the relative positions between one mobile body 201 and another mobile body 201 traveling on another route that shares the route intersection 302 with the route traveled by the one mobile body 201. This allows the mobile body control device 1 to suppress deadlock and smoothly and safely guide the mobile body 201 through the route intersection 302.
[0074] 9 is a flowchart showing an example of the procedure of the mobile object control system (i.e., the control device 101 and the mobile object 201) according to 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 mobile object information acquisition unit 103 acquires mobile object information, and the acquisition unit 11 acquires the state quantity of the mobile object 201. The mobile object information includes, for example, the number, attributes, size, travel route, position, speed, acceleration, longitudinal position, longitudinal speed, longitudinal acceleration, steering angle, attitude, and angular velocity of the mobile object 201.
[0077] In step S13, the target position trajectory setting unit 12 of the mobile body control device 1 calculates the target position trajectory (ξ q (s * (t)), t 0 ≦t≦t 0 +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 target longitudinal acceleration a [i] 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 kth moving object 201k receives the control amount transmitted in step S15.
[0081] In step S22, the actuator control unit 203k calculates the steering angle δ of the received control amount. [k] The actuator control unit 203k controls the EPS motor of the k-th moving body 201k based on the target longitudinal acceleration a [k]The brake control unit and the power train unit of the kth moving body 201k are controlled based on the above. After step S22, the process returns to step S11. Note that if 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 First Embodiment Figure 10 is a diagram showing the travel of the mobile object 201 when the magnitude relationship of the relative positions is not maintained on the target position trajectory. Figure 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 mobile object on the XY plane, and the lower half of the diagram shows the relative position of the mobile object 201 with respect to the path intersection 302 using a number line. If the magnitude relationship of the relative positions is not maintained, mobile object i may accelerate just before the path intersection 302 and arrive at the path intersection 302 before mobile object j. In this case, if the target position trajectory of mobile object j is planned while ignoring mobile object i, there is a risk that mobile object j will collide with mobile object i.
[0083] 11 is a diagram showing the traveling of the mobile 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 mobile objects 201 pass the route intersection 302 in order, starting with the mobile object 201 closest to the route intersection 302. In other words, the mobile object i will not arrive at the route intersection 302 before the mobile 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 objects 201 can pass through the route intersection 302 in order starting from the mobile objects 201 closest to the route intersection 302, as shown in FIG. 11 , so that deadlock can be prevented and the mobile objects 201 can be guided smoothly and safely through the route intersection 302.
[0085] <Embodiment 2> In embodiment 1, 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 mobile body 201 with respect to the route intersection 302. In contrast to this, in embodiment 2, it is possible to set a target position trajectory that flexibly responds to requests such as traffic manners and rules by setting a target position trajectory in consideration of mobile body information, that is, the state quantities of each mobile body 201.
[0086] 12 is a block diagram showing an example of the configuration of a mobile object control system according to Embodiment 2. In the following, among the components according to 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 body control device 1 of Fig. 1 , except that a priority determination unit 13 is added. The priority determination unit 13 of Fig. 12 determines a priority p of each mobile body 201 based on the state quantity of each mobile body 201. For example, the priority determination unit 13 may determine the priority p according to a predetermined rule based on the state quantity of each mobile body 201, or may determine the priority p by searching a predetermined scenario based on the state quantity of each mobile body 201. Furthermore, 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 body 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 body 201.
[0088] <Method of Determining Priority> Hereinafter, an example will be described 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 assigns a higher priority to the mobile object 201 moving on the main road in accordance with traffic regulations and traffic etiquette.
[0090] <Setting of target position trajectory taking priority into consideration> The target position trajectory setting unit 12 sets the target position trajectory taking priority into consideration. s and the set JJ of Eq. (7) x Instead of this, the set JJ of the following equation (12) s and the set JJ of the following equation (13) x is used.
[0091]
[0092]
[0093] p [i] (t 0 ), p [j] (t 0 ) at time t 0 The set JJ in Equation (12) represents the priority of mobile units i and j. s is the time t 0 In the equation (13), the set JJ of all other moving objects that have a higher priority than the moving object i and travel on the same route as the moving object i. x is the time t 0 represents a set of all other moving objects that have a higher priority than the target moving object i and are traveling on other routes that share a route intersection 302 with the route traveled by the moving object i.
[0094] The set JJ of equation (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) x is 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 taking into consideration only the priority of other moving bodies 201 that are 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 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 the setting procedure of the target position trajectory setting unit 12 according to the present embodiment 2. The setting procedure in Fig. 13 is the same as the setting procedure in Fig. 4 except that step S2 is changed to step S2a, and therefore step S2a will be mainly described here.
[0096] In the first embodiment, in step S2 of 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 that is most advanced in position. In contrast, in the second embodiment, in step S2a of 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 coincide with a leading position.
[0097] <Procedure of Mobile Object Control System> Fig. 14 is a flowchart showing an example of the procedure of the mobile object control system according to Embodiment 2. The procedure of Fig. 14 is the same as the procedure of Fig. 9 with step S17 added between step S12 and step S13, and therefore 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] (i∈II). The priority determination unit 13 determines the priority p [i] may be determined.
[0099] In step S13, the target position trajectory setting unit 12 calculates the target position trajectory (ξ q (s * (t)), t 0 ≦t≦t 0 +T).
[0100] Summary of Second Embodiment According to the mobile body control device 1 according to the second embodiment as described above, a target position trajectory is set in consideration of priority. With this configuration, for example, when a plurality of mobile bodies 201 travel through a route intersection 302, it is possible to set a target position trajectory that takes into consideration safety, urgency, requests, and the like, and ultimately a target position trajectory that meets more complex 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, 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 that have a lower priority than the one moving body 201, but this is not limited to this. For example, the priority determination unit 13 may relax the constraints for collision avoidance as the priority becomes lower, or may set the target position trajectory for mobility less frequently. With this configuration, the constraints on 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 equation (13) set JJ x The target position trajectory may be set by appropriately switching between the set of
[0103] <Embodiment 3> A block diagram showing an example of the configuration of a mobile object control system according to Embodiment 3 is similar to the block diagram of Embodiment 2 shown in Fig. 12. Hereinafter, among the components according to Embodiment 3, components that are the same as or similar to the components described above will be assigned 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 and manners. In contrast, in the third embodiment, the priority rule is different from that in the second embodiment and is defined as follows.
[0105] <Method of determining priority> (Priority rule 1) When two moving bodies 201 are traveling on one route that has a route intersection 302 among the target traveling route 301, the priority of the moving body that is ahead in the direction of travel is determined to be higher.
[0106] (Priority Rule 2) When two moving bodies 201 are traveling on two routes of the target traveling route 301 that share a route intersection 302, the priority of the moving body with the smaller relative position distance to the route intersection 302 is determined to be higher.
[0107] (Priority Rule 3) When there are three or more moving bodies 201, a unique priority order is determined that satisfies all of the priority magnitude relationships determined by Rules 1 and 2. When a unique priority order cannot be determined due to a contradiction in the magnitude relationships, a unique priority order is determined, for example, based on the attribute information of each moving body 201.
[0108] Priority rule 1 is a rule for setting a safe target position trajectory. The target position trajectory setting unit 12 sets target position trajectories for moving bodies in descending order of priority, and sets a target position trajectory for one moving body 201 while ignoring other moving bodies 201 that have a lower priority than the one moving body 201. Here, assume that, of two moving bodies 201 traveling on the same route, the rear moving body 201 is determined to have a higher priority. In this case, 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 prevent such problems, priority rule 1 is defined in this third embodiment.
[0109] Priority rule 2 is also a rule for allowing moving bodies 201 to safely pass or merge with one another, and, like priority rule 1, is a rule for setting a safe target position trajectory. For example, assume that moving body i is located exactly at route intersection 302, and moving body j is heading toward 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 defined by priority rule 1, i.e., the relationship between two moving bodies 201 traveling on the same route. Therefore, if the target position trajectory of moving body j is set while ignoring moving body i, a safe target position trajectory cannot be set for moving body i, as in the situation described with priority rule 1. In order to prevent such problems, rule 2 is defined in this third embodiment.
[0110] Here, an example will be described in which a unique order of priority is determined from the magnitude relationships of multiple priorities obtained by priority rules 1 and 2. For example, it is assumed that the relationships obtained from priority rules 1 and 2 are that the priority of the first moving body is less than the priority of the second moving body, the priority of the second moving body is less than the priority of the third moving body, and the priority of the first moving body is less than the priority of the 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: [1] , p [2] , p [3] are all real numbers greater than or equal to 0. This equation is equivalent to the following equation (14):
[0111]
[0112] Therefore, satisfying a unique priority ordering is [1] , p [2] , p [3] means that all the constraints of the formula (14) are satisfied. Therefore, the priority determination unit 13 may solve the optimization problems expressed by the following formulas (15) and (16) to determine a unique order of priority. Note that the evaluation function includes min(d 1 +d 2 +d 3 ) may be used.
[0113]
[0114]
[0115] Priority rule 3 is a rule for preventing deadlock when the priorities set by priority rules 1 and 2 contradict each other when combined, making it impossible to determine a unique priority order. For example, suppose that, among the first moving object 2011 to the third moving object 2013, a plurality of priority magnitude relationships are obtained, such as the priority of the first moving object < the priority of the second moving object, the priority of the second moving object < the priority of the third moving object, and the priority of the third moving object < the priority of the first moving object. Since the first moving object cannot pass the route intersection 302 after the second moving object, the second moving object cannot pass after the third moving object, and the third moving object cannot pass after the first moving object, the target position trajectory setting unit 12 cannot set a target position trajectory based on the plurality of priority magnitude relationships. Therefore, the priority determination unit 13 may determine a unique priority order based on attribute information of each moving object 201, or may determine a unique priority order by randomly adjusting the priority of each moving object 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 relationship obtained by priority rules 1 and 2 is 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] (Priority Rule 4) 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 traveling on two routes that share a route intersection 302 among the target traveling routes 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 with 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 the stopping position trajectory.
[0122] (Priority exchange condition 4) The distance between one of the stop positions with 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 the 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, such as when a higher priority is determined for a moving body behind, and the speed of the moving body ahead needs to be increased to match the moving body behind.
[0124] The reason why the condition for priority exchange 2 is that the priorities must be consecutive is that the exchange of non-consecutive priorities can be realized by combining the exchange of consecutive priorities. For example, p 1 >p 2 >p 3 In the priority 1 and p 3 When exchanging with p 1 and p 3 and then p 1 and p 3 Just replace it with.
[0125] Priority exchange conditions 3 and 4 are conditions for ensuring the prevention of accidents due to priority exchange. When the priority of a moving body 201 is lowered due to priority exchange, the constraints for determining the target position trajectory of that moving body 201 become stricter. For example, the moving body 201 with a lower priority is constrained to wait a sufficient time for the moving body 201 with a higher priority to pass through the route intersection 302 after decelerating and stopping. Therefore, if the priority of a moving body 201 is inappropriately lowered due to priority exchange, the moving body 201 may be rear-ended by a moving body 201 behind it due to a sudden stop, or may be rear-ended by another moving body 201 entering from the side at the route intersection 302. Therefore, priority exchange condition 3 is specified to ensure that there is sufficient space between the moving body 201 with a lower priority and the moving body 201 behind it so that the moving body 201 can react even if the moving body 201 with a lower priority safely decelerates. Priority exchange condition 4 is defined so as not to obstruct the passage of the mobile unit 201 with a higher priority 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 process of step S2a, the priority determination unit 13 may use the priority ranking determined in the initial process of step S2a. For example, in a configuration in which the ranking 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 other moving objects related to the new moving object according to priority rules 1 and 2, and then re-determine the priority ranking according to priority rule 3.
[0127] Summary of Third Embodiment According to the mobile body control device 1 according to the third embodiment as described above, the priority determination unit 13 assigns a higher priority to each mobile body 201 as the distance of the relative position of the mobile body 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. With this configuration, when the moving bodies 201 pass each other or merge at the route intersection 302, the target position trajectory for guiding the moving body 201 can be modified in accordance with various requests under certain conditions, so that the moving body 201 can be safely and flexibly guided through the route intersection 302.
[0129] 15 is a block diagram showing an example of the configuration of a mobile object control system according to Embodiment 4. Among the components according to Embodiment 4, 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 below.
[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. In other words, 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] <Procedure of Mobile Object Control System> Fig. 16 is a flowchart showing an example of the procedure of the mobile object control system according to Embodiment 4. The procedure of Fig. 16 is the same as the setting procedure of Fig. 9 except that step S14 is changed to step S14a, and therefore 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 Embodiment 4> According to the mobile body control device 1 of Embodiment 4 as described above, similarly to Embodiment 1, the control amount for driving each mobile body 201 is determined based on the target position trajectory in which the magnitude relationship of the relative positions of the mobile bodies 201 with respect to the path intersection 302 is maintained. Therefore, similarly to Embodiment 1, deadlock can be suppressed, and the mobile body 201 can be guided smoothly and safely through the path intersection 302.
[0135] 17 is a block diagram showing an example of the configuration of a mobile object control system according to Embodiment 5. Among the components according to Embodiment 5, 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 below.
[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 body control device 1 to each mobile body 201, and each mobile body 201 determines the control amount for each mobile body 201 based on the target position trajectory. The configurations of the control device 101 and the mobile body 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. The target position trajectory is transmitted using wireless communication using, for example, a Wi-Fi (registered trademark) router and a 5G router.
[0139] The kth moving body 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 body 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 kth 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 the same as 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 uses the control amount determined by the trajectory tracking control unit 206k as a command value and controls the driving of the actuator of the k-th moving body 201k based on the control amount.
[0144] <Procedure of Mobile Object Control System> Fig. 18 is a flowchart showing an example of the procedure of the mobile object control system according to Embodiment 5. In steps S11 to S13 in Fig. 18, the same processes as in steps S11 to S13 in Fig. 9 are performed.
[0145] In step S15a after step S13, the transmitting unit 105 of the control device 101 receives the target position trajectory (ξ q (s * (t)), t 0 ≦t≦t 0 +T) to the k-th mobile station 201k.
[0146] In step S21a, the receiving unit 202k of the kth moving body 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 mobile object information acquisition unit 205k acquires mobile object information, including, for example, the number, attributes, size, travel route, position, speed, acceleration, longitudinal position, longitudinal speed, longitudinal acceleration, steering angle, attitude, and angular velocity of the kth mobile object 201k.
[0149] In step S21d, the trajectory tracking control unit 206k determines a control amount for causing the k-th moving body 201k to 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 target longitudinal acceleration a [i] is.
[0150] In step S22, the actuator control unit 203k adjusts the steering angle δ of the determined control amount. [k] The actuator control unit 203k controls the EPS motor of the k-th moving body 201k based on the determined target longitudinal 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 Embodiment 5 According to the mobile body control device 1 according to the present embodiment 5 as described above, similarly to Embodiment 1, a control amount for driving each mobile body 201 is determined based on a target position trajectory that maintains the magnitude relationship of the relative positions of the mobile bodies 201 with respect to the path intersection 302. Therefore, similarly to Embodiment 1, deadlock can be suppressed, and the mobile body 201 can be guided smoothly and safely through the path intersection 302.
[0152] Furthermore, 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 previously performed by the control device 101, and therefore 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 cycle can be set to be short, and it is expected that safety will be improved.
[0153] 19 is a block diagram showing an example of the configuration of a controlled mobile body according to Embodiment 6. Hereinafter, among the components according to Embodiment 6, components that are the same as or similar to the components described above will be assigned the same or similar reference numerals, and different components will be mainly described.
[0154] In the fifth embodiment, the control device 101 includes the mobile object control device 1 and transmits the target position trajectory of each mobile object 201 to each mobile object 201 .
[0155] In contrast to this, in the sixth embodiment, a controlled mobile object 211 included in the mobile object 201 has the 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 regarding a target travel route that is set in advance as a travel destination of the mobile body 201. The route information acquired by the route information acquisition unit 212 is the same as 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 mobile body 201 using V2V (Vehicle-to-Vehicle) communication. Using 5G communication for V2V communication can improve the control performance of each mobile body.
[0158] The mobile object information acquisition unit 213 acquires mobile object information relating to the 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 the same as 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 other mobile objects 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 of Figure 19 is equipped with an acquisition unit 11 and a target position trajectory setting unit 12 similar to the acquisition unit 11 and 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] <Procedure of Controlling Mobile Body> FIG. 20 is a flowchart showing an example of a procedure of the controlling mobile body 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 driving 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 the 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)), t 0 ≦t≦t 0 +T).
[0165] In step S34, the trajectory tracking control unit 214 determines the control amount for causing the controlled mobile body 211 to follow the target position trajectory based on the route information in step S31, the mobile body 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 moving object 211 based on the determined control amount.
[0167] <Summary of Sixth Embodiment> According to the mobile body control device 1 of the sixth embodiment as described above, the control amount for driving the controlled mobile body 211 is determined based on the target position trajectory that maintains the magnitude relationship of the relative position of the mobile body 201 with respect to the path intersection 302, as in the first embodiment. Therefore, as in the first embodiment, deadlock can be suppressed, and the controlled mobile body 211 can pass through the path intersection 302 smoothly and safely.
[0168] 21 is a block diagram showing an example of the configuration of a controlled mobile body according to Embodiment 7. Hereinafter, among the components according to Embodiment 7, components that are the same as or similar to the components described above will be assigned 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 uses the control amount determined by the mobile object control device 1 as a command value and performs control to drive the actuator of the controlled mobile object 211 based on the control amount.
[0170] <Procedure of Controlling Mobile Body> Fig. 22 is a flowchart showing an example of the procedure of the controlling mobile body 211 according to the present embodiment 7. The procedure of Fig. 22 is the same as the setting procedure of Fig. 20 except that step S34 is changed to step S34a, and therefore 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 the control amount for causing the controlled mobile body 211 to follow 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 Embodiment 7> According to the mobile body control device 1 of Embodiment 7 as described above, the control amount for driving the controlled mobile body 211 is determined based on the target position trajectory that maintains the magnitude relationship of the relative position of the mobile body 201 with respect to the path intersection 302, as in Embodiment 1. Therefore, as in Embodiment 1, deadlock can be suppressed, and the controlled mobile body 211 can pass through the path 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 hereinafter be 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: an acquisition unit 11 that acquires state quantities of one or more moving bodies 201 and a target travel route that is set in advance as a travel target for the moving bodies 201; and a target position trajectory setting unit 12 that, when the one or more moving bodies 201 are multiple moving bodies 201 and the target travel route 301 has a route intersection 302, indicates the target position of each moving body 201 at each time along the target travel route 301 based on the state quantities of the multiple moving bodies 201, the target travel route 301, and the route intersection 302, and sets a target position trajectory that maintains the magnitude relationship between the relative positions of each moving body 201 and the route intersection 302. The processing circuit 81 may be implemented by dedicated hardware or by a processor that executes a program stored in a memory. Examples of processors include central processing units, processing units, arithmetic units, microprocessors, microcomputers, and DSPs (Digital Signal Processors).
[0174] When the processing circuitry 81 is dedicated hardware, the processing circuitry 81 may be, 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 thereof. The functions of each unit, such as the acquisition unit 11, may be realized by a circuit in which processing circuits are distributed, or the functions of each unit may be realized together by a single processing circuit.
[0175] When the processing circuitry 81 is a processor, the functions of the acquisition unit 11 and the like are realized in combination with software and the like. Note that software and the like may include, for example, software, firmware, or software and firmware. The software and the like are written as programs and stored in memory. As shown in FIG. 24 , a processor 82 applied to the processing circuitry 81 realizes the functions of each unit by reading and executing programs 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 following steps: acquiring state quantities of one or more mobile bodies 201 and a target travel route that is set in advance as a target for travel of 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 position 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 that maintains the magnitude relationship of the relative positions of each mobile body 201 with respect to the route intersection 302. 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 RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable Read Only Memory), or an EEPROM (Electrically Erasable Programmable Read Only Memory), a 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 and the like is realized either by hardware or software, etc. However, this is not limited to this, and a configuration in which part of the acquisition unit 11 and the like 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 the above-described 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.
[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. A mobile body control device comprising: an acquisition unit that acquires state quantities of one or more moving bodies and a target driving route that is set in advance as a target for the driving of the moving bodies; and a target position trajectory setting unit that, when the one or more moving bodies are multiple moving bodies and the target driving route has a route intersection, indicates a target position of each of the moving bodies at each time along the target driving route based on the state quantities of the multiple moving bodies, the target driving route, and the route intersection, and sets a target position trajectory that maintains the magnitude relationship of the relative positions of each of the moving bodies with respect to the route intersection.
2. A mobile object control device according to claim 1, wherein the state quantity of the mobile object includes a position of the mobile object.
3. A mobile body control device as described in claim 1 or claim 2, further comprising a priority determination unit that determines a priority of each of the mobile bodies based on the state quantity of each of the mobile bodies, and the target position trajectory setting unit sets the target position trajectory taking into account the priority.
4. A mobile body control device as described in claim 3, wherein the target position trajectory setting unit sets the target position trajectory of one of the mobile bodies while ignoring other mobile bodies that have a lower priority than the one of the mobile bodies.
5. A mobile body control device according to claim 3 or 4, wherein the priority determination unit increases the priority of the mobile body as the distance of the relative position of the mobile body to the route intersection is smaller.
6. A mobile body control device as described in any one of claims 3 to 5, wherein the priority determination unit is capable of exchanging the priority of the first moving body and the priority of the second moving body when a first moving body and a second moving body included in the plurality of moving bodies satisfy a predetermined priority exchange condition.
7. A mobile body control device as described in claim 6, wherein the priority exchange conditions include: the first moving body and the second moving body are each traveling on two routes that share the route intersection among the target driving routes; the priority of the first moving body and the priority of the second moving body are continuous; when one of the first moving body and the second moving body that has 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 stopping position trajectory; and the distance on the stopping position trajectory between the stopping position of the one of the higher priority bodies and the route intersection is equal to or greater than a predetermined distance.
8. A mobile body control device according to any one of claims 1 to 7, further comprising a trajectory tracking control unit that determines a control amount for driving the mobile body based on the target position trajectory.
9. A mobile body control system comprising: a control device equipped with a mobile body control device described in any one of claims 1 to 7; and one or more mobile bodies capable of communicating with the control device, wherein the control device further comprises a route information acquisition unit that acquires route information regarding the target driving route, a mobile body information acquisition unit that acquires mobile body information regarding the state quantities of the one or more mobile bodies, a trajectory tracking control unit that determines a control amount for driving the mobile body based on the target position trajectory set by the mobile body control device, and a transmission unit that transmits the control amount to the mobile body, and wherein the mobile body comprises a receiving unit that receives the control amount from the control device, and an actuator control unit that controls an actuator based on the control amount.
10. A mobile body control system comprising: a control device equipped with the mobile body control device described in claim 8; and one or more mobile bodies capable of communicating with the control device, wherein the control device further comprises a route information acquisition unit that acquires route information regarding the target driving route, a mobile body information acquisition unit that acquires mobile body information regarding the state quantities of the one or more mobile bodies, and a transmission unit that transmits the control amount determined by the mobile body control device, and the mobile body comprises a receiving unit that receives the control amount from the control device, and an actuator control unit that controls an actuator based on the control amount.
11. A mobile body control system comprising: a control device equipped with a mobile body control device described in any one of claims 1 to 7; and one or more mobile bodies capable of communicating with the control device, wherein the control device further comprises a route information acquisition unit that acquires route information regarding the target driving route, a mobile body information acquisition unit that acquires mobile body information regarding the state quantities of the one or more mobile bodies, and a transmission unit that transmits the target position trajectory set by the mobile body control device to the mobile body, and the mobile body comprises a receiving unit that receives the target position trajectory from the control device, a trajectory tracking control unit that determines a control amount for driving the mobile body based on the target position trajectory, and an actuator control unit that controls an actuator based on the control amount.
12. A controlled mobile body as described in any one of claims 1 to 7, further comprising: a mobile body control device that sets the target position trajectory for the controlled mobile body; a route information acquisition unit that acquires route information regarding the target driving route; a mobile body information acquisition unit that acquires mobile body information regarding the state quantities of the one or more mobile bodies including the controlled mobile body; a trajectory tracking control unit that determines a control amount for driving the controlled mobile body based on the target position trajectory set by the mobile body control device; and an actuator control unit that controls an actuator based on the control amount.
13. A controlled mobile body as described in claim 8, further comprising: a mobile body control device that sets the target position trajectory for the controlled mobile body and determines the control amount for the controlled mobile body; a route information acquisition unit that acquires route information regarding the target driving route; a mobile body information acquisition unit that acquires mobile body information regarding the state amount of the one or more mobile bodies including the controlled mobile body; and an actuator control unit that controls an actuator based on the control amount determined by the mobile body control device.
14. A method for controlling a moving body, comprising: acquiring state quantities of one or more moving bodies and a target driving route that is set in advance as a target for the driving of the moving bodies; and, when the one or more moving bodies are multiple moving bodies and the target driving route has a route intersection, indicating a target position of each of the moving bodies at each time along the target driving route based on the state quantities of the multiple moving bodies, the target driving route, and the route intersection, and setting a target position trajectory that maintains the magnitude relationship of the relative positions of each of the moving bodies with respect to the route intersection.
Citation Information
Patent Citations
Vehicle control system
JP2006338596A
Vehicle travel control device and vehicle travel control method
JP2013061788A
Vehicle control system, server device, vehicle control method, and vehicle control program
WO2018179277A1
