Route tracking controller and route tracking control method

Abstract

To make it easy to adjust a control gain in a route tracking controller that performs control to make a vehicle track a target route.SOLUTION: A route tracking controller (3) includes an arithmetic unit (33) that repeatedly executes at predetermined intervals cascade control (C) which includes major loop processing (S2) including first acquisition processing (S21) of acquiring a position actual result value (y) and a vehicle position target value (yr), and first calculation processing (S22) of calculating an advancing direction target value (θr) of a vehicle (1) on the basis of a vehicle position deviation and a position control gain (Ky), and minor loop processing (S3) including second acquisition processing (S31) of acquiring an advancing direction target value and an advancing direction actual result value (θ), second calculation processing (S32) of calculating a curvature target value (κr) of a travel route of the vehicle on the basis of the advancing direction deviation and a direction control gain (Kθ), and output processing (S33) of outputting information based on the curvature target value.SELECTED DRAWING: Figure 4

Description

[Technical field]

[0001] The present invention relates to a path tracking control device and a path tracking control method. [Background technology]

[0002] Conventionally, various path tracking control technologies have been proposed for automatically driving a vehicle along a target route. For example, Patent Document 1 describes a driving control method and device that, for an arbitrary target route with a variable curvature, provides a reference point on the target route so that the vehicle position is in the normal direction of the target route, converts the coordinate system from the absolute coordinate system of the vehicle position to a relative coordinate system based on the reference point, calculates or measures the relative lateral and azimuth deviations from the target route, and determines the steering angle based on a feedback operation amount according to the deviation amount and a feedforward operation amount according to the curvature and lateral deviation amount of the target route.

[0003] Patent document 2 also describes an autonomous field work vehicle that includes a position deviation calculation unit that calculates a vehicle position deviation, which is the lateral positional deviation of the vehicle relative to a target route, an orientation deviation calculation unit that calculates an orientation deviation between the target route, a first control calculation unit that outputs a first steering value using the position deviation and the target position deviation as input parameters, a second control calculation unit that outputs a second steering value using the orientation deviation, the target orientation deviation, and the position deviation as input parameters, and a steering angle for traveling along the target route based on the first steering value and the second steering value. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] JP 2002-215239 A [Patent Document 2] JP 2019-038535 A Summary of the Invention [Problem to be solved by the invention]

[0005] When the accuracy of tracking the target route is not sufficient as a result of automatically driving the vehicle, it becomes necessary to manually adjust the control gains used in calculating the steering angle in each control device. However, in all of the conventional technologies, a control device that controls the vehicle position deviation, which is the lateral deviation of the vehicle from the target route, and a control device that controls the traveling direction deviation, which is the difference between the traveling direction and the orientation in which the vehicle faces, are configured to operate in parallel. In addition, in the conventional technologies, the steering angles calculated by each control device are integrated to determine the final steering angle. For this reason, in the conventional technologies, the result of adjusting the control gain of one control device may affect the adjustment of the control gain of the other control device. In this way, when one control gain has some relationship with the performance of the other control device, it is necessary to adjust each control gain simultaneously. Such adjustment takes time and effort, and the adjustment may become insufficient due to the time required for adjustment.

[0006] An object of one aspect of the present invention is to facilitate adjustment of control gains in a path tracking control device that controls a vehicle to follow a target path. [Means for solving the problem]

[0007] A path following control device according to one embodiment of the present invention is a path following control device connected to a vehicle to be controlled, and includes a calculation unit that repeatedly executes cascade control at a predetermined period, the cascade control including a major loop process including a first acquisition process for acquiring a position actual value indicating the actual position of the vehicle and a vehicle position target value which is a target value of the position of the vehicle, and a first calculation process for calculating a traveling direction target value which is a target value of the traveling direction of the vehicle based on a vehicle position deviation which is a deviation between the vehicle position target value and the position actual value and a position control gain, the major loop process including a second acquisition process for acquiring from the vehicle the traveling direction target value calculated in the first calculation process and a traveling direction actual value which is an actual traveling direction of the vehicle, a second calculation process for calculating a curvature target value which is a target value of the curvature of the traveling path of the vehicle based on a traveling direction deviation which is a deviation between the traveling direction target value and the traveling direction actual value and a direction control gain, and an output process for outputting information based on the curvature target value.

[0008] A path following control method according to another aspect of the present invention is a path following control method for causing a controlled vehicle to follow a target path, the method including: a first acquisition step of acquiring a position actual value indicating the actual position of the vehicle and a vehicle position target value which is a target value of the position of the vehicle; a first calculation step of calculating a traveling direction target value which is a target value of the traveling direction of the vehicle based on a vehicle position deviation which is a deviation between the vehicle position target value and the position actual value and a position control gain; a second acquisition step of acquiring from the vehicle the traveling direction target value calculated in the first calculation step and a traveling direction actual value which indicates the actual traveling direction of the vehicle; a second calculation step of calculating a curvature target value which is a target value of the curvature of the traveling path of the vehicle based on a traveling direction deviation which is a deviation between the traveling direction target value and the traveling direction actual value and a direction control gain; and an output step of outputting information based on the curvature target value, at a predetermined period. Effect of the Invention

[0009] According to one aspect of the present invention, in a path tracking control device that controls a vehicle to follow a target path, adjustment of a control gain can be easily performed. [Brief description of the drawings]

[0010] [Figure 1] 1 is a block diagram showing an example of a mechanical configuration of a path following control device according to an embodiment of the present invention; [Diagram 2] 2 is a schematic diagram showing a relationship between a vehicle, a target route, and each value that is a control target of the path following control device according to the embodiment; FIG. [Diagram 3] 2 is a block diagram showing an example of a steering operation model of a vehicle to be controlled by a path following control device according to the embodiment; FIG. [Figure 4] 2 is a block diagram showing an example of a functional configuration of the path following control device according to the embodiment; FIG. [Diagram 5] 4 is a graph showing a trajectory and a target route of a vehicle that is automatically traveled under the control of the path following control device according to the embodiment; [Figure 6] 1 is a block diagram showing an example of a functional configuration of a path following control device according to a first modification, and a steering operation model of a vehicle to be controlled. FIG. [Figure 7] 10 is a diagram for explaining the concept of the operation of a path following control device according to Modification 1. FIG. [Figure 8] 6 is a graph showing a trajectory and a target route of a vehicle traveling automatically under the control of a path following control device according to the first modification. [Figure 9] FIG. 11 is a block diagram showing an example of a functional configuration of a path following control device according to a second modification, and a steering operation model of a vehicle to be controlled. [Figure 10] 11 is a graph showing a trajectory and a target route of a vehicle traveling automatically under the control of a path following control device according to a second modification. [Figure 11] FIG. 11 is a block diagram showing an example of a functional configuration of a path following control device according to a third modification, and a steering operation model of a vehicle to be controlled. [Figure 12] 10 is a flowchart showing an example of the flow of a path tracking control method according to an embodiment of another aspect of the present invention. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0011] <Autonomous driving system> Hereinafter, an autonomous driving system 100 (hereinafter, system 100) according to an embodiment of the present invention will be described in detail.

