Path following system for vehicle, path following method for vehicle, and computer program

Abstract

To suppress the occurrence of displacement with respect to a target route due to a response delay of a steering device when a vehicle is controlled to follow the target route having a large absolute value of curvature.SOLUTION: A path following system for a vehicle includes a setting unit that sets a target path and a speed target value that is a constant speed, a point determination unit that determines a target arrival point of the vehicle on the target path, a steering angle determination unit that determines a target value of a steering angle using a curvature of the target path at the target arrival point, a difference between a reference point and a position of the vehicle, and a difference between a tangential direction angle of the target path at the reference point and a yaw angle of the vehicle, and a speed determination unit that determines a rotation target value of a drive motor of the vehicle.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to a vehicle path tracking system, a vehicle path tracking method, and a computer program. [Background technology]

[0002] A control system for driving a vehicle so as to follow a target route is known (see, for example, Patent Document 1). The system described in Patent Document 1 acquires a signal related to the vehicle's position and predicts the vehicle's behavior when a control signal that reduces the deviation between the vehicle's position and the target route is input to the vehicle. This control system further inputs a control signal predicted for a predetermined time period after the current time by repeatedly predicting the vehicle's behavior from the current time point after a sampling period. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent No. 7212702 Summary of the Invention [Problem to be solved by the invention]

[0004] A response delay occurs between the time a control signal is input and the time the steering device operates in accordance with the input control signal. In the system described in Patent Document 1, a point to be reached on a target route after a predetermined time (hereinafter referred to as a "look-ahead point") is predicted, and the curvature of the look-ahead point is used to control the steering angle of the vehicle. However, if the absolute value of the curvature of the target route is large, the vehicle speed changes due to the lateral force generated on the steering wheel, and the prediction accuracy decreases with changes in vehicle speed. As a result, even if control is performed using the curvature of the look-ahead point, the prediction accuracy is low, so there is a risk that the effect of the response delay of the steering device will not be suppressed.

[0005] The present invention has been made to solve at least some of the above-mentioned problems, and aims to suppress the occurrence of positional deviation from a target path due to delayed response of the steering device when controlling a vehicle to follow a target path with a large absolute value of curvature. [Means for solving the problem]

[0006] The present invention has been made to solve at least part of the above-mentioned problems, and can be realized in the following forms.

[0007] (1) According to one aspect of the present invention, there is provided a path tracking system for a vehicle, the path tracking system including a setting unit that sets a target path for the vehicle and a target speed value that is a constant speed of the vehicle while traveling, an acquisition unit that acquires the position of the vehicle at a current time T1 and the yaw angle of the vehicle with respect to a reference direction at time T1, a point determination unit that determines a target destination point for the vehicle on the target path at time T2 after time T1, and a curvature of the target path at time T2, a difference between the position of the vehicle at time T1 and a reference point that is an intersection of the target path and a straight line extending from the position of the vehicle at time T1 in a direction perpendicular to the longitudinal direction of the vehicle, and the position of the vehicle at time T1, and a tangential angle of the target path at the reference point and the tangential angle of the target path at time T1. and a speed determination unit that determines a rotation target value, which is a target value for the rotation speed of a drive motor of the vehicle, using the absolute value of the curvature of the reference point and the target speed value, wherein the speed determination unit increases the rotation target value over time in a section where the absolute value of the curvature of the reference point increases, decreases the rotation target value over time in a section where the absolute value of the curvature of the reference point is constant, and decreases and then increases the rotation target value over time in a section where the absolute value of the curvature of the reference point decreases.

[0008] According to this configuration, the vehicle is controlled to travel along a target route at a set constant speed target value. To achieve this control, a target steering angle is determined using the vehicle's position at the current time T1 and the vehicle's yaw angle relative to a reference direction, a target destination point for the vehicle at time T2, which is later than time T1, and a reference point, which is the intersection of a line extending from the vehicle's position at time T1 in a direction perpendicular to the vehicle's longitudinal direction and the target route. The speed determination unit determines a target rotation value for the drive motor that rotates the vehicle's drive wheels based on the section where the absolute value of the curvature of the reference point increases, remains constant, or decreases. When the absolute value of the curvature of the target route is large, the vehicle speed (vehicle speed) changes due to the lateral force generated on the steered wheels, and the change in vehicle speed reduces the accuracy of predicting the destination point for the vehicle. According to this configuration, the target rotation value for the drive motor is determined so that the vehicle's traveling speed is a constant target speed value. As a result, when the vehicle is caused to follow a target route with a large absolute value of curvature, the occurrence of deviation of the vehicle's position from the target route due to a delay in the response of the steering device is suppressed, thereby enabling the vehicle to travel while following the target route.

[0009] (2) In the path tracking system of the above aspect, the point determination unit may determine, as the target arrival point at time T2, a point on the target path that is a distance away from the position of the vehicle at time T1 that is calculated by multiplying a previously identified steering device response delay time by the target speed value. With this configuration, a point ahead on the target route that is a distance equal to the product of the response delay time and the target speed is re-determined as the target arrival point for each control cycle of the steering device. Therefore, the target value of the steering angle is recalculated each time using the re-determined target arrival point. This allows the vehicle to travel while accurately tracking the target route.

