Path following control system

Abstract

To provide a route follow-up control system capable of improving route follow-up accuracy by suppressing the enlargement of a route follow-up error caused by the secondary delay of a steering device.SOLUTION: The route following control system includes a forklift F which is an automatic traveling machine and a ECU11 which is a control device for controlling the forklift F. The forklift F includes rear wheels 3 that are steered wheels whose steering angle can be changed, and a steering unit 5 as a steering device that changes the steering angle based on a control input value. The ECU11 acquires the steering angle and speed of the rear wheel 3 and the position and attitude angle of the forklift F, and calculates control inputs for the forklift F to travel along a predetermined target path based on an equation representing a model including both the dead time and the secondary delay of the steering unit 5.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present invention relates to a path following control system that causes an autonomous vehicle, such as an unmanned forklift, to travel along a predetermined target path. [Background technology]

[0002] BACKGROUND ART A path-following control system is known that causes an autonomous vehicle, such as an unmanned guided vehicle or an unmanned forklift that handles cargo in a warehouse, to travel along a predetermined target path based on the position and posture of the vehicle in the warehouse.

[0003] Patent Document 1 discloses a vehicle driving control device that suppresses deterioration of the tracking performance of trajectory tracking control. This vehicle driving control device includes a sensor that detects the driving state of the vehicle and a control device that performs trajectory tracking control to control the driving of the vehicle so that the vehicle follows a target trajectory. The trajectory tracking control includes a movement amount estimation process that estimates the movement amount of the vehicle during a delay compensation time based on sensor detection information that indicates the detection results by the sensor, a delay compensation process that corrects the deviation between the vehicle and the target trajectory based on the estimated movement amount so as to compensate for the control delay that is the time that represents the control delay of the trajectory tracking control, and a driving control process that controls the driving of the vehicle so as to reduce the above deviation (see [Claim 1]). [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Japanese Patent Application Publication No. 2020-179749 Summary of the Invention [Problem to be solved by the invention]

[0005] However, the configuration of Patent Document 1 cannot suppress the increase in path tracking error due to the second-order delay of the steering device. In other words, the control delay described in Patent Document 1 refers to a certain delay (so-called dead time) such as the calculation processing time and the information communication time (see paragraph

[0002] ), and does not take into account the second-order delay, which increases the faster the steering, resulting in a problem of not being able to improve path tracking accuracy.

[0006] The present invention has been made in consideration of the above circumstances, and an object of the present invention is to provide a path tracking control system that can improve path tracking accuracy by suppressing the increase in path tracking error due to secondary delay of the steering device. [Means for solving the problem]

[0007] In order to solve the above problem, the path following control system of the present invention is a path following control system comprising an autonomous vehicle and a control device that controls the autonomous vehicle, wherein the autonomous vehicle is equipped with steering wheels whose steering angle can be changed and a steering device that changes the steering angle based on a control input value, and the control device acquires the steering angle, the speed of the steering wheels, and the position and attitude angle of the autonomous vehicle, and calculates the control input value for the autonomous vehicle to travel along a predetermined target route based on an equation representing a model that includes both the dead time and second-order delay of the steering device, and inputs the control input value to the steering device, wherein the dead time is a constant delay time, and the second-order delay is a delay from when the control input value is calculated to when the steering device changes the steering angle based on the control input value, which includes the dead time and is a response delay that varies according to the rate of change of the control input value.

[0008] Furthermore, it is preferable that the control device calculates a reference point that is located on the target route and closest to the autonomous vehicle, and the equation is a nonlinear ordinary differential equation composed of: a first equation representing the time derivative of a parameter related to the reference point; a second equation representing the time derivative of a position error indicating the difference between the position of the autonomous vehicle and the position of the reference point; a third equation representing the time derivative of an attitude error indicating the difference between the attitude angle of the autonomous vehicle and the target attitude angle at the reference point; a fourth equation representing the time derivative of the steering angle; and a fifth equation representing the time derivative of the rate of change of the steering angle, and the control device calculates the control input value using nonlinear model predictive control based on the nonlinear ordinary differential equation.

[0009] Furthermore, it is preferable that the control device calculates a reference point that is located on the target route and closest to the autonomous vehicle, and the equation is a nonlinear ordinary differential equation composed of a first equation representing the derivative of a position error that indicates the difference between the position of the autonomous vehicle and the position of the reference point, a second equation representing the derivative of an attitude error that indicates the difference between the attitude angle of the autonomous vehicle and the target attitude angle at the reference point, a third equation representing the derivative of the steering angle, and a fourth equation representing the derivative of the rate of change of the steering angle, and that the control device calculates the control input value using nonlinear model predictive control based on the nonlinear ordinary differential equation.

[0010] Furthermore, it is preferable that the control device calculates a reference point that is located on the target route and closest to the autonomous vehicle, and the equation is a linear ordinary differential equation that linearizes a nonlinear ordinary differential equation composed of: a first equation representing the time derivative of a parameter related to the reference point; a second equation representing the time derivative of a position error indicating the difference between the position of the autonomous vehicle and the position of the reference point; a third equation representing the time derivative of an attitude error indicating the difference between the attitude angle of the autonomous vehicle and the target attitude angle at the reference point; a fourth equation representing the time derivative of the steering angle; and a fifth equation representing the time derivative of the rate of change of the steering angle, and that the control device calculates the control input value using linear model predictive control based on the linear ordinary differential equation.

[0011] Furthermore, it is preferable that the control device calculates a reference point that is located on the target route and closest to the autonomous vehicle, and the equation is a linear ordinary differential equation linearized from a nonlinear ordinary differential equation composed of: a first equation representing the derivative of a position error that indicates the difference between the position of the autonomous vehicle and the position of the reference point; a second equation representing the derivative of an attitude error that indicates the difference between the attitude angle of the autonomous vehicle and the target attitude angle at the reference point; a third equation representing the derivative of the steering angle; and a fourth equation representing the derivative of the rate of change of the steering angle, and that the control device calculates the control input value using linear model predictive control based on the linear ordinary differential equation.

[0012] Furthermore, it is preferable that the autonomous vehicle is equipped with a drive device that changes the speed of the steering wheels, which also serve as drive wheels, and that the control device controls the drive device to reduce the speed of the steering wheels when the calculated control input value is not within a predetermined range.

[0013] Furthermore, it is preferable that the control device restricts the driving of the autonomous vehicle when the calculated control input value is not within a predetermined range.

[0014] It is also preferable that the control device is mounted on the autonomous vehicle. [Effects of the Invention]

[0015] According to the present invention, it is possible to provide a path tracking control system that can suppress an increase in path tracking error due to a second-order delay element of a steering device and improve path tracking accuracy. [Brief explanation of the drawings]

[0016] [Figure 1] 1 is a schematic configuration diagram of an autonomous vehicle equipped with a path tracking control system according to an embodiment of the present invention. [Figure 2] FIG. 2 is a schematic diagram for explaining a model used in model predictive control according to the embodiment. [Figure 3] 4 is a flowchart showing a flow of path following control according to the embodiment; DETAILED DESCRIPTION OF THE INVENTION

[0017] A path tracking control system according to one embodiment of the present invention will be described with reference to the drawings. In this embodiment, the path tracking control system is configured by a forklift F, which is an autonomous vehicle, and an ECU (Electronic Control Unit) 11 mounted on the forklift F.

[0018] As shown in Figure 1, the forklift F is a reach forklift truck equipped with a fork 1 consisting of a pair of left and right claws 1A, 1B. The forklift F is an AGF (Automated Guided Forklift) that transports loads unmanned and travels autonomously along a predetermined target route r (see Figure 2).

[0019] The forklift F also has a pair of left and right front wheels 2A, 2B and a single rear wheel 3 as wheels that roll on the road surface for travel. The forklift F mainly travels backward, which is one side of the fore-and-aft direction X indicated by the arrow in the figure, except when picking up or placing a load. In this embodiment, the direction toward the tips of the forks 1 is defined as the forward direction, but the direction opposite to the direction toward the tips of the forks 1 (i.e., the direction toward the bumper of the forklift F) may also be defined as the forward direction.

[0020] The front wheels 2A, 2B are spaced apart in the left-right direction Y. The front wheels 2A, 2B are driven wheels that are not connected to a prime mover such as an electric motor, and are fixed wheels that roll in the front-rear direction X.

[0021] The rear wheels 3 are configured to have a larger diameter than the front wheels 2A, 2B. The rear wheels 3 are drive wheels that transmit power from a drive unit 4 (described later) to the road surface, and are also steerable wheels with a variable steering angle δ (see FIG. 2). In other words, the single rear wheel 3 serves as both a steerable wheel and a drive wheel. Changing the steering angle δ changes the rolling direction Dt (see FIG. 2) of the rear wheels 3.

[0022] The forklift F also includes a drive unit 4, a steering unit 5, a steering angle sensor 6, a wheel speed sensor 7, a laser scanner 8, an estimation device 9, a memory 10, and an ECU 11.

[0023] The drive unit 4 is a drive device that changes the speed (hereinafter referred to as "speed v") of the rear wheels 3. The drive unit 4 is composed of a traction motor, which is an electric motor that transmits power to the rear wheels 3, and a drive circuit for driving the traction motor.

[0024] The steering unit 5 is a steering device that changes the steering angle δ of the rear wheels 3 based on a control input value, which will be described later. The steering unit 5 is composed of a mechanical mechanism connected to a support mechanism for the rear wheels 3, a steering motor which is an electric motor that transmits power to the mechanical mechanism, and a drive circuit for driving the steering motor.

[0025] The steering angle sensor 6 is a steering angle detection unit for detecting the steering angle δ of the rear wheels 3. The steering angle sensor 6 detects, for example, the amount of movement of the mechanical mechanism or electric motor of the steering unit 5. Based on the amount of movement detected by the steering angle sensor 6, the steering angle δ of the rear wheels 3 is calculated.

[0026] The wheel speed sensor 7 is a speed detection unit for detecting the peripheral speed of the rear wheel 3 as the speed v. The wheel speed sensor 7 detects the rotational speed of the rear wheel 3 or a bearing (not shown) that supports the rear wheel 3. The speed v of the rear wheel 3 is calculated based on the rotational speed detected by the wheel speed sensor 7.

[0027] The laser scanner 8 and the estimation device 9 are a position detection unit for detecting the position of the forklift F (i.e., the vehicle itself) and an attitude angle detection unit for detecting the attitude angle θ (see FIG. 2) of the forklift F. In this embodiment, the position of the forklift F is the position of a vehicle reference point Vp (see FIG. 2) described later.

[0028] The laser scanner 8 is a LiDAR (Light Detection and Ranging) sensor that projects laser light around the forklift F and receives the reflected light to acquire environmental information around the forklift F. In other words, the laser scanner 8 is a distance measurement sensor that detects the distance to an object existing around the forklift F.

[0029] The estimation device 9 is a device that estimates the position and attitude angle θ of the forklift F based on the environmental information acquired by the laser scanner 8. The estimation device 9 performs SLAM (Simultaneous Localization And Mapping) processing that simultaneously creates an environmental map of the area around the forklift F and estimates the position and attitude angle θ of the forklift F on the environmental map.

[0030] The memory 10 is a storage unit that stores programs executed by the ECU 11 and information required for executing the programs. Specifically, the memory 10 stores information related to a target route r, information related to an instructed speed when traveling along the target route r, information related to a wheelbase L (see FIG. 2) that is the distance between the front wheels 2A, 2B and the rear wheels 3, and information related to a vehicle model that takes into account delays in the steering unit 5 (i.e., steering delays), etc.

[0031] The ECU 11 is a control device that controls the drive unit 4 and the steering unit 5 so that the forklift F travels along the target route r. That is, the ECU 11 controls the drive unit 4 so as to drive the rear wheels 3 based on a preset command speed, and controls the steering unit 5 based on a control input value (hereinafter referred to as "control input value u") calculated in a manner described below, thereby causing the forklift F to travel along the target route r.

[0032] In order to calculate the control input value u, the ECU 11 calculates the steering angle δ of the rear wheels 3 based on the output of the steering angle sensor 6, calculates the speed v of the rear wheels 3 based on the output of the wheel speed sensor 7, and acquires the position and attitude angle θ of the forklift F based on the output of the estimation device 9. The ECU 11 performs model predictive control based on the steering angle δ of the rear wheels 3, the speed v of the rear wheels 3, and the position and attitude angle θ of the forklift F acquired as described above, and controls the steering angle δ of the rear wheels 3 by inputting the calculated control input value u to the steering unit 5.

[0033] Model predictive control (MPC) is a technique for controlling a controlled object by performing calculations to solve an optimization problem at each control cycle based on a model of the controlled object and using the calculation results. The ECU 11 calculates a control input value u for the forklift F to travel along a target route r based on an equation representing a model including both the dead time and second-order delay of the steering unit 5. The dead time is a fixed delay time from when the ECU 11 calculates the control input value u until when the steering unit 5 changes the steering angle δ of the rear wheels 3 based on the control input value u. The second-order delay is a delay from when the ECU 11 calculates the control input value u until when the steering unit 5 changes the steering angle δ of the rear wheels 3 based on the control input value u, and is a response delay that varies depending on the rate of change of the control input value u. In other words, the second-order delay varies depending on the steering angular velocity ξ (described later) and increases as the steering is performed faster (in other words, decreases as the steering is performed slower).

[0034] As an equation representing a model including both the dead time and second-order delay of the steering unit 5, any of the following ordinary differential equations (Equation 1) to (Equation 4) can be used. (Equation 1): A nonlinear ordinary differential equation where the speed v of rear wheel 3 is an arbitrary speed. (Equation 2): A nonlinear ordinary differential equation assuming that the speed v of rear wheel 3 is constant (Equation 3): A linear ordinary differential equation that linearizes a nonlinear ordinary differential equation in which the speed v of rear wheel 3 is an arbitrary speed. (Equation 4): A linear ordinary differential equation obtained by linearizing the nonlinear ordinary differential equation when the speed v of rear wheel 3 is assumed to be constant.

[0035] That is, the ECU 11 can solve an optimization problem based on any one of (Equation 1) to (Equation 4) and calculate the control input value u. When the ECU 11 calculates the control input value u based on (Equation 1) or (Equation 2), the ECU 11 calculates the control input value u by nonlinear model predictive control using, for example, the C / GMRES (Continuation / Generalized Minimum RESidual method). When the ECU 11 calculates the control input value u based on (Equation 3) or (Equation 4), the ECU 11 calculates the control input value u by, for example, linear predictive model control using a continuous LQR (Linear Quadratic Regulator) or linear model predictive control using a discontinuous LQR after performing difference approximation (Euler method).

[0036] As an optimization problem, ECU 11 solves a minimization problem that analyzes the state in which the value of the evaluation function is minimized in order to reduce the position error (hereinafter referred to as "position error e") and attitude error (hereinafter referred to as "attitude error φ") described below.

[0037] With reference to FIG. 2, parameters related to the vehicle model according to the embodiment will be described. The lateral center axis C of the forklift F is an axis parallel to the longitudinal direction X of the forklift F, and on the lateral center axis C are located a rear wheel reference point Tp indicating the position of the rear wheel 3 and a vehicle reference point Vp indicating the position of the forklift F. The wheelbase L of the forklift F represents the distance from the rear wheel reference point Tp to the vehicle reference point Vp.

[0038] The attitude angle θ of the forklift F is the angle between the x-axis that defines the two-dimensional Cartesian coordinate system and the left-right central axis C of the forklift F. The unit vector E1 that represents the forward / backward direction X of the forklift F and the unit vector E2 that represents the left-right direction Y of the forklift F can be expressed using the attitude angle θ as column vectors defined by the following equations (1) and (2).

number

number

[0039] The vehicle reference point Vp, which indicates the position of the forklift F, is located between the front wheels 2A and 2B, and the rear wheel reference point Tp and the vehicle reference point Vp satisfy the following formula (3). p ” is a column vector indicating the coordinates of the rear wheel reference point Tp, and similarly, “V p " is a column vector indicating the coordinates of the vehicle reference point Vp.

number

[0040] By substituting the time derivative of the vehicle reference point Vp, expressed in the following equation (4), and the time derivative of the unit vector E1, "dE1 / dt = E2 dθ / dt," into the equation obtained by time differentiating both sides of equation (3), the following equation (5) is obtained. Note that equation (4) is an equation obtained from the constraint that the front wheels 2A, 2B move only in a direction parallel to the fore-and-aft direction X, assuming that the front wheels 2A, 2B do not slip on the road surface. "V" in the equation represents the traveling speed V of the forklift F, which is the moving speed of the vehicle reference point Vp.

number

number

[0041] As shown in equation (5), the time derivative of the rear wheel reference point Tp can be expressed as a linear combination of unit vectors E1 and E2. Therefore, using the steering angle δ of the rear wheels 3, which is the angle between the rolling direction Dt of the rear wheels 3 and the lateral center axis C, the relationship between the speed v of the rear wheels 3, which represents the time derivative of the wheel reference point Tp, and the traveling speed V of the forklift F, which represents the time derivative of the vehicle reference point Vp, can be expressed by the following equation (6).

number

[0042] Therefore, the time derivative of the vehicle reference point Vp can be expressed by the following equations (7) and (8) obtained by substituting equation (6) into equation (4). In the equations, "x" is the x-coordinate of the vehicle reference point Vp, and "y" is the y-coordinate of the vehicle reference point Vp.

number

number

[0043] Furthermore, the time derivative of the attitude angle θ can be expressed by the following equation (9) using the velocity v of the rear wheel 3.

number

[0044] The time differential of the steering angle δ is defined as the steering angle velocity ξ, which represents the rate of change of the steering angle δ per minute time, and is defined by the following equation (10).

number

[0045] The time derivative of the steering angular velocity ξ can be expressed by the following formula (11). delay " represents the past control input value u (past command steering angle) to the steering unit 5, and when the steering angle δ and steering angular velocity ξ at any time t ared before (i.e., time t-τ d ) is the control input value u at time t. That is, the control input value u calculated at time t is d This means that the steering angle δ is reflected in the delay " is the dead time of the steering unit 5 (constant time τ d ), and the equations (10) and (11) include the dead time τ d and second-order delay. In equation (11), the second-order delay is expressed as the time derivative of the steering angular velocity ξ (rate of change of the steering angle), and in this embodiment, the equation representing the second-order delay includes dead time.

number

[0046] In the formula, "k", "m", and "γ" are constants that characterize the relationship between the steering angle δ and the past control input value u, and these constants can be set based on road tests of the forklift F.

[0047] From the above, the following equation (12) is obtained by combining the equations (7) to (11) as an equation showing the time evolution of the position and steering angle δ of the forklift F.

number

[0048] The position error e indicates the distance between the vehicle reference point Vp and the reference point Rp, i.e., the difference between the position of the forklift F and the position of the reference point Rp, and the attitude error φ indicates the difference between the attitude angle θ of the forklift F and the tangent angle θr, which is the target attitude angle at the reference point Rp. The reference point Rp is a point located on the target route r and is the point closest to the vehicle reference point Vp. The tangent angle θr is the angle between the x-coordinate axis and the tangent Tr of the target route r at the reference point Rp.

[0049] The unit vector e1 representing the direction parallel to the tangent line Tr and the unit vector e2 representing the direction perpendicular to the tangent line Tr can be expressed as column vectors using the tangent angle θr, in the same way as the unit vectors E1 and E2 above.

[0050] The derivation method of (Equation 1) to (Equation 4), which describe the time evolution for path tracking control, will be explained in detail.

[0051] Since it is inconvenient to directly handle the relationship between the position and attitude angle θ of the forklift F and the target route r, the model is transformed into a form based on the target route r. First, since analysis is inconvenient if the domain of the parameter is set to [0, 1], point r(l) on the target route r is expressed as a parameter, which is the length l along the target route r from the starting point of the target route r. In other words, point r(l) represents a point that has progressed "l" distance from the starting point of the target route r along the target route r, and the domain of "l" can be expressed as [0, Lr] using the total length Lr of the target route r. In addition, in the following formulas, the curvature κ of the target route r at point r(l) is expressed as "κ(l)".

[0052] Using the length l from the start point of the target route r to the reference point Rp closest to the forklift F, the progress s is defined as a parameter related to the reference point Rp. The progress s is defined by the following formula (13). "r(l)" in the formula is a column vector indicating the coordinates of point r(l) on the target route r. The pair of vertical bars in the formula represent the Euclidean norm.

number

[0053] Furthermore, the position error e, which indicates the distance between the vehicle control point Vp and the reference point Rp, can be expressed as the inner product of the two-dimensional vector shown in the following formula (14). "r(s)" in the formula is a column vector indicating the coordinates of the reference point Rp, according to the definition of the progress s above.

number

[0054] Moreover, the attitude error φ, which indicates the difference between the attitude angle θ and the tangent angle θr, can be expressed by the following equation (15).

number

[0055] Next, consider the time evolution of the progress s, position error e, and attitude error φ. First, transform Equation (13) into an equation that is easy to differentiate. When the function of length l is defined by Equation (16) below, its derivative can be expressed by Equation (17) below.

number

number

[0056] Since equation (16) has a minimum value when l = s, the derivative equation (17) becomes 0 when l = s. Therefore, the following equation (18) is obtained.

number

[0057] By differentiating both sides of equations (18), (14), and (15) and substituting equation (12), the following equations (19) to (21) are obtained.

number

number

number

[0058] From the above, combining (19) to (21) and equations (10) and (11), we obtain the following equation (22).

number

[0059] The equation shown in formula (22) is the above-mentioned (Equation 1), and is a nonlinear ordinary differential equation composed of the first equation representing the time derivative of the progress s with respect to the reference point Rp, the second equation representing the time derivative of the position error e, the third equation representing the time derivative of the attitude error φ, the fourth equation representing the time derivative of the steering angle δ, and the fifth equation representing the time derivative of the steering angular velocity ξ, which is the rate of change of the steering angle δ. This nonlinear ordinary differential equation is linearized around a solution described later to form the above-mentioned (Equation 3).

[0060] Furthermore, if the speed v does not change or if the change in speed v is sufficiently small, the dimension of the equation can be reduced by changing the independent variable of time t to progress s. In other words, if we assume that the speed v is constant, the right-hand side of the first equation in equation (21) does not include time t, so by substituting the following equation (23) into the time derivative on the left-hand side of equation (22), we obtain the following equation (24).

number

number

[0061] The equation shown in formula (24) is the above-mentioned (Equation 2), and is a nonlinear ordinary differential equation composed of formula 1 representing the derivative of the position error e, formula 2 representing the derivative of the attitude error φ, formula 3 representing the derivative of the steering angle δ, and formula 4 representing the derivative of the steering angular velocity ξ, which is the rate of change of the steering angle δ. A linear ordinary differential equation that linearly approximates this nonlinear ordinary differential equation becomes the above-mentioned (Equation 4).

[0062] Next, linear approximation of nonlinear ordinary differential equations will be explained. When a target route r and a command speed according to progress s are given, we consider whether a forklift F positioned on the target route r can travel without deviating from the target route r. This is done by changing the input value u corresponding to a solution in which the position error e and the attitude error φ are 0 at all times t from a state in which the position error e and the attitude error φ are 0 at a given progress s0. * This can be seen as a problem that requires

[0063] If the time when the forklift F reaches point r(l) on the target route r after starting to travel is "T," there is only one solution where the position error e and attitude error φ are 0 at all times t that satisfy 0≦t≦T, provided that the curvature κ(s) is twice differentiable. If the attitude error φ is 0, the time derivative of the position error e shown in equation (20) is 0, and so the position error e does not change from 0. Furthermore, when the position error e is 0, the steering angle δ that makes the right-hand side of equation (21) 0 can be expressed by the following equation (25).

number

[0064] Therefore, if the forklift F is located at any point r(l) on the target route r, the steering angle δ that allows it to remain on the target route r is determined only by that position. By substituting this δ(t) into equation (19), the first equation of the following equation (26) is obtained. d "(t)" represents the command speed at time t. Due to the existence and uniqueness of the solution, if the initial value of progress s is set to "s0", the following equation (27) can be obtained using the solution to the initial value problem shown in equation (25). The right-hand side of the first equation in equation (27) is the solution to the initial value problem.

number

number

[0065] In addition, by substituting formula (25) into formulas (10) and (11), the input value u expressed by formula (28) below is obtained. * is obtained.

number

[0066] Therefore, if the target route r and the command speed are given, a unique traveling motion of the forklift F is determined. Hereinafter, the trajectory (i.e., the solution) determined by the target route r and the command speed is called the command trajectory.

[0067] Equation (22) is summarized and expressed as the following equation (29).

number

[0068] When the difference z between the solution evolving over time on the target path r is defined by the following equation (30), "w = uu * ", the time evolution of the difference z can be expressed by the following equation (31).

number

number

[0069] When w=0, the ordinary differential equation shown in Equation (31) has an equilibrium point at the origin. The linear approximation around this equilibrium point is given by Equation (32) below. x " is the differential of "g" with respect to "x", and "g u " is the differential of "g" with respect to "u".

number

[0070] Therefore, by linearly approximating the nonlinear ordinary differential equation of Equation (22) at the equilibrium point, a linear ordinary differential equation can be obtained that linearizes the nonlinear ordinary differential equation when the speed v of the rear wheel 3 is any speed. Also, by linearly approximating the nonlinear ordinary differential equation of Equation (24) at the equilibrium point, a linear ordinary differential equation can be obtained that linearizes the nonlinear ordinary differential equation when it is assumed that the speed v of the rear wheel 3 is constant.

[0071] Note that when the target route r is defined by a spline or the like, the relationship between the length l along the target route r from the starting point of the target route r (i.e., the distance from the starting point) and the curvature κ cannot be easily calculated. Therefore, a parameter other than the length l is used. The progress s can be expressed using the parameter τ as shown in the following equation (33).

number

[0072] The time derivative of the progress s shown in the first equation of equation (26) can be expressed by the following equation (34), and therefore the time derivative of the parameter τ can be expressed by the following equation (35).

number

number

[0073] By using the equation using the parameter τ above instead of the equation (26), it is possible to calculate even a target route r expressed in a parameter format such as a spline.

[0074] The flow of path following control using model predictive control by the ECU 11 will be described with reference to Fig. 3. Steps S1 to S4 shown in Fig. 3 are repeatedly performed at short control intervals while the forklift F is traveling. That is, a series of processes from steps S1 to S4 is regarded as one step, and this step is repeatedly performed.

[0075] First, the ECU 11 acquires the vehicle state (step S1). Specifically, the ECU 11 acquires the steering angle δ of the rear wheels 3 based on the output of the steering angle sensor 6, acquires the speed v of the rear wheels 3 based on the output of the wheel speed sensor 7, and acquires the vehicle reference point Vp, which is the position of the forklift F, and the attitude angle θ from the output of the estimation device 9.

[0076] Next, the ECU 11 calculates a coefficient matrix constituting an evaluation function based on the vehicle state acquired in step S1 (step S2). Specifically, the ECU 11 calculates the components of the coefficient matrix based on the steering angle δ, speed v, vehicle reference point Vp, attitude angle θ, and reference point Rp and tangent angle θr identified from the vehicle reference point Vp, etc., acquired in step S1. The evaluation function is a function that is set in advance based on a model expressed by any of the above (Equation 1) to (Equation 4). A known method (for example, the method described in JP 2023-137764 A) can be used to calculate the coefficient matrix.

[0077] Next, the ECU 11 calculates a control input value u that minimizes the evaluation function specified by the coefficient matrix calculated in step S2 (step S3). In model predictive control, when calculating the control input value u, a future vehicle state (i.e., 1 to N steps ahead) is predicted, and the control input value u is calculated taking into account the current position and attitude angle θ of the forklift F and the future predicted position and predicted attitude angle.

[0078] Then, the ECU 11 controls the steering unit 5 using the control input value u calculated in step S3 (step S4). That is, the ECU 11 controls the steering angle δ of the rear wheels 3 via the steering unit 5 by inputting the control input value u to the steering unit 5 as a command steering angle.

[0079] In this embodiment, the following effects are obtained. (1) The ECU 11 (control device) acquires the steering angle δ and speed v of the rear wheels 3 (steered wheels) as well as the position and attitude angle θ of the forklift F (autonomous vehicle), and calculates a control input value u for the forklift F to travel along a predetermined target route r based on an equation representing a model including both the dead time and second-order delay of the steering unit 5 (steering device). With this configuration, the steering unit 5 is controlled taking into account not only the dead time, which is a constant response delay of the steering unit 5, but also the second-order delay of the steering unit 5, which is a response delay that varies depending on the rate of change of the control input value u. Therefore, it is possible to suppress the increase in path tracking error due to the second-order delay of the steering unit 5, and improve path tracking accuracy.

[0080] (2) The ECU 11 calculates the control input value u by nonlinear model predictive control based on (Equation 1), which is a nonlinear ordinary differential equation composed of Equations 1 to 5. This configuration can improve the path tracking accuracy compared to a configuration in which the control input value u is calculated based on (Equation 2).

[0081] (3) The ECU 11 calculates the control input value u by nonlinear model predictive control based on (Equation 2), which is a nonlinear ordinary differential equation composed of Equations 1 to 4. This configuration can reduce the calculation load compared to a configuration in which the control input value u is calculated based on (Equation 1).

[0082] (4) The ECU 11 calculates the control input value u by linear model predictive control based on (Equation 3), which is a linear ordinary differential equation obtained by linearizing the nonlinear ordinary differential equation constituted by Equations 1 to 5. This configuration can reduce the calculation load compared to a configuration in which the control input value u is calculated based on (Equation 1), and can improve the path tracking accuracy compared to a configuration in which the control input value u is calculated based on (Equation 4).

[0083] (5) The ECU 11 calculates the control input value u by linear model predictive control based on a linear ordinary differential equation obtained by linearizing a non-linear ordinary differential equation composed of the first to fourth equations (Equation 4). According to this configuration, the computational load can be reduced compared to the configuration of calculating the control input value u based on (Equation 3).

[0084] (6) The ECU 11 is mounted on the forklift F. According to this configuration, the forklift F can calculate the control input value u representing the commanded steering angle without receiving the commanded steering angle from the outside.

[0085] The present invention is not limited to the above embodiment, and the above configuration can also be changed. For example, it can be implemented by changing as follows, or can be implemented by combining the following changes.

[0086] · When the calculated control input value u is not included in a predetermined range, that is, when the control input value u does not satisfy the constraints of the steering unit 5 (for example, -π / 2 < u < π / 2) (in other words, when it is out of the constraints), the drive unit 4 may be controlled to reduce the speed v of the rear wheels 3. According to this configuration, it is possible to intervene in the control of the drive unit 4 based on the commanded speed and improve the path following accuracy.

[0087] · In the above embodiment, the ECU 11 calculates the control input value u based on the steering angle δ, the speed v, the position and the attitude angle θ of the forklift F obtained from the outputs of the steering angle sensor 6, the wheel speed sensor 7, and the estimation device 9. However, when the forklift F is stopped, the control input value u * may be calculated based on those predicted values. The predicted value of the steering angle δ can use the control input value u calculated as the commanded steering angle. Also, the predicted value of the speed v can use a preset commanded speed, and the predicted values of the position and the attitude angle θ of the forklift F can be calculated from a preset target path r and its tangent Tr.

[0088] That is, before the forklift F starts moving, the ECU 11 calculates the control input value u based on the formula (28). * and calculate the control input value u * is not within a predetermined range (i.e., when the constraint of the steering angle δ is not satisfied), it may be determined that there is an abnormality that makes it impossible to perform path following control, and the drive unit 4 may be controlled to restrict the traveling of the forklift F. According to this configuration, by prohibiting the traveling of the forklift F, it is possible to avoid the forklift F from stopping after starting to travel due to an inability to perform path following control.

[0089] The equation representing the model including both the dead time and second-order delay of the steering unit 5 may be modified as appropriate as long as it substantially represents the content of the equation described in the embodiment.

[0090] The forklift may be an AGF other than a laser-guided type. For example, the forklift may be a gyro-guided AGF equipped with a gyro sensor, or a magnetic-guided AGF equipped with a magnetic sensor.

[0091] The forklift is not limited to a reach forklift, but may be a counterbalance forklift. The present invention may also be applied to an automatic vehicle other than a forklift (for example, an automatic guided vehicle).

[0092] The control device that constitutes the path tracking control system does not have to be installed on the autonomous vehicle. For example, a control device installed outside the autonomous vehicle may be configured to communicate wirelessly with the autonomous vehicle and remotely control the steering device of the autonomous vehicle. [Explanation of symbols]

[0093] 1 fork 2A,2B Front wheel 3 Rear wheels (drive wheels, steering wheels) 4 Drive unit (drive device) 5 Steering unit 11 ECU (control unit) F Forklift (autonomous vehicle) Rp reference point Vp Vehicle reference point r Target Route δ Rudder angle θ attitude angle

Claims

1. A path following control system including an automated driving vehicle and a control device that controls the automated driving vehicle, the autonomous vehicle includes a steering wheel with a changeable steering angle and a steering device that changes the steering angle based on a control input value; the control device acquires the steering angle, the speed of the steered wheels, and the position and attitude angle of the autonomous vehicle, and calculates the control input value for the autonomous vehicle to travel along a predetermined target route based on an equation representing a model including both a dead time and a second-order delay of the steering device; The dead time is a constant delay time, The secondary delay is a delay from when the control input value is calculated until when the steering device changes the steering angle based on the control input value, and includes the dead time, and is a response delay that varies depending on the rate of change of the control input value. A path tracking control system characterized by:

2. The control device calculates a reference point that is located on the target route and closest to the autonomous driving vehicle; The equation is: a first equation representing the time derivative of the parameter with respect to the reference point; a second equation representing a time derivative of a position error indicating a difference between the position of the autonomous driving vehicle and the position of the reference point; a third equation representing a time derivative of an attitude error indicating a difference between the attitude angle of the autonomous vehicle and a target attitude angle at the reference point; A fourth equation representing the time derivative of the steering angle; a fifth equation representing a time derivative of the rate of change of the steering angle, The control device calculates the control input value by nonlinear model predictive control based on the nonlinear ordinary differential equation.

2. The path tracking control system according to claim 1.

3. The control device calculates a reference point that is located on the target route and closest to the autonomous driving vehicle; The equation is: a first equation representing a differential of a position error indicating a difference between the position of the autonomous driving vehicle and the position of the reference point; a second equation representing a differential of an attitude error indicating a difference between an attitude angle of the autonomous vehicle and a target attitude angle at the reference point; A third equation representing the differential of the steering angle; a fourth equation representing the derivative of the rate of change of the steering angle, The control device calculates the control input value by nonlinear model predictive control based on the nonlinear ordinary differential equation.

2. The path tracking control system according to claim 1.

4. The control device calculates a reference point that is located on the target route and closest to the autonomous driving vehicle; The equation is: a first equation representing the time derivative of the parameter with respect to the reference point; a second equation representing a time derivative of a position error indicating a difference between the position of the autonomous driving vehicle and the position of the reference point; a third equation representing a time derivative of an attitude error indicating a difference between the attitude angle of the autonomous vehicle and a target attitude angle at the reference point; A fourth equation representing the time derivative of the steering angle; a linear ordinary differential equation obtained by linearizing a nonlinear ordinary differential equation configured by the above equation; and a fifth equation representing a time derivative of the rate of change of the steering angle, The control device calculates the control input value by linear model predictive control based on the linear ordinary differential equation.

2. The path tracking control system according to claim 1.

5. The control device calculates a reference point that is located on the target route and closest to the autonomous driving vehicle; The equation is: a first equation representing a differential of a position error indicating a difference between the position of the autonomous driving vehicle and the position of the reference point; a second equation representing a differential of an attitude error indicating a difference between an attitude angle of the autonomous vehicle and a target attitude angle at the reference point; A third equation representing the differential of the steering angle; a fourth equation representing the derivative of the rate of change of the steering angle; and a linear ordinary differential equation obtained by linearizing the nonlinear ordinary differential equation configured by the fourth equation, The control device calculates the control input value by linear model predictive control based on the linear ordinary differential equation.

2. The path tracking control system according to claim 1.

6. the autonomous vehicle includes a drive device that changes the speed of the steering wheels that also serve as drive wheels; When the calculated control input value is not within a predetermined range, the control device controls the drive device to reduce the speed of the steered wheels.

2. The path tracking control system according to claim 1.

7. The control device restricts the traveling of the autonomous vehicle when the calculated control input value is not within a predetermined range.

2. The path tracking control system according to claim 1.

8. The control device is mounted on the autonomous vehicle.

8. The path tracking control system according to claim 1, wherein:

Images