[0012] As shown in FIG. 1, the system 100 includes a vehicle 1, a speed control device 2, and a path following control device 3.

[0013] [vehicle] Vehicle 1 is capable of automatically traveling along a predetermined target route by being connected to speed control device 2 and route following control device 3. "Connected" includes being "mounted" (integrated) on vehicle 1 and being able to "communicate" with vehicle 1 via wire or wirelessly. Vehicle 1 automatically operates the accelerator and brake in accordance with the operation input from speed control device 2, and automatically changes its own speed. Vehicle 1 also automatically operates the direction of the wheels in accordance with the operation input from route following control device 3, and automatically changes its own traveling direction.

[0014] The vehicle 1 is also equipped with various sensors (not shown). The various sensors include a position sensor, a speed sensor, an orientation sensor, and the like. The position sensor repeatedly measures the position of the vehicle 1 at a predetermined period and outputs the measured value as an actual position value (x, y). The speed sensor repeatedly measures the speed of the vehicle 1 at a predetermined period and outputs the measured value as an actual speed value v. The orientation sensor repeatedly measures the direction of the vehicle 1 at a predetermined period and outputs the measured value as an actual traveling direction value θ. The various sensors can be configured using, for example, a GNSS (Global Navigation Satellite System) receiver, a LiDAR, a camera, and the like.

[0015] For simplicity, the following description will be given assuming that the vehicle 1 automatically travels while facing approximately in the direction of extension of the X-axis of a Cartesian coordinate system (the predetermined target route R extends approximately along the X-axis) as shown in Fig. 2. Note that the coordinate axes do not necessarily need to be determined in this manner, and when a different coordinate system is used, the present invention can be applied after rotating the coordinate axes so that the X-axis faces the traveling direction of the vehicle 1. Also, when the target route R curves significantly (for example, at a right angle to the X-axis) midway and the direction of the target route R deviates significantly from the direction of the X-axis, the coordinate axes can be rotated at that point so that the target route R extends approximately along the X-axis direction, and the present invention can be applied.

[0016] The equation of motion of the vehicle 1 is generally expressed as in the following formulas (1) to (3). In the formulas, x and y are the position of the vehicle 1, v is the speed of the vehicle 1, and θ is the traveling direction of the vehicle 1. The dots placed above each variable x, y, and θ represent the time derivatives (dx / dt, dy / dt, dθ / dt) of each variable. Furthermore, in the following formulas (1) to (3), L is the distance between the front and rear wheels in the case of a four-wheeled vehicle. Note that, in the case of a vehicle 1 whose front and rear wheels are steerable (for example, a carrier that transports materials in a steelworks), L is the distance from the center of the vehicle 1 to the front wheel.

number

number

number

[0017] The operation input for changing the traveling direction of the vehicle 1 is the steering angle φ. However, instead of the steering angle φ, the curvature κ defined by the following equation (4) can be used as the operation input from the path following control device 3.

number

[0018] Furthermore, if we consider that the traveling direction θ of the vehicle 1 is approximately along the extension direction of the X-axis (θ ≒ 0), the above formulas (1) to (3) can be simplified to the following formulas (5) to (7). Of the following formulas (5) to (7), the formulas related to the lateral position of the vehicle 1 are formulas (6) and (7). Here, the "lateral direction" refers to the extension direction of the Y-axis.

number

number

number

[0019] When the equations of motion of the vehicle 1 are simplified as in the above equations (5) to (7), the steering operation of the vehicle 1 (with respect to the curvature target value κ r When the response from the target curvature κ to the actual lateral position y is modeled, it becomes a double integral system with two integrators connected in series, as shown in the area surrounded by the dashed line in Figure 3. Here, r is the target value of the actual curvature value κ. For simplicity, the target curvature value κ r There is no dynamics from r )

[0020] [Speed ​​control device] The speed control device 2 controls the speed v of the vehicle 1. As shown in FIG. 1, the speed control device 2 is connected to the vehicle 1 to be controlled. The speed control device 2 includes an input / output unit, a storage unit, and a calculation unit, all of which are not shown. The calculation unit executes a speed control program stored in the storage unit. Specifically, the loop process S1 as shown in FIG. 4 is repeatedly executed at a predetermined cycle. The loop process S1 includes a speed acquisition process S11, a torque calculation process S12, a conversion process S13, and an output process S14. In the speed acquisition process S11, the calculation unit calculates a speed actual value v and a vehicle speed target value v r In the torque calculation process S12, the calculation unit obtains the function G v (s) to obtain the vehicle speed target value vr A driving torque τ is calculated so that the deviation between the actual speed value v and the acceleration / deceleration speed value v quickly becomes zero. In a conversion process S13, the calculation unit converts the driving torque τ calculated in the torque calculation process S12 into an accelerator operation amount a or a brake operation amount b using a function g(τ). In an output process S14, the calculation unit outputs the converted accelerator operation amount a or brake operation amount b. Various conventionally known techniques can be used for calculating the driving torque τ and converting it into the accelerator operation amount a or brake operation amount b.

[0021] [Configuration of the path tracking control device] The path tracking control device 3 controls the position and traveling direction θ of the vehicle 1. As shown in Fig. 1, the path tracking control device 3 is connected to the vehicle 1 to be controlled. The path tracking control device 3 includes an input / output unit 31, a storage unit 32, and a calculation unit 33.