[0010] (3) In the path tracking system of the above aspect, the speed determination unit may change the rotation target value from decreasing to increasing when the absolute value of the vehicle longitudinal component of the estimated value of the lateral force generated on the steering wheel turns to decrease in a section where the absolute value of the curvature of the reference point decreases. According to this configuration, in the section where the absolute value of the curvature at the reference point decreases, the target rotation value of the drive motor decreases over time and then increases. In this configuration, the timing at which the target rotation value changes from decreasing to increasing is identified as the timing at which the absolute value of the vehicle longitudinal component of the estimated value of the lateral force acting on the steered wheels changes from decreasing. As a result, the accuracy of the timing at which the target rotation value changes from decreasing to increasing is improved, and the occurrence of deviation of the vehicle's position from the target route with respect to the target route is suppressed.

[0011] The present invention can be realized in various forms, for example, in the form of a vehicle path tracking device, a vehicle control device, a steering control device, a vehicle automatic steering device, a vehicle path tracking method, a vehicle control method, a system including these devices, a computer program for executing these devices, a server device for distributing this computer program, a non-transitory storage medium on which a computer program is stored, etc. [Brief explanation of the drawings]

[0012] [Figure 1] 1 is a block diagram of a vehicle path tracking system according to an embodiment of the present invention; [Figure 2] FIG. 2 is an explanatory diagram of the current position and yaw angle of a vehicle. [Figure 3] FIG. 2 is an explanatory diagram of a lateral force generated in a steered wheel. [Figure 4] FIG. 4 is an explanatory diagram of the time change characteristics of a lateral force. [Figure 5] 2 is a flowchart of a vehicle path tracking method according to the present embodiment. [Figure 6] FIG. 4 is an explanatory diagram of a change over time in a target value of a drive motor. DETAILED DESCRIPTION OF THE INVENTION

[0013] First Embodiment FIG. 1 is a block diagram of a path tracking system 100 for a vehicle Ve according to one embodiment of the present invention. In this embodiment, the path tracking system 100 included in the vehicle Ve uses state variables acquired by sensors to control the steering device (not shown) and drive device (not shown) of the vehicle Ve so that the vehicle Ve travels at a constant speed while following a set target path. The greater the absolute value of the curvature of the target path, the greater the absolute value of the lateral force (a force component parallel to the direction perpendicular to the steering wheel direction) acting on the steering wheels of the vehicle Ve, making the speed of the vehicle Ve more likely to change. However, in this embodiment, the rotational speed of the drive motor that rotates the drive wheels is controlled in accordance with the absolute value of the curvature of the target path, and the steering angle of the steering wheels is controlled in accordance with the response delay time of the steering device, thereby suppressing positional deviation from the target path caused by a response delay of the steering device. Note that in this embodiment, velocity represents a vector quantity having a directional component, and speed represents a scalar quantity of velocity without a directional component. The vehicle Ve in this embodiment is an electric forklift.

[0014] 1, the path tracking system 100 of this embodiment includes an ECU (Electric Control Unit) 10 that controls each part of the vehicle Ve, a sensor system 20 that acquires state quantities of the vehicle Ve and transmits them to the ECU 10, a steering control unit 30 that controls the steering device, a drive control unit 31 that controls the drive device, and an input unit 40 that accepts various operations. The input unit 40 is composed of a keyboard, a mouse, and a microphone, and accepts various operations from the user.

[0015] The sensor system 20 includes an RTK-GPS (Real Time Kinematic - Global Positioning System) 21, a LiDAR (Light Detection And Ranging) 22, and a camera 23. The RTK-GPS 21 detects the position and attitude (orientation) of the vehicle Ve. The LiDAR 22 is disposed in front of, behind, and on the left and right sides of the vehicle Ve. The LiDAR 22 detects obstacles and the like present around the LiDAR 22 by detecting and ranging using light. The cameras 23 are disposed in front of and behind the vehicle Ve and capture images of a predetermined range in front of and behind the vehicle Ve.

[0016] The ECU 10 is a so-called CPU (Central Processing Unit), and is connected to a ROM (Read Only Memory) and a RAM (Random Access Memory), not shown, and controls each part of the vehicle Ve by expanding a computer program stored in the ROM into the RAM and executing it. The ECU 10 also functions as a setting unit 11, an acquisition unit 12, a location determination unit 13, a steering angle determination unit 14, a speed determination unit 15, and a communication unit 16 that acquires various information from other devices, servers, etc. via wireless communication.

[0017] The setting unit 11 sets a target route for the vehicle Ve to follow and a target value (target speed value) V * In this embodiment, the target value V * is set as a constant speed. That is, in this embodiment, the speed of the vehicle Ve is controlled to be constant. * is acquired via the input unit 40. In other embodiments, the data may be acquired from another device or the like via the communication unit 16, or previously used data may be used.

[0018] The acquisition unit 12 controls the control period t c Every time time passes, the current position Pv (coordinates (x v , y v )) and yaw angle θv and are acquired from the sensor system 20. FIG. 2 shows the current position Pv and yaw angle θ of the vehicle Ve. v 2 is an explanatory diagram of the present position Pv of the vehicle Ve. The midpoint between the left front wheel and the right front wheel is defined as the current position Pv of the vehicle Ve. FIG. 2 shows a schematic bird's-eye view of the target route TR and the vehicle Ve. In FIG. 2, an orthogonal coordinate system CS is set, which is made up of an X-axis and a Y-axis that are orthogonal to each other, in a plane direction that is orthogonal to the vertical direction. The coordinates of the current position Pv of the vehicle Ve are (x v , y v ) The yaw angle θ at the current position Pv of the vehicle Ve v is a parameter that indicates the orientation of the vehicle Ve, and in this embodiment, is expressed as an angle relative to the X axis that is the reference direction of the Cartesian coordinate system CS.