[0022] [Input / output section] The input / output unit 31 is configured with a connector to which a cable extending from the vehicle 1 is connected, or a communication module that communicates with the vehicle 1 in a wired or wireless manner. The input / output unit 31 may be divided into an input unit and an output unit. In addition, when the path following control device 3 is configured integrally with the vehicle 1, the path following control device 3 does not need to include the input / output unit 31 (the calculation unit 33 may be directly connected to a calculation unit included in the vehicle).

[0023] [Storage section] The storage unit 32 stores a path tracking control program 321. The path tracking control program describes the operations to be executed by the calculation unit 33 of the path tracking control device 3. The storage unit 32 also stores a position control gain K y and directional control gain K θ Position control gain K y (Directional control gain K θ ) is a coefficient used by the calculator 33 in the calculation. y and directional control gain K θ The storage unit 32 stores the position control gain Ky and directional control gain K θ Therefore, a person who performs maintenance on the vehicle 1 can easily change the position control gain K y and directional control gain K θ The storage unit 32 according to this embodiment is configured with a semiconductor memory or a hard disk. Note that when a path tracking control program is not required (for example, when the calculation unit 33 performs various calculations described later using logic circuits), the path tracking control device 3 does not need to include the storage unit 32.

[0024] [Arithmetic unit] The calculation unit 33 repeatedly executes the cascade control C as shown in Fig. 4 at a predetermined cycle. The calculation unit 33 according to this embodiment is composed of a processor. This processor executes a path tracking control program stored in the storage unit 32 to execute the cascade control C. The cascade control C includes a major loop process S2 and a minor loop process S3. Furthermore, the calculation unit 33 according to this embodiment executes a target value calculation process S4 (third calculation process) before starting the major loop process S2.

[0025] (Target value calculation process) In the target value calculation process S4, the calculation unit 33 calculates the vehicle position target value y r When the target route R is expressed by the relational expression y=route(x), the calculation unit 33 calculates the vehicle position target value y by inputting the position of the vehicle 1 in the traveling direction (actual position value x) into the above relational expression. r The calculation unit 33 executes this target value calculation process S4, thereby functioning as a target value calculation unit 331 (third calculation unit, see FIG. 1).

[0026] (Major loop processing) The major loop process S2 includes a first acquisition process S21 and a first calculation process S22.

[0027] First acquisition process In the first acquisition process S21, the calculation unit 33 calculates the vehicle position target value y calculated in the target value calculation process S4. r The actual position value y is a value indicating the actual position of the vehicle 1. The target vehicle position value y r is a target value of the position of the vehicle 1 corresponding to the acquired actual position value y. In the first acquisition process S21 according to this embodiment, the calculation unit 33 acquires the actual position value y via the input / output unit 31 connected to the vehicle 1. Note that the calculation unit 33 may acquire the actual position value y indirectly from another device different from the vehicle 1 (for example, a measuring device connected to the vehicle 1) instead of directly from the vehicle 1. The calculation unit 33 functions as a first acquisition unit 332 (see FIG. 1) by executing this first acquisition process S21.

[0028] First calculation process In the first calculation process S22, the calculation unit 33 calculates the vehicle position deviation and the position control gain K y Based on this, the target value θ r The vehicle position deviation is calculated by multiplying the vehicle position target value y r and the deviation between the actual position value y (difference y r -y). The target value of the travel direction θ r is the target value for the traveling direction of the vehicle 1. Position control gain K y may be a scalar, or may be a value expressed as a function of a Laplace operator s similar to that used in PID control and change according to frequency. In the first calculation process S22 according to this embodiment, the calculation unit 33 calculates the vehicle position deviation and the position control gain K y The transfer function G for direction calculation is determined by y (s) and the product of this is the target value θ r It is calculated as follows: G y The s in (s) is the Laplace operator. By executing this first calculation process S22, it functions as the first calculation unit 333 (see FIG. 1).

[0029] (Minor loop processing) The minor loop process S3 includes a second acquisition process S31 and a second calculation process S32.

[0030] Second acquisition process In the second acquisition process S31, the calculation unit 33 calculates the travel direction target value θ r The actual travel direction value θ is calculated by multiplying the travel direction target value θ by the travel direction actual value θ. r In the second acquisition process S31 according to the present embodiment, the calculation unit 33 acquires the traveling direction actual value θ via the input / output unit 31 connected to the vehicle 1. The calculation unit 33 functions as a second acquisition unit 334 (see FIG. 1) by executing the second acquisition process S31.

[0031] Second calculation process In the second calculation process S32, the calculation unit 33 calculates the deviation in the traveling direction and the directional control gain K θ Based on this, the curvature target value κ r The deviation in the travel direction is calculated based on the travel direction target value θ r and the deviation between the actual travel direction value θ (difference θ r -θ). The curvature target value κ r is the target value of the curvature of the travel path of the vehicle 1. The directional control gain K θ is the position control gain K y In the second calculation process S32 according to the present embodiment, the calculation unit 33 calculates the deviation in the traveling direction and the directional control gain K θ The transfer function G for calculating the curvature is determined by θ (s) and the product of this is the target value θ r It is calculated as follows: G θ The s in (s) is the Laplace operator. The calculation unit 33 functions as a second calculation unit 335 (see FIG. 1) by executing this second calculation process S32.

[0032] Output processing In the output process S33, the calculation unit 33 outputs the curvature target value κr In the output process S33 according to the present embodiment, the calculation unit 33 outputs information based on the curvature target value κ r In the output process S33 according to the present embodiment, the calculation unit 33 outputs the curvature target value κ r The calculation unit 33 outputs information based on the curvature target value κ r The information based on the target curvature value κ may be indirectly output to a device other than the vehicle 1 (for example, a computing device connected to the vehicle 1) instead of being directly output to the vehicle 1. r The vehicle 1, to which the command is input, automatically controls the direction of the wheels and automatically changes its own traveling direction θ. The calculation unit 33 executes this output process S33, thereby functioning as an output processing unit 336 (see FIG. 1).

[0033] [Regarding control gain] When the vehicle 1 travels approximately along the extension direction of the X-axis, the moment-to-moment steering operation of the vehicle 1 is performed in accordance with the curvature target value κ r The curvature target value κ can be expressed as a model in which κ is multiplied twice by the velocity v. r is the transfer function G for calculating the deviation in the direction of travel and the curvature θ (s) and the target value θ r is the transfer function G for calculating the position deviation and direction y (s). Therefore, this model uses the direction calculation transfer function G y (s) and the transfer function G for curvature calculation θ (s) is multiplied by the speed v. This means that the steering operation of the vehicle 1 depends on the speed v. In other words, the greater the speed v of the vehicle 1, the greater the lateral movement of the vehicle 1. This can cause the running of the vehicle 1 to become unstable.

[0034] For this reason, the direction calculation transfer function G y (s) and the transfer function G for curvature calculation θ (s) may be set to be inversely proportional to the speed v of the vehicle 1, as shown in the following equations (8) and (9).

number

number

[0035] This direction calculation transfer function G y (s) and the transfer function G for curvature calculation θ When (s) is applied to the model shown in Fig. 3, the speed v appears to be eliminated from the calculated travel direction actual value θ and position actual value y. This means that the steering operation of the vehicle 1 does not depend on the speed v. In other words, even if the speed v of the vehicle 1 increases, the lateral movement of the vehicle 1 is suppressed, so the running of the vehicle 1 becomes stable.

[0036] In addition, the directional control gain K θ and position control gain K y If is a scalar, the target direction θ r The relationship between the actual travel direction value θ and the directional control gain K θ The vehicle position target value y r The relationship between the actual position value y and the directional control gain K θ and position control gain K y It is expressed as a second-order delay system transfer function using

number

number

[0037] The above equation (10) expresses the response of the vehicle in the traveling direction as the position control gain K y is independent of the directional control gain K θ is the position control gain K y Therefore, it is not necessary to adjust the directional control gain K θThen, the adjusted directional control gain K θ Using the above equation (11), the position control gain K is calculated so that the lateral position response of the vehicle 1 is desired. y can be adjusted.

[0038] The transfer function of the second-order delay system is expressed by the damping factor ζ and the natural angular frequency ω as shown in the following equation (12). n Therefore, the directional control gain K θ and position control gain K y It is not necessary to directly adjust the damping ratio ζ and the natural angular frequency ω n When the following formula (12) coincides with the above formula (11), the directional control gain K θ and position control gain K y By calculating each of these, the directional control gain K θ and position control gain K y In this case, the directional control gain K θ and position control gain K y is expressed by the following equations (13) and (14).

number

number

number

[0039] The operator of vehicle 1 determines the damping ratio ζ and the natural angular frequency ω n By looking at the specific value of r Therefore, by doing as described above, it is possible to easily predict the lateral position y of the vehicle 1 when the directional control gain K θ and position control gain K yIn general path tracking control, it is desirable that the damping factor ζ is within the range of 0.6 to 0.8. For this reason, the damping factor ζ is set to, for example, 0.7, and the natural angular frequency ω n It is also possible to adjust the coefficient by increasing it within a range where the movement of the vehicle 1 does not become oscillatory. In this way, the coefficient to be adjusted is the natural angular frequency ω n Therefore, the directional control gain K θ and position control gain K y This makes adjustment even easier.

[0040] [Effects of the path tracking control device] The calculation unit 33 of the path tracking control device 3 described above calculates the travel direction target value θ r and a major loop process S2 for calculating the curvature target value κ r Therefore, the change in the control gain of the major loop does not affect the response of the control system of the minor loop. As a result, the directional control gain K θ and position control gain K y When it becomes necessary to adjust the directional control gain K θ and position control gain K y First, the calculation unit 33 is set to an adjustment mode in which only the minor loop processing is performed (major loop processing is not performed), and the travel direction target value θ r A test signal such as a step signal is input as the vehicle 1. Then, a response waveform of the travel direction actual value θ output by the vehicle 1 is obtained. Then, a directional control gain K θ After that, the calculation unit 33 is returned to a mode in which the normal major loop process is also executed, and the vehicle position target value y r A test signal such as a step signal is input as the vehicle position control gain K y In this way, according to the path tracking control device 3, the manager adjusts the directional control gain K θ and position control gain K yIn other words, the administrator can adjust the directional control gain K θ and position control gain K y The adjustment can be easily performed.

[0041] In addition, the path tracking control device 3 according to this embodiment has a directional control gain K θ and position control gain K y The present invention can easily adjust the directional control gain K of the path tracking control device 3, and can perform the path tracking control of the vehicle 1 with at least the same accuracy as the conventional method. To demonstrate this, the inventors prepared a path tracking control device 3 according to this embodiment and performed the following experiment. First, the directional control gain K θ and position control gain K y with damping ratio ζ and natural angular frequency ω n Specifically, the damping ratio ζ was set to 0.7, and the natural angular frequency ω was adjusted in a range where the movement of the vehicle 1 does not become oscillatory. n Next, the directional control gain K θ and position control gain K y Next, the direction calculation transfer function G is calculated using the above equations (8) and (9). y (s) and the transfer function G for curvature calculation θ (s) was decided.

[0042] As shown above, the directional control gain K θ and position control gain K yThe path following control device 3 with the adjusted speed was connected to the vehicle 1 according to the above embodiment. An experiment was conducted to park the vehicle 1 in the center of the parking area without colliding with the left and right boundaries of the parking area. The vehicle 1 used was 18.2 m long and 3.4 m wide. The parking area was set to a rectangle with a length of 11.03 m and a width of 3.8 m. The initial state of the vehicle 1 was set to a state in which the front end of the vehicle 1 was 7 m away from the entrance of the parking area, shifted 0.3 m in the Y-axis (horizontal) direction from the center line of the parking area, and tilted by 1 deg. In addition, a target route R was set as shown by the dashed line in FIG. 5. The vehicle 1 in this initial state was automatically driven along the target route R to the center of the parking area. The solid line in FIG. 5 is the trajectory of the center of the vehicle 1 (plots of the position actual value y for multiple times).

[0043] As a result of the experiment, the trajectory of the vehicle 1 generally coincided with the target route R, as shown in FIG. 5. This indicates that the route tracking control device 3 according to this embodiment can perform route tracking control of the vehicle 1 with at least the same level of accuracy as in the past. However, since deviation from the target route R was observed in the early stages of the experiment, there is a possibility that the vehicle 1 may come into contact with the side wall of the parking area when the driving conditions are more severe (for example, when there is an obstacle on the way, when the target route R has a more complicated curve, etc.).