[0019] 1 determines a look-ahead point (target arrival point) Pf at the current time T1. The look-ahead point Pf is determined by a steering device response delay time t d Time T2 (=T1+t c ) is a position where the vehicle Ve is desired to reach. The look-ahead point Pf in this embodiment is a position where the target value V of the speed of the vehicle Ve set by the setting unit 11 at time T2 is reached. * and the previously identified response delay time t d The distance V is the product of * t d The point is determined as a point on the target route TR just ahead.

[0020] The steering angle determination unit 14 shown in FIG. 1 determines a target value δ of the steering angle of the vehicle Ve when the vehicle Ve moves from the current position Pv to the look-ahead point Pf. * The steering angle is defined as a positive value in the counterclockwise direction. The steering angle determination unit 14 determines the target value δ of the steering angle. * In order to determine the curvature C of the target route TR at the look-ahead point Pf p and the target speed V of the vehicle Ve * Using this, the response delay time t d The desired yaw rate of the vehicle after p V *Note that the curvature and yaw rate are defined as positive in the counterclockwise direction.

[0021] The steering angle determination unit 14 calculates a lateral position deviation e associated with the difference between the current reference point Pc (FIG. 2) and the current position Pv of the vehicle Ve. pos and yaw angle deviation e yaw Calculate the reference point Pc (coordinates (x c ,y c )) corresponds to the intersection of a line extending from the current position Pv in a direction perpendicular to the vehicle longitudinal direction and the target route TR. pos is the distance between the current position Pv and the reference point Pc, as shown in Figure 2. The yaw angle deviation e yaw is the yaw angle θ of the vehicle Ve at the current position Pv v and the tangential angle θ of the target route TR at the reference point Pc. c The steering angle determination unit 14 calculates the lateral position deviation e pos and yaw angle deviation e yaw Calculate.

[0022]

number

[0023] The steering angle determination unit 14 determines the target value r of the current vehicle yaw rate. * is calculated using the following formula (3). pos and K. yaw is the feedback gain.

number

[0024] The steering angle determination unit 14 determines the target value r of the current vehicle yaw rate calculated by the above formula (3). * and the current target value δ of the steering angle is calculated from the following equation (4): * Calculate L in equation (4) w is the wheelbase.

number