[0044] [Variation 1] Transfer function G for direction calculation y (s) and the transfer function G for curvature calculation θ When (s) is determined as in the above equations (8) and (9), the vehicle position target value y r As mentioned above, the relationship between the actual position value y and the actual position value y is expressed by a second-order delay transfer function such as the above formula (11). When a point (x, y) on the target route R is expressed by the relational formula y = route(x), the actual position value x in the forward direction can be expressed by the following formula: r When the vehicle position target value y r The relationship between the actual position value (x, y) and the actual position value (x, y) is as shown in FIG. 2. When the vehicle 1 is located at a position indicated by the actual position value y at a certain time, the vehicle position target value y at the certain time is rIt is not possible to instantly move to the location indicated by the arrow; it takes time to move there.

number

[0045] Therefore, the calculation unit 33 may have an early start compensation function. That is, as shown in FIG. 6, in the target value calculation process S4, the calculation unit 33 calculates the delay time T F The target value of the position of vehicle 1 in the future from the time when the position actual value y was acquired is set to the vehicle position target value y r The delay time T F is a vehicle position target value y at a certain time when the vehicle 1 is located at a position indicated by the position actual value y at the certain time. r Specifically, the calculation unit 13 first calculates the delay time T F The calculation unit 13 calculates the position actual value x1 in the forward traveling direction according to the following formula (16). Next, the calculation unit 13 calculates the delay time T F The coordinate y of the point on the target route R corresponding to the next position actual value x1 r1 Then, calculate the calculated coordinate y r1 are the vehicle position target values ​​in the lateral direction. Note that these values ​​x, x1, y, y r、 y r1 The relationship is as shown in Figure 7. Note that the time delay T F In the case of a second-order delay system as shown in the above equation (12), the damping constant σ(=ζω n ) can be taken as the inverse of

number

number

[0046] By providing such an early compensation function, the path tracking control device 3 can reduce the path tracking error (vehicle position target value y rIn other words, according to the path tracking control device 3 of the first modification, it is possible to improve the accuracy of tracking the target path R while maintaining the ease of adjusting the control gains described in the above embodiment.

[0047] In order to demonstrate this, the inventors prepared a path tracking control device 3 according to the first modification and conducted the following experiment. First, the directional control gain K θ and position control gain K y was adjusted in the same manner as in the experiment of the above embodiment. F was set to the reciprocal of the damping constant σ. This path tracking control device 3 was connected to the same vehicle 1 as that used in the experiment of the above embodiment. Then, an experiment was conducted to park the vehicle 1 in the center of the parking area, similar to the above embodiment. The solid line in FIG. 8 is the trajectory of the center of the vehicle 1. As a result of the experiment, the trajectory of the vehicle 1 almost coincided with the target path R even near the initial position, as shown in FIG. 8. This shows that the path tracking control device 3 according to the first modification can perform path tracking control of the vehicle 1 with higher accuracy than the conventional method. This also shows that the early exit compensation function can safely move the vehicle 1 to the parking area even under harsher driving conditions.

[0048] [Variation 2] Vehicle target position y r1 If we set it as in the following equation (18), then theoretically y r = y, it is possible to further reduce the path tracking error due to the time delay in the control system.

number

[0049] The above formula (18) can be rewritten as the following formulas (19) and (20).

number

number

[0050] The Laplace operator s is the operator that represents differentiation. That is, sy r is expressed as the following equation (21): r The first time derivative of s 2 y r is the vehicle position target value y r The velocity v of vehicle 1 is the first time differential of the vehicle position in the traveling direction, as shown in the following equation (23).

number

number

number

[0051] Using the above equations (21) to (23), the target lateral vehicle position value y r The first spatial derivative of (vehicle position target value y r The coordinates (x, y r ) the slope of the tangent line that touches the target path R at r / dx, and the second spatial derivative (coordinates (x, y r ) the curvature of the target path R at 2 y r / dx 2 is expressed by the following equations (24) and (25). By substituting the following equations (24) and (25) into the above equation (20), the vehicle position target value y r1 is expressed as the following equation (26).

number

number

number

[0052] Therefore, when the target route R is given as in the above formula (15), the calculation unit 33 may execute a tilt calculation process S5 (fourth calculation process) and a curvature calculation process S6 (fifth calculation process) as shown in FIG. r The curvature calculation process S6 is a process for calculating the gradient of a tangent line that is in contact with the target route R at the position indicated by the vehicle position target value y r The calculation unit 33 calculates the curvature of the target route R at the position indicated by the arrow (the second-order spatial differential value of the target route R). Then, the calculation unit 33 calculates the first-order spatial differential value and the second-order spatial differential value of the target route R as the FF control term, and calculates the travel direction target value θ r and the curvature target value κ r Then, in the second acquisition process S31, the calculation unit 33 calculates the travel direction target value θ r In the output process S33, the curvature target value κ r It will output information based on the above.

[0053] By having such an FF control function, the path tracking control device 3 can reduce path tracking errors caused by time delays in the control system more than in the case of employing the above-mentioned Modification 1. In other words, the path tracking control device 3 according to Modification 2 can improve the accuracy of tracking the target path R more than the path tracking control device 3 according to Modification 1 while maintaining the ease of adjusting the control gains described in the above embodiment.

[0054] In order to demonstrate this, the inventors prepared a path tracking control device 3 according to the modified example 2 and conducted the following experiment. First, the directional control gain K θ and position control gain K ywas adjusted in the same manner as in the experiment of the above embodiment. This path following control device 3 was connected to the same vehicle 1 as that used in the experiment of the above embodiment. Then, an experiment was conducted to park the vehicle 1 in the center of the parking area, similar to the above embodiment. The solid line in FIG. 10 is the trajectory of the center of the vehicle 1. As a result of the experiment, the trajectory of the vehicle 1 was more consistent with the target path R than in the case of the modified example 1, as shown in FIG. 10. This shows that the path following control device 3 according to the modified example 2 can perform the path following control of the vehicle 1 with even higher accuracy than the conventional one. This also shows that the FF control function can safely move the vehicle 1 to the parking area even under harsher conditions.

[0055] [Variation 3] When the vehicle 1 requires a steering angle φ as an operation input, the calculation unit 33 may be configured to execute a determination process S34 between the second calculation process S32 and the output process S33 as shown in Fig. 11. The steering angle φ is the magnitude of the angle between the front-rear direction of the vehicle 1 and the direction in which the wheels roll. In this case, in the determination process S34, the calculation unit 33 determines the curvature target value κ r In this case, in the output process S33, the calculation unit 33 may be configured to output the steering angle φ calculated in the determination process S34 as information based on the curvature target value.

[0056] In addition, when the vehicle 1 requires an angular velocity ω (calculated as ω=dθ / dt=vκ from the formula (7)) which is a time change amount of the traveling direction θ of the vehicle 1 as an operation input (for example, when the vehicle 1 is a caterpillar type such as a bulldozer), in the determination process S34, the calculation unit 33 calculates the curvature target value κ r Specifically, the calculation unit 33 may determine the angular velocity ω based on the curvature target value κ rThe angular velocity ω is calculated by multiplying the actual velocity value v by the angular velocity ω. In this case, in the output process S33, the calculation unit 33 may be configured to output the angular velocity ω calculated in the determination process S34 as information based on the curvature target value. The calculation unit 33 may execute the determination process S34 in the second calculation process S32 or the output process S33.

[0057] [Variation 4] When the calculation unit 33 executes the above-mentioned determination process S34, the curvature target value κ r The response delay t d In this case, the curvature κ(t) at time t is the dead time t d Time after the lapse (t+t d ) affects the traveling direction θ of vehicle 1. In other words, the response of vehicle 1 has a dead time t d In the presence of x(t+t d ), y(t+t d ), θ(t+t d ), v(t+t d ) at time (t+t d ) are the actual position value in the traveling direction of the vehicle 1, the actual position value in the lateral direction, the actual traveling direction value, and the actual speed value.

number

number

number

[0058] The above formulas (27) to (29) are expressed as x(t+t d ) to x, and y(t+t d ) to y, and θ(t+t d ) to θ, v(t+t d) to v and κ(t) to κ, the equations (5) to (7) correspond to the simplified equations of motion of the vehicle 1. r The response delay t d If there is a dead time t, the calculation unit 33 calculates the dead time t from the current time t in the first calculation process S22. d Prediction of the actual position value after t d ), y(t+t d ) may be calculated using the above formulas (27) and (28). In this case, the calculation unit 33 may calculate the time t from the current time t in the second calculation process S32. d The predicted value of the actual travel direction after the passage of time θ(t+t d ) may be calculated using the above formula (29). The calculation unit 33 then calculates the predicted value x(t+t d ), y(t+t d ), and the predicted value of the actual travel direction θ(t+t d ) based on the curvature target value κ r The calculation unit 33 may also calculate the dead time t d The predicted value of the actual speed after the lapse of time v(t+t d ) is calculated by using, for example, the following equation (30): r The calculation unit 33 may calculate the time delay t from the calculated current time t. d Each predicted value x(t+t d ), y(t+t d ), θ(t+t d ), v(t+t d ) may be regarded as x, y, θ, and v, respectively, and tracking control may be performed.

number

[0059] In this way, the path tracking control device 3 can reduce the path tracking error caused by the dead time. r The calculation of the dead time t from the current time t dAfter (t+t d ) is used. Therefore, when the vehicle position target value changes over time, the vehicle position target value is calculated by subtracting y r (t+t d On the other hand, when the vehicle position target value changes according to the traveling direction position as in the above formula (15), x(t+t d ) to calculate the vehicle position target value.

[0060] [Variation 5] In the above embodiment, the series of processes in the cascade control C are all executed by the single calculation unit 33 included in the path tracking control device 3. However, the path tracking control device 3 executes the major loop process S2 (the target travel direction value θ r A calculation unit that performs the minor loop process S3 (calculating the curvature target value κ r The calculation unit that executes the major loop process S2 and the calculation unit that executes the minor loop process S3 may be provided separately. Also, the calculation unit that executes the major loop process S2 and the calculation unit that executes the minor loop process S3 do not have to be provided in one path following control device 3. That is, the calculation unit that executes the major loop process S2 may be provided in the vehicle position control device, and the calculation unit that executes the minor loop process S3 may be provided in the traveling direction control device. In this case, when adjusting the control gain, the vehicle position control device is removed, a test signal is input to the traveling direction control device, and the direction control gain K θ Then, connect the vehicle position control device and input the test signal to the vehicle position control device to adjust the position control gain K y will be adjusted.

[0061] [Variation 6] In the above embodiment, the calculation unit 33 executes the path following control program to execute a series of processes in the cascade control C. However, some or all of the above processes can be realized by logic circuits. Therefore, an integrated circuit in which a logic circuit that functions as each control block corresponding to each of the above processes is formed is also included in the scope of the present invention.

[0062] <Path following control method> Hereinafter, a path tracking control method according to an embodiment of another aspect of the present invention will be described.

[0063] [Path following control method flow] The path tracking control method is a method for causing a controlled vehicle 1 to follow a target path R. As shown in Fig. 12, in the path tracking control method, a first acquisition step A21, a first calculation step A22, a second acquisition step A31, a second calculation step A32, and an output step A33 are repeated at a predetermined period until the autonomous driving ends (Step A34: Yes).

[0064] (First acquisition step) In the first acquisition step A21, the actual position value y is acquired (A211). Then, the target vehicle position value y r The first acquisition step A11 can be performed, for example, by using the above-mentioned path tracking control device 3. In the first acquisition step A21, the position actual value y and the vehicle position target value y r may be obtained simultaneously.

[0065] (First calculation step) Position actual value y and vehicle position target value y r After acquiring the vehicle position target value y r and the position actual value y, and the vehicle position deviation and the position control gain K y Based on this, the target value θ r The first calculation step A22 can be performed, for example, by using the above-mentioned path following control device 3.

[0066] (Second acquisition step) Direction target value θ r After the calculation, the process proceeds to a second acquisition step A31. In the second acquisition step A31, first, the travel direction target value θ r(A311). Then, the travel direction actual value θ is acquired (A312). The second acquisition step A31 can be performed, for example, by using the above-mentioned path tracking control device 3. Note that, in the second acquisition step A31, the travel direction target value θ r and the travel direction actual value θ may be acquired simultaneously.

[0067] (Second calculation step) Direction target value θ r After the travel direction actual value θ is acquired, the process proceeds to a second calculation step A32. In the second calculation step A32, the travel direction target value θ r and the deviation between the actual travel direction value θ and the direction control gain K θ Based on this, the curvature target value κ of vehicle 1 is calculated. r The second calculation step A32 can be performed, for example, by using the above-mentioned path following control device 3.

[0068] (Output step) Curvature target value κ r After calculating the curvature target value κ, the process proceeds to output step A33. r In the output step A33, the information based on the curvature target value κ r The target curvature value κ r The output step A33 may be performed by using the path following control device 3, for example.

[0069] [Vehicle operation flow] When the path tracking control method is implemented, the vehicle 1 repeats a first measurement / output step B1, a second measurement / output step B2, and a steering step B3 at a predetermined period until the automatic driving ends (step B4: Yes).

[0070] (First measurement and output step) In the initial first measurement / output step B1, the vehicle 1 measures its own position and outputs the measurement value as an actual position value y to an entity (such as the path following control device 3) that performs the above-mentioned first acquisition step A21.

[0071] (Second measurement and output step) After outputting the actual position value y, the process proceeds to a second measurement / output step B2. In the second measurement / output step B2, the vehicle 1 measures its own traveling direction and outputs the measured value as an actual traveling direction value θ to the subject performing the second acquisition step A31. Note that the actual traveling direction value θ may be output prior to the output of the actual position value y.

[0072] (Steering step) After outputting the travel direction actual value θ, the process proceeds to a steering step B3. In the steering step B3, the vehicle 1 is r Based on the information (curvature target value κ r Or, the steering operation is performed based on the steering angle φ.

[0073] The large loop including the first measurement / output step B1, first acquisition step A21, and first calculation step A22 described above corresponds to a major loop in the cascade control C. On the other hand, the small loop including the second measurement / output step B2, second acquisition step A31, second calculation step A32, and output step A33 corresponds to a minor loop in the cascade control C. This is because the calculation result in the first calculation step A22 is used in the second acquisition step A31.

[0074] [Effects of the path tracking control method] The above-described path tracking control method has the same flow as that in which the path tracking control device 3 executes the cascade control C. Therefore, according to the path tracking control method, the manager can easily control the directional control gain K θ and position control gain K y In other words, the administrator can adjust the directional control gain K θ and position control gain K y The adjustment can be easily performed.

[0075] <Summary> A path following control device according to a first aspect of the present invention is a path following control device connected to a vehicle to be controlled, and includes a major loop process including a first acquisition process of acquiring a position actual value indicating an actual position of the vehicle and a vehicle position target value which is a target value of the position of the vehicle, and a first calculation process of calculating a traveling direction target value which is a target value of the traveling direction of the vehicle based on a vehicle position deviation which is a deviation between the vehicle position target value and the position actual value, and a position control gain; the control unit is provided with a calculation unit that repeatedly executes, at a predetermined cycle, a cascade control including a minor loop process including: a second acquisition process that acquires from the vehicle the travel direction target value calculated in the first calculation process and a travel direction actual value that indicates the actual travel direction of the vehicle; a second calculation process that calculates a curvature target value that is a target value of the curvature of the travel path of the vehicle based on a travel direction deviation that is the deviation between the travel direction target value and the travel direction actual value and a directional control gain; and an output process that outputs information based on the curvature target value.

[0076] A path following control device according to aspect 2 of the present invention may be configured in the above aspect 1 such that, in the first calculation process, the calculation unit calculates the product of the vehicle position deviation and a direction calculation transfer function determined by the position control gain as the traveling direction target value, and in the second calculation process, the calculation unit calculates the product of the traveling direction deviation and a curvature calculation transfer function determined by the direction control gain as the curvature target value, and the direction calculation transfer function and the curvature calculation transfer function are set to be inversely proportional to the speed of the vehicle.

[0077] A path tracking control device according to a third aspect of the present invention may be configured in the above-mentioned second aspect such that the position control gain and the directional control gain are scalars, and the relationship between the vehicle position target value and the actual position value is expressed by a transfer function of a second-order lag system characterized by a damping rate and a natural angular frequency.

[0078] A path following control device according to a fourth aspect of the present invention, in any one of the first to third aspects above, may be configured such that, before starting the major loop process, the calculation unit executes a third calculation process to calculate, based on a predetermined target route, a target value for a position of the vehicle in the future from a time when the actual position value was acquired as the vehicle position target value, by an amount equivalent to a delay time that is the time required for the vehicle, which is present at a position indicated by the actual position value at a certain time, to move to a position indicated by the vehicle position target value at the certain time.

[0079] A path following control device according to aspect 5 of the present invention, in any one of aspects 1 to 3 above, may be configured such that before starting the major loop processing, the calculation unit executes a third calculation process to calculate the vehicle position target value based on a predetermined target route, a fourth calculation process to calculate a slope of a tangent to the target route at a position indicated by the vehicle position target value, and a fifth calculation process to calculate a curvature of the target route at the position indicated by the vehicle position target value, and acquires the traveling direction target value to which the slope has been added in the second acquisition process, and outputs information based on the curvature target value to which the curvature has been added to the vehicle in the output processing.

[0080] A path tracking control device according to a sixth aspect of the present invention may be configured such that, in any of the first to fifth aspects above, when there is a response delay between when the curvature target value is changed and when the traveling direction actual value changes, the calculation unit calculates, in the second calculation process, the current curvature target value based on a predicted value of the position actual value after the delay time has elapsed from the present and a predicted value of the traveling direction actual value, calculated using the past curvature target value.

[0081] A path following control device according to aspect 7 of the present invention may be configured such that, in any of aspects 1 to 6 above, the calculation unit executes a determination process to determine a steering angle, which is the magnitude of the angle between the fore-and-aft direction of the vehicle and the direction in which the wheels roll, based on the curvature target value, and in the output process, outputs the steering angle as information based on the curvature target value.

[0082] A path following control device according to aspect 8 of the present invention may be configured in any one of aspects 1 to 7 above, such that the calculation unit executes a determination process to determine an angular velocity, which is the amount of change over time in the direction of travel of the vehicle, based on the curvature target value, and in the output process, outputs the angular velocity as information based on the curvature target value.

[0083] A path following control method according to a ninth aspect of the present invention is a path following control method for causing a controlled vehicle to follow a target path, the method comprising: a first acquisition step of acquiring a position actual value indicating the actual position of the vehicle and a vehicle position target value which is a target value of the position of the vehicle; a first calculation step of calculating a traveling direction target value which is a target value of the traveling direction of the vehicle based on a vehicle position deviation which is the deviation between the vehicle position target value and the position actual value and a position control gain; a second acquisition step of acquiring from the vehicle the traveling direction target value calculated in the first calculation step and a traveling direction actual value which is an actual traveling direction of the vehicle; a second calculation step of calculating a curvature target value which is a target value of the curvature of the traveling path of the vehicle based on a traveling direction deviation which is the deviation between the traveling direction target value and the traveling direction actual value and a direction control gain; and an output step of outputting information based on the curvature target value, at a predetermined period.

[0084] The present invention is not limited to the above-described embodiments, and various modifications are possible within the scope of the claims. Embodiments obtained by appropriately combining the technical means disclosed in different embodiments are also included in the technical scope of the present invention. [Explanation of symbols]

[0085] 100 Autonomous Driving System 1 vehicle 2 Speed ​​control device 3. Path Following Control Device 31 Input / output section 32 Storage section 321 Path Following Control Program 33 Arithmetic section 331 target value calculation unit (third calculation unit) 332 First Acquisition Section 333 First Calculation Section 334 Second Acquisition Division 335 Second Calculation Section 336 Output Processing Section

Claims

1. A route tracking control device connected to a vehicle to be controlled, a first acquisition process for acquiring a position actual value indicating an actual position of the vehicle and a vehicle position target value which is a target value of the position of the vehicle, and a first calculation process for calculating a traveling direction target value which is a target value of the traveling direction of the vehicle, based on a vehicle position deviation which is a deviation between the vehicle position target value and the position actual value, and a position control gain; a second acquisition process for acquiring from the vehicle the travel direction target value calculated in the first calculation process and a travel direction actual value indicating an actual travel direction of the vehicle, a second calculation process for calculating a curvature target value that is a target value of a curvature of a travel path of the vehicle based on a travel direction deviation that is a deviation between the travel direction target value and the travel direction actual value and a directional control gain, and an output process for outputting information based on the curvature target value; A calculation unit that repeatedly executes cascade control including the above at a predetermined cycle. A path following control device comprising:

2. The calculation unit is In the first calculation process, a product of the vehicle position deviation and a direction calculation transfer function determined by the position control gain is calculated as the traveling direction target value; the second calculation process is configured to calculate a product of the traveling direction deviation and a curvature calculation transfer function determined by the directional control gain as a curvature target value, The direction calculation transfer function and the curvature calculation transfer function are set to be inversely proportional to the speed of the vehicle.

2. The path following control device according to claim 1 .

3. the position control gain and the directional control gain are scalars; a relationship between the vehicle position target value and the position actual value is expressed by a transfer function of a second-order delay system characterized by a damping rate and a natural angular frequency; 3. The path following control device according to claim 2.

4. the calculation unit executes a third calculation process, before starting the major loop process, to calculate, based on a predetermined target route, a target value of a position of the vehicle in the future from a time when the position actual value is acquired, as the vehicle position target value, by an amount equivalent to a delay time that is a time required for the vehicle, which is present at a position indicated by the position actual value at a certain time, to move to the position indicated by the vehicle position target value at the certain time.

4. The path following control device according to claim 1, wherein the path following control device is a control device for controlling a vehicle.

5. The calculation unit is a third calculation process for calculating the vehicle position target value based on a predetermined target route before starting the major loop process; a fourth calculation process for calculating a gradient of a tangent line that is tangent to the target route at a position indicated by the vehicle position target value; a fifth calculation process for calculating a curvature of the target route at a position indicated by the vehicle position target value; Run In the second acquisition process, the travel direction target value to which the inclination has been added is acquired; In the output process, information based on the curvature target value to which the curvature has been added is output to the vehicle.

4. The path following control device according to claim 1, wherein the path following control device is a control device for controlling a vehicle.

6. 、 when a response delay exists between when the curvature target value is changed and when the traveling direction actual value is changed, the calculation unit calculates the current curvature target value in the second calculation process based on a predicted value of the position actual value after the delay time has elapsed from the present and a predicted value of the traveling direction actual value, the predicted value being calculated using the past curvature target value.

4. The path following control device according to claim 1, wherein the path following control device is a control device for controlling a vehicle.

7. The calculation unit is Executing a determination process for determining a steering angle, which is the magnitude of the angle between the front-rear direction of the vehicle and the direction in which the wheels roll, based on the curvature target value; In the output process, the steering angle is output as information based on the curvature target value.

4. The path following control device according to claim 1, wherein the path following control device is a control device for controlling a vehicle.

8. The calculation unit is Executing a determination process for determining an angular velocity, which is a time change amount in a traveling direction of the vehicle, based on the curvature target value; In the output process, the angular velocity is output as information based on the curvature target value.

4. The path following control device according to claim 1, wherein the path following control device is a control device for controlling a vehicle.

9. A path following control method for causing a controlled vehicle to follow a target path, comprising: a first acquisition step of acquiring a position actual value indicating an actual position of the vehicle and a vehicle position target value which is a target value of the position of the vehicle; a first calculation step of calculating a traveling direction target value that is a target value in the traveling direction of the vehicle based on a vehicle position deviation that is a deviation between the vehicle position target value and the position actual value and a position control gain; a second acquisition step of acquiring, from the vehicle, the travel direction target value calculated in the first calculation step and a travel direction actual value indicating an actual travel direction of the vehicle; a second calculation step of calculating a curvature target value that is a target value of the curvature of a travel path of the vehicle based on a traveling direction deviation that is a deviation between the traveling direction target value and the traveling direction actual value and a directional control gain; an output step of outputting information based on the curvature target value; Repeat at a predetermined interval. A path following control method comprising:

Images