[0025] The speed determination unit 15 shown in FIG. 1 calculates the curvature C of the target route TR at the reference point Pc. c (Hereafter, we will simply refer to the curvature C of the reference point Pc. c ") and the target speed V * and the target value (rotation target value) ω of the rotation speed of the drive motor that rotates the drive wheels FT of the vehicle Ve is calculated using the above equation. * m The speed determination unit 15 determines the target value ω of the rotation speed of the drive motor. * m To determine the target speed V * Using the following equation (5), the reference value ω of the rotation speed of the drive motor is calculated. * mr Calculate K in equation (5) m is a proportionality constant determined by the reduction ratio of the drive unit and the tire radius of the drive wheel FT. * mr is the speed of the vehicle Ve at the target value V * This is the same as the target value of the rotational speed of the drive motor when traveling straight ahead.

[0026]

number

[0027] When turning along the target path TR, the target value ω of the rotational speed of the drive motor is * m ω * mr When the vehicle speed is fixed at a constant target value V, the speed of the vehicle Ve changes depending on the lateral force generated at the steering wheel RT. * The target value ω of the rotational speed of the drive motor is set to * m Determine.

[0028] Figure 3 shows the lateral force F generated on the steering wheel RT. yr FIG. 3 is an explanatory diagram of the lateral force F generated on the steering wheel RT. yr, the vehicle longitudinal direction DR1, the center of gravity G of the vehicle Ve, and the velocity V of the center of gravity G in the vehicle longitudinal direction x and the vehicle lateral velocity V y The lateral force F yr is defined as the left direction of the steering wheel RT being positive. The longitudinal velocity of the vehicle at the center of gravity G is V x is defined as the forward direction of the vehicle Ve being positive. The lateral velocity of the vehicle at the center of gravity G is V y is defined as the left direction of the vehicle Ve being positive. Also, the longitudinal velocity of the vehicle at the center of gravity G is V x The magnitude of corresponds to the speed of the vehicle Ve. The vehicle Ve in this embodiment is a 2WD vehicle in which the front wheels, which are drive wheels FT, are rotated by the drive motor MT, and the power of the drive motor MT is not transmitted to the rear wheels, which are steered wheels RT. The steering angle of the front wheels does not change, but the steering angle of the rear wheels changes. The distance between the drive wheels FT and the center of gravity G along the vehicle longitudinal direction DR1 is L f The distance between the steering wheel RT and the center of gravity G along the vehicle longitudinal direction DR1 is L r is.

[0029] When the steering angle δ of the steering wheel RT shown in FIG. 3 occurs, a lateral force F is applied to the steering wheel RT in a direction perpendicular to the direction of the steering wheel RT. yr Lateral force F yr is expressed as the following formula (6). Note that K in formula (6) r is the cornering stiffness, which is a known value. Equation (6) is the lateral force F yr is the lateral velocity of the vehicle V y and changes depending on the vehicle yaw rate r and steering angle δ.

[0030]

number

[0031] FIG. 4 shows the rotational speed of the drive motor when the target value is a reference value ω * mr When fixed at the yr 4(a) to 4(f) are explanatory diagrams of the time-varying characteristics of the curvature C. FIG. 4 shows a case where a vehicle Ve turns right along a target route TR.c The absolute value of , the steering angle δ, and the lateral velocity V y , vehicle yaw rate r, and lateral force F yr and the vehicle longitudinal velocity V x The graphs show the changes over time. As shown in FIG. 4(a), the target route TR is a straight line before time t0, and has a curvature C c The absolute value of δ gradually increases in the section SC1. Therefore, in the section SC1, the steering angle δ increases as shown in FIG.

[0032] The target route TR has a curvature C c That is, in the section SC2, the steering angle δ remains constant as shown in FIG. 4(b). The target route TR changes into a curve with a curvature C c The absolute value of δ gradually decreases, and after time t3, it changes to a straight line. Therefore, in section SC3, the steering angle δ decreases as shown in FIG. 4(b).

[0033] Curvature C c In the section SC1 where the absolute value of increases, the lateral velocity V y increases slower than the steering angle δ, and the vehicle yaw rate r decreases slower than the steering angle δ, as shown in Figure 4(d). Therefore, the steering angle δ in the above equation (6) becomes dominant. Therefore, as the steering angle δ increases, the lateral force F yr As a result, the longitudinal velocity of the vehicle V increases as shown in Figure 4(f). x decreases, the speed of the vehicle Ve decreases to the target value V * will be lower than

[0034] Curvature C c In the section SC2 where the absolute value of remains constant, the speed V y increases, and the vehicle yaw rate r decreases as shown in Figure 4(d). Therefore, from the above equation (6), the lateral force F yrAs a result, the longitudinal velocity of the vehicle V decreases as shown in Figure 4(f). x increases, but the target value V * Since the speed of the vehicle Ve is lower than the target value V * It will tend to be lower than

[0035] Curvature C c In the section SC3 where the absolute value of decreases, the lateral velocity V y decreases, and the vehicle yaw rate r increases as shown in Figure 4(d). Therefore, from the above equation (6), the lateral force F yr After decreasing, it starts to increase and finally becomes zero. As a result, as shown in Figure 4(f), the longitudinal velocity of the vehicle V x increases to the target value V * Since the speed of the vehicle Ve is higher than the target value V * It will tend to be higher than

[0036] The speed determination unit 15 of this embodiment determines whether the speed of the vehicle Ve is equal to or exceeds the target value V regardless of the change in the curvature of the target route TR. * So, the lateral force F yr Based on the time-varying characteristics of the drive motor MT, the target value ω * m Specifically, the speed determination unit 15 determines the curvature C c In the section SC1 where the absolute value of increases, the target value ω of the drive motor MT * m The reference value ω * mr As a result, in the section SC1, the vehicle longitudinal speed V x does not decrease, the speed of the vehicle Ve reaches the target value V * is maintained.

[0037] The speed determination unit 15 also determines the curvature C c In the section SC2 where the absolute value of is constant, the target value ω of the drive motor MT * mAs a result, in the section SC2, the vehicle longitudinal speed V x Since the increase of is suppressed, the speed of the vehicle Ve is set to the target value V * is maintained.

[0038] The speed determination unit 15 also determines the curvature C c In the section SC3 where the absolute value of decreases, the target value ω of the drive motor MT * m is decreased over time and then increased. As a result, in the section SC3, the vehicle longitudinal speed V x Since the increase of is suppressed, the speed of the vehicle Ve is set to the target value V * is maintained.

[0039] 5 is a flowchart of the route tracking method for the vehicle Ve according to the present embodiment. In the route tracking flow, first, the setting unit 11 sets a target route TR for the vehicle Ve and a target value V for the speed of the vehicle Ve. * The acquisition unit 12 performs a setting step of setting the control period t c Each time the time elapses, the current position Pv of the vehicle Ve and the yaw angle θ v The point determination unit 13 performs an acquisition step of determining a pre-read point Pf at the current time T1 (step S3). The point determination unit 13 determines a target value V of the speed of the vehicle Ve. * and the previously identified response delay time t d The distance V is the product of * t d A point ahead on the target route TR, which is a distance of .gtoreq..times ...

[0040] The steering angle determination unit 14 determines a target value δ of the steering angle of the vehicle Ve when the vehicle Ve moves from the current position Pv to the look-ahead point Pf. * In the steering angle determination step, the steering angle determination unit 14 determines the curvature C of the target route TR at the look-ahead point Pf (step S4). p and the target speed V of the vehicle Ve * Using this, the response delay time td The desired yaw rate of the vehicle after p V * The steering angle determination unit 14 sets a lateral position deviation e related to the difference between the reference point Pc and the current position Pv of the vehicle Ve. pos and yaw angle deviation e yaw The steering angle determination unit 14 calculates the target value r of the current vehicle yaw rate using the above formulas (1) and (2). * The steering angle determination unit 14 calculates the target value r of the calculated current vehicle yaw rate using the above formula (3). * From the above equation (4), the current steering angle target value δ * Calculate.

[0041] The speed determination unit 15 determines a target value ω of the drive motor MT that rotates the drive wheels FT of the vehicle Ve. * m To determine the target speed V, we use the above equation (5). * From the reference value ω * mr (Step S5), and the curvature C c It is determined whether the absolute value of the curvature C is increasing, constant, or decreasing (step S6). c If it is determined that the absolute value of ω is increasing (step S6: increase), the speed determination unit 15 determines the target value ω of the drive motor MT. * m The reference value ω * mr (Step S7).

[0042] In the process of step S6, the curvature C c If it is determined that the absolute value of ω is constant (step S6: constant), the speed determination unit 15 determines the target value ω of the drive motor MT. * m is decreased over time (step S8). In the process of step S6, the curvature C c If it is determined that the absolute value of ω is decreasing (step S6: decrease), the speed determination unit 15 determines the target value ω of the drive motor MT. * mis decreased and then increased over time (step S9).

[0043] When any of the processes in steps S7 to S9 is completed, the ECU 10 determines whether or not to terminate the route tracking flow (step S10). The determination of whether or not to terminate is made based on the arrival of the vehicle Ve at the target point on the target route TR, the input unit 40 receiving an operation to terminate the route tracking flow, etc. If it is determined that the route tracking flow is not to be terminated (step S10: NO), the ECU 10 determines whether or not to terminate the control period t c The process from step S1 onwards is repeated each time the route tracking flow is determined to end (step S10: YES), the route tracking flow ends.

[0044] As described above, in the path tracking system 100 for the vehicle Ve according to this embodiment, the steering angle determination unit 14 determines the target value δ of the steering angle of the vehicle Ve when the vehicle Ve moves from the current position Pv (FIG. 2) to the look-ahead point Pf. * The steering angle determination unit 14 determines the lateral position deviation e calculated by the above equations (1) and (2). pos and yaw angle deviation e yaw Using the above equations (3) and (4), the current target value of the steering angle δ * The speed determination unit 15 calculates the curvature C c and the target speed V * and the target rotation speed ω of the drive motor that rotates the drive wheels FT of the vehicle Ve is calculated using the above equation. * m The speed determination unit 15 determines the target speed V of the vehicle Ve. * From the above equation (5), the reference value ω of the rotation speed of the drive motor is calculated. * mr The speed determination unit 15 calculates the curvature C c In the section SC1 where the absolute value of increases, the target value ω of the drive motor MT * m The reference value ω * mr The speed determination unit 15 increases the curvature C c In the section SC2 where the absolute value of is constant, the target value ω of the drive motor MT * mThe speed determining unit 15 reduces the curvature C c In the section SC3 where the absolute value of decreases, the target value ω of the drive motor MT * m In this embodiment, the target value V * The steering angle determination unit 14 determines the target value δ of the steering angle to realize the control. * The speed determination unit 15 determines the curvature C c The target value ω of the drive motor MT that rotates the drive wheels FT of the vehicle Ve is determined according to each of the sections SC1 where the absolute value of increases, SC2 where it is constant, and SC3 where it decreases. * m As a result, when the vehicle Ve is made to follow a target route TR having a large absolute value of curvature, the steering device response delay time t d This reduces the occurrence of positional deviation of the route along which the vehicle Ve travels from the target route TR, thereby enabling the vehicle Ve to travel while following the target route TR.

[0045] In addition, the point determination unit 13 of this embodiment determines the target value V of the speed of the vehicle Ve set by the setting unit 11. * and the previously identified response delay time t d The distance V is the product of * t d In this embodiment, a point ahead on the target route TR is determined as the look-ahead point Pf. c Each time, the response delay time t d and the target speed V * A point ahead on the target route TR, which is a distance equal to the product of the distance and the response delay time t d and the target value V * This allows the vehicle Ve to travel while accurately following the target route TR.

[0046] Second Embodiment In the second embodiment, the speed determination unit 15 determines the control period tc Curvature C c The estimated value of the lateral force F yr_e Calculate the estimated value F yr_e The target value ω of the drive motor MT is calculated using * m Therefore, in the second embodiment, the estimated value of the lateral force F yr_e Calculation method of and target value ω of drive motor MT * m The calculation method of the above will be described, and the explanation of the same calculations and controls as in the first embodiment will be omitted.

[0047] The speed determination unit 15 determines the curvature C of the target route TR at the reference point Pc, which changes with the passage of time t. c (t) and the target speed V * Therefore, the ideal vehicle yaw rate r is calculated using the following equation (7). i Calculate (t).

number

[0048] The speed determination unit 15 determines the wheelbase L w Using the above, the ideal steering angle δ is calculated using the following equation (8). i Calculate (t).

number

[0049] The speed determination unit 15 calculates the estimated value F yr_e (t) with curvature C c The speed determination unit 15 calculates the curvature C from time t0 to time t1 by dividing the cases into an interval where the absolute value of (t) increases, an interval where it is constant, and an interval where it decreases. c In the section where the absolute value of (t) increases, the estimated value of the lateral force F yr_e (t) is the cornering stiffness K r It is calculated using the following formula (9).

[0050]

number

[0051] From time t1 to time t2, the absolute value of the curvature C c (t) changes with a time constant T yr in the section where it changes constantly. Therefore, the speed determination unit 15 uses a low-pass filter with a pre-identified time constant T F and the input u F (t) and output y f (t) of the low-pass filter to calculate the estimated value F f (t) of the lateral force. The speed determination unit 15 defines the input u yr_e (t) of the low-pass filter as shown in the following formula (10). f (t) is defined as follows:

[0052]

Equation

[0053] Assuming the output y f (t1) = 0 at time t1, the speed determination unit 15 calculates the output y f (t) at time t (t1 < t ≤ t2) according to the following formula (11). The speed determination unit 15 uses the output y f (t) and a pre-adjusted coefficient K1 to calculate the estimated value F c (t) of the lateral force in the section where the absolute value of the curvature C yr_e (t) changes constantly according to the following formula (12).

Equation

[0054] From time t2 to time t3, in the section where the absolute value of the curvature C c (t) decreases, the lateral force F yr changes with a time constant T F in the section where it changes constantly. Therefore, the speed determination unit 15 uses a low-pass filter with a pre-identified time constant T F and the input u f (t) and output y f (t) of the low-pass filter to calculate the estimated value F yr_e(t) is calculated. The speed determination unit 15 defines the input to the low-pass filter as in the following formula (13). The speed determination unit 15 sets the output y f (t2) of the low-pass filter at time t2 to 0, and uses the following formula (14) to calculate the output y f (t) of the low-pass filter at time t (t2 < t ≤ t3). The speed determination unit 15 uses the output y f (t) and a pre-adjusted coefficient K2 to calculate the estimated value F c (t) of the lateral force in the section where the absolute value of the curvature C yr_e (t) decreases, using the following formula (15).

[0055]

Equation

[0056] The speed determination unit 15 substitutes the estimated value F yr_e (t) of the lateral force calculated in each case of the above formulas (9), (12), and (15) into the following formula (16) to calculate the longitudinal component F yr_e (t) of the lateral force in the vehicle longitudinal direction. yr_e_x (t).

Equation

[0057] The speed determination unit 15 uses the calculated longitudinal component F yr_e_x (t) of the vehicle and a pre-adjusted proportional coefficient K F to calculate the target value ω * m (t) of the drive motor MT at time t according to the following formula (17).

Equation

[0058] Figure 6 shows the target value ω * m6(a) to 6(d) are explanatory diagrams of the time change of the curvature C when the vehicle Ve turns right along the target route TR. c and the estimated value of the lateral force F yr_e and the vehicle longitudinal component F yr_e_x and the target value ω of the drive motor MT * m The graph shows the change in curvature C over time. c In the section from time t2 to time t3 where the absolute value of decreases, the estimated value of the vehicle longitudinal component shown in FIG. 6(c) and the target value ω of the drive motor MT shown in FIG. 6(d) * m The timing of this change is determined by the vehicle longitudinal component F yr_e_x That is, the speed determination unit 15 determines the timing when the curvature C c In the section where the absolute value of decreases, the vehicle longitudinal component F of the estimated lateral force generated on the steering wheel RT yr_e_x After the absolute value of turns from increasing to decreasing, the target value ω of the drive motor MT * m Change from decreasing to increasing.

[0059] As described above, the speed determination unit 15 of the second embodiment determines the curvature C c In the section SC3 where the absolute value of decreases, the vehicle longitudinal component F of the estimated lateral force generated on the steering wheel RT yr_e_x After the absolute value of turns from increasing to decreasing, the target value ω of the drive motor MT * m In this embodiment, the curvature C at the reference point Pc is changed from decreasing to increasing. c In the section SC3 where the absolute value of decreases, the target value ω of the drive motor MT * m decreases and then increases over time. In this embodiment, the target value ω * m The timing at which the value of the estimated lateral force acting on the steering wheel RT changes from a decrease to an increase is yr_e_x As a result, the target value ω *m The accuracy of the timing at which the vehicle speed changes from a decrease to an increase is improved, and the occurrence of deviation of the vehicle's position from the target route TR is suppressed.

[0060] <Modifications of the embodiment> The present invention is not limited to the above-described embodiment, and can be implemented in various forms without departing from the spirit of the present invention, including, for example, the following modifications: In the above-described embodiment, part of the configuration realized by hardware may be replaced by software, and conversely, part of the configuration realized by software may be replaced by hardware.

[0061] In the above first and second embodiments, an example of the path tracking system 100 for the vehicle Ve has been described. However, the path tracking system 100 is not limited to the path tracking system 100 in which the vehicle Ve moves at a constant speed with a target value V * To run at a curvature of C c The lengths of time of the section SC1 where the absolute value of increases, the section SC2 where it remains constant, and the section SC3 where it decreases can be changed.

[0062] The vehicle Ve shown in Figures 2 and 3 is an example and can be modified. The vehicle Ve does not have to be an electric forklift and can be modified within the range of a vehicle that runs using a drive motor MT as a power source. The vehicle Ve may be a hybrid vehicle using a drive motor MT and a gasoline engine as a power source. The relationship between the drive wheels FT and steered wheels RT of the vehicle Ve can also be modified; for example, the front wheels and rear wheels may be drive wheels. The target route TR shown in Figure 2 is a curve in which the curvature of the route of the vehicle Ve that follows the target route TR does not change, but the curvature may also change. Even in the case of a route in which the curvature changes, the determination of the look-ahead point Pf and the target value δ of the steering angle can be modified as in the first embodiment. * The determination of the curvature C c The target value ω of the drive motor MT is set according to the absolute value of * m can be increased or decreased.

[0063] In the first embodiment, the point determination unit 13 determines the target value V of the speed of the vehicle Ve. * and the steering response delay time t d The distance V is the product of * t d However, the distance from the position of the vehicle Ve to the look-ahead point Pf determined on the target route TR is a distance V * t d may be less than the distance V * t d may be greater than the distance V * t d It is preferable that:

[0064] The feedback gain K in the first embodiment pos ,K yaw and wheelbase L w and the proportionality coefficient K m and cornering stiffness K r and the coefficients K1 and K2 in the second embodiment, and the proportionality coefficient K F are values ​​obtained by running the vehicle Ve in advance. These coefficients and the like may be set appropriately depending on the vehicle Ve. The sensor system 20 is capable of changing the position and attitude (orientation) of the vehicle Ve within a detectable range. For example, the sensor system 20 may not have the camera 23, or may have sensors other than the RTK-GPS 21, the LiDAR 22, and the camera 23.

[0065] This aspect has been described above based on embodiments and modifications. However, the above-described embodiments are intended to facilitate understanding of this aspect and are not intended to limit this aspect. This aspect may be modified or improved without departing from the spirit and scope of the claims, and equivalents thereof are included in this aspect. Furthermore, if a technical feature is not described as essential in this specification, it may be deleted as appropriate.

[0066] The present invention can also be realized in the following forms. [Application example 1] A vehicle path following system, comprising: a setting unit that sets a target route for the vehicle and a target speed value that is a constant speed of the vehicle while traveling; an acquisition unit that acquires the position of the vehicle at the current time T1 and the yaw angle of the vehicle with respect to a reference direction at the time T1; a point determination unit that determines a target arrival point of the vehicle on the target route at time T2 that is later than time T1; a steering angle determination unit that determines a target value of the steering angle until the vehicle moves to the target arrival point at time T2, using the curvature of the target route at time T2, the difference between the position of the vehicle at time T1 and a reference point that is an intersection between the target route and a straight line extending from the position of the vehicle at time T1 in a direction perpendicular to the vehicle's longitudinal direction, and the reference point, and the difference between the tangential angle of the target route at the reference point and the yaw angle of the vehicle at time T1; a speed determination unit that determines a rotation target value, which is a target value of the rotation speed of a drive motor of the vehicle, using the absolute value of the curvature of the reference point and the speed target value; Equipped with The speed determination unit In a section where the curvature of the reference point increases, the rotation target value is increased over time; In a section where the curvature of the reference point is constant, the rotation target value is decreased over time; A path tracking system that decreases and then increases the rotation target value over time in a section where the curvature of the reference point decreases. [Application example 2] The path tracking system according to Application Example 1, The point determination unit determines, as the target arrival point at time T2, a point on the target route that is a distance away from the position of the vehicle at time T1 that is a distance calculated by multiplying a previously identified steering device response delay time by the target speed value. [Application example 3] The path tracking system according to Application Example 1 or Application Example 2, a speed determination unit that changes the rotation target value from decreasing to increasing when an absolute value of a vehicle longitudinal component of an estimated value of a lateral force generated on a steering wheel turns to decrease in a section where an absolute value of a curvature of the reference point decreases. [Application example 4] A method for path following a vehicle, comprising: a setting step of setting a target route for the vehicle and a target speed value that is a constant speed of the vehicle while traveling; an acquisition step of acquiring the position of the vehicle at the current time T1 and the yaw angle of the vehicle relative to the reference direction at the time T1; a point determination step of determining a target arrival point of the vehicle on the target route at a time T2 that is later than the time T1; a steering angle determination process for determining a target value of the steering angle until the vehicle moves to the target arrival point at time T2, using the curvature of the target route at time T2, the difference between the position of the vehicle at time T1 and a reference point which is an intersection between the target route and a straight line extending from the position of the vehicle at time T1 in a direction perpendicular to the vehicle's longitudinal direction, and the reference point, and the difference between the tangential angle of the target route at the reference point and the yaw angle of the vehicle at time T1; a speed determination step of determining a rotation target value, which is a target value of the rotation speed of a drive motor of the vehicle, using the absolute value of the curvature of the reference point and the speed target value; Run The speed determining step includes: In a section where the absolute value of the curvature of the reference point increases, the rotation target value is increased over time; In a section where the absolute value of the curvature of the reference point is constant, the rotation target value is decreased over time; A path tracking method, wherein in a section where the absolute value of the curvature of the reference point decreases, the rotation target value is decreased over time and then increased. [Application example 5] A computer program comprising: a setting function for setting a target route for the vehicle and a target speed value, which is a constant speed of the vehicle while traveling; An acquisition function for acquiring the position of the vehicle at the current time T1 and the yaw angle of the vehicle relative to the reference direction at the time T1; a point determination function that determines a target arrival point of the vehicle on the target route at time T2, which is later than time T1; a steering angle determination function that determines a target value of the steering angle until the vehicle moves to the target arrival point at time T2, using the curvature of the target route at the target arrival point at time T2, the difference between the position of the vehicle at time T1 and a reference point that is an intersection of the target route and a straight line extending from the position of the vehicle at time T1 in a direction perpendicular to the vehicle's longitudinal direction, and the reference point, and the difference between the tangential angle of the target route at the reference point and the yaw angle of the vehicle at time T1; a speed determination function that determines a rotation target value, which is a target value of the rotation speed of a drive motor of the vehicle, using the absolute value of the curvature of the reference point and the speed target value; This is realized by a computer, The speed determination function is In a section where the absolute value of the curvature of the reference point increases, the rotation target value is increased over time; In a section where the absolute value of the curvature of the reference point is constant, the rotation target value is decreased over time; a computer program that decreases and then increases the rotation target value over time in a section where the absolute value of the curvature at the reference point decreases; [Explanation of symbols]

[0067] 11...Settings section 12…Acquisition part 13...Location determination section 14...Steering angle determination unit 15...Decision Section 16…Communications Department 20...Sensor system 23...Camera 30...Steering control unit 31...Drive control unit 40...Input section 100...Path following system DR1: Vehicle longitudinal direction FT...Drive wheels RT…Steering wheel F yr ...lateral force F yr_e ...Estimated lateral force F yr_e_x ...vehicle longitudinal component MT: Drive motor Pc…Reference point Pf...Forecast point (target point) Pv…Current location SC1: Section where the absolute value of curvature increases SC2: Section where the absolute value of the curvature is constant SC3: Section where the absolute value of curvature decreases TR...Target route Ve…Vehicle ω * m …Drive motor target value t c …Control period

Claims

1. A vehicle path following system, comprising: a setting unit that sets a target route for the vehicle and a target speed value that is a constant speed of the vehicle while traveling; an acquisition unit that acquires a position of the vehicle at a current time T1 and a yaw angle of the vehicle with respect to a reference direction at the time T1; a point determination unit that determines a target arrival point of the vehicle on the target route at time T2 that is later than time T1; a steering angle determination unit that determines a target value of the steering angle until the vehicle moves to the target arrival point at time T2, using the curvature of the target route at time T2, the difference between the position of the vehicle at time T1 and a reference point that is an intersection between the target route and a straight line extending from the position of the vehicle at time T1 in a direction perpendicular to the vehicle's longitudinal direction, and the reference point, and the difference between the tangential angle of the target route at the reference point and the yaw angle of the vehicle at time T1; a speed determination unit that determines a rotation target value, which is a target value of the rotation speed of a drive motor of the vehicle, using the absolute value of the curvature of the reference point and the speed target value; Equipped with The speed determination unit In a section where the absolute value of the curvature of the reference point increases, the rotation target value is increased over time; In a section where the absolute value of the curvature of the reference point is constant, the rotation target value is decreased over time; A path tracking system that decreases and then increases the rotation target value over time in a section where the absolute value of the curvature of the reference point decreases.

2. 2. The path following system of claim 1, The point determination unit determines, as the target arrival point at time T2, a point on the target route that is a distance away from the position of the vehicle at time T1 that is a distance calculated by multiplying a previously identified steering device response delay time by the target speed value.

3. 3. A path tracking system according to claim 1 or 2, a speed determination unit that changes the rotation target value from decreasing to increasing when an absolute value of a vehicle longitudinal component of an estimated value of a lateral force generated on a steering wheel turns to decrease in a section where an absolute value of a curvature of the reference point decreases.

4. A method for path following a vehicle, comprising: a setting step of setting a target route for the vehicle and a target speed value that is a constant speed of the vehicle while traveling; an acquisition step of acquiring a position of the vehicle at a current time T1 and a yaw angle of the vehicle with respect to a reference direction at the time T1; a point determination step of determining a target arrival point of the vehicle on the target route at a time T2 that is later than the time T1; a steering angle determination process for determining a target value of the steering angle until the vehicle moves to the target point at time T2, using the curvature of the target route at time T2, the difference between the position of the vehicle at time T1 and a reference point, which is an intersection between the target route and a straight line extending from the position of the vehicle at time T1 in a direction perpendicular to the vehicle's longitudinal direction, and the reference point, and the difference between the tangential angle of the target route at the reference point and the yaw angle of the vehicle at time T1; a speed determination step of determining a rotation target value, which is a target value of the rotation speed of a drive motor of the vehicle, using the absolute value of the curvature of the reference point and the speed target value; Run The speed determining step includes: In a section where the absolute value of the curvature of the reference point increases, the rotation target value is increased over time; In a section where the absolute value of the curvature of the reference point is constant, the rotation target value is decreased over time; A path tracking method, wherein in a section where the absolute value of the curvature of the reference point decreases, the rotation target value is decreased over time and then increased.

5. A computer program comprising: a setting function for setting a target route for the vehicle and a target speed value, which is a constant speed of the vehicle while traveling; an acquisition function for acquiring the position of the vehicle at the current time T1 and the yaw angle of the vehicle relative to the reference direction at the time T1; a point determination function for determining a target arrival point of the vehicle on the target route at time T2, which is later than time T1; a steering angle determination function that determines a target value of the steering angle until the vehicle moves to the target arrival point at time T2, using the curvature of the target route at time T2, the difference between the position of the vehicle at time T1 and a reference point that is an intersection of the target route and a straight line extending from the position of the vehicle at time T1 in a direction perpendicular to the vehicle's longitudinal direction, and the reference point, and the difference between the tangential angle of the target route at the reference point and the yaw angle of the vehicle at time T1; a speed determination function that determines a rotation target value, which is a target value of the rotation speed of a drive motor of the vehicle, using the absolute value of the curvature of the reference point and the speed target value; This is realized by a computer, The speed determination function is In a section where the absolute value of the curvature of the reference point increases, the rotation target value is increased over time; In a section where the absolute value of the curvature of the reference point is constant, the rotation target value is decreased over time; a computer program that decreases and then increases the rotation target value over time in a section where the absolute value of the curvature at the reference point decreases;

Images