A path planning and control method for an unmanned formula racing car

Abstract

The present invention discloses a path planning and control method for an unmanned formula racing car, which mainly includes mapping parameters of an unmanned racing car; structural analysis and model establishment of an unmanned racing car; mathematical modeling of a track and a racing car; setting constraints and solving targets according to the performance of the track and the racing car; solving the optimal curve track using the optimal control theory; discretizing the planned track; planning each discrete point using model predictive control according to the constraints; solving the vehicle control rate, sending the steering wheel angle and pedal opening; the actuator executes the control rate, and the unmanned racing car moves according to the planned path and control rate. The algorithm of the present invention can plan the optimal track line of a given track so that the racing car can achieve the minimum lap time and can exert the vehicle limit, and provides a path following algorithm with strong anti-interference and adaptability, which can be applied to the fields of racing car design, racing car parameter selection, driver training, etc.

Description

Technical Field

[0001] The invention belongs to the technical field of driverless racing cars, and in particular relates to a path planning and control method for an driverless formula racing car. Background Art

[0002] Since the beginning of the new century, with the booming development of network technology and artificial intelligence, driverless and intelligent driving have become increasingly important. As one of the important branches, driverless vehicles have attracted the research of many experts and scholars. Reducing traffic accidents caused by drivers due to fatigue driving or other improper driving behaviors and saving countless lives are important functions of driverless vehicles; and driverless vehicles can liberate drivers, improve travel experience, reduce driving fatigue, and enhance driving pleasure; in addition, driverless vehicles can also be combined with other Internet products such as the Internet of Things and big data to further promote the progress of the Internet. The development and promotion of driverless cars are of great significance in ensuring driving safety, building urban smart rail transit, and improving the traffic capacity of drivers. As a platform for the development and innovation of automotive technology and a compass of the automotive industry, racing sports have also followed the trend of the times and begun to move towards intelligent roads; path planning is at the upper level of the driverless system and is one of the most important parts of the driverless system. Therefore, path planning for the track is crucial for driverless events.

[0003] Path planning algorithms include traditional graph search methods, A* algorithms, grid decoupling methods, artificial potential field methods, and optimal curve methods. Graph search methods are divided into breadth-first and depth-first. The disadvantage of breadth-first graph search is that it consumes a lot of memory when the tree level is deep and the number of child nodes is large. Excessive memory consumption is easy to cause the host to crash for unmanned racing cars equipped with many sensors, resulting in vehicle out of control; breadth-first search is suitable for nodes with a small number of child nodes and the tree level is not too deep; the disadvantage of depth-first search is that it is difficult to find the optimal solution, and only solutions can be found. Its advantage is that it consumes less memory, which overcomes the disadvantages of breadth-first search just mentioned. Unmanned racing can decide the outcome in a few seconds, so only the optimal solution can meet the needs of unmanned racing, and depth-first graph search algorithms are often difficult to meet. The A* algorithm requires the establishment of a grid map, but the track cannot be accurately and reasonably divided using a grid map. Using the A* algorithm will reduce the utilization rate of the track by the car. The disadvantage of the grid decoupling method is that if the resolution is high, the computing resources and memory requirements are high, which is difficult to meet the requirements of high-speed calculations in racing cars. The disadvantage of the artificial potential field method is that it may fall into the oscillation point of the local minimum, and it may not be able to reach the end point when obstacles are close to the end point.

[0004] At present, common path tracking algorithms mainly include: path tracking control based on roads and geometric figures, such as pure tracking control (Pursuit) and Stanley control (Stanley). Since these two algorithms are actually pure geometric tracking methods based on kinematic models, they do not consider the dynamic model of the car and cannot maximize the performance of the car. In addition, these two algorithms are offline control rates and have poor anti-interference capabilities. The actual performance of the algorithm is poor in high-speed environments.

[0005] The algorithm of the present invention does not need to establish a grid map, and fully utilizes the entire track; at the same time, the kinematic and dynamic models of the car are considered to maximize the performance of the car; the model predictive control is used for real-time rolling calculation, the calculation cost is low, the online control rate is generated, and the anti-interference ability is enhanced. The car's ability in path planning and path following is improved, and the lap time is reduced; and the planned optimal route can also act on the car design link, and guide the selection of car parameters, such as tire type, transmission ratio and aerodynamics; and the optimal path is used to train the car driver to improve the driving ability of the car driver; the planned optimal path can also be used for deep learning and neural network processing, so as to obtain appropriate weights for autonomous driving learning. Summary of the invention

[0006] The purpose of the present invention is to solve the above technical defects and provide a path planning and control method for an unmanned formula racing car. The algorithm can make full use of the entire track, maximize the performance of the racing car, improve the racing car's ability in path planning and path following, and reduce lap time; and the planned optimal route can be applied to racing car design, racing car driver training and other aspects.

[0007] The technical solution adopted by the present invention to solve the above-mentioned deficiencies in the prior art is:

[0008] A path planning and control method for an unmanned formula racing car comprises the following steps:

[0009] S1 maps the parameters of driverless racing cars;

[0010] Structural analysis and model building of S2 driverless racing car;

[0011] S3 mathematically models the track and the car;

[0012] S4 sets constraints and solution goals based on the track and car performance;

[0013] S5 uses optimal control theory to solve the optimal curve track;

[0014] S6 discretizes the planned track;

[0015] S7 uses model predictive control to plan each discrete point according to the constraints;

[0016] S8 solves the vehicle control rate and sends the steering wheel angle and pedal opening;

[0017] The S9 actuator executes the control rate, and the driverless car moves according to the planned path and control rate.

[0018] In step S1, the surveying parameters include the vehicle curb weight m, the center of mass height cog, the wheelbase l, the distance a between the center of mass and the front axle, the wheel rolling radius, the front axle track w1, the rear axle track w r , rolling resistance coefficient, vehicle width b, vehicle length L, yaw inertia, front axle lift coefficient, rear axle lift coefficient, front axle braking force distribution coefficient, rear axle braking force distribution coefficient and vehicle air resistance coefficient.

[0019] In step S2, in order to give full play to the dynamics of the racing car, it is necessary to focus on studying the tire performance and use the magic formula for analysis. The common form of the magic formula is as follows:

[0020] Y(x)=Dsin[Carctan{Bx-E(Bx-arctan(Bx))}]

[0021] Wherein, B is the curve stiffness factor, C is the curve shape factor, D is the curve peak factor, and E is the curve curvature factor. When x represents the tire's side slip angle or longitudinal slip rate, Y(x) can be the lateral force, the self-aligning moment, or the longitudinal force.

[0022] By analyzing the tires from the combined longitudinal force under steady-state conditions, the combined lateral force under steady-state conditions, and the combined self-aligning torque under steady-state conditions, the magic formula coefficients of the front and rear wheels can be obtained. The magic formula coefficients include the front wheel tire B coefficient, the front wheel tire C coefficient, the load dependence of the front tire coefficient D, the front wheel tire E coefficient, the rear wheel tire B coefficient, the rear wheel tire C coefficient, the load dependence of the rear tire coefficient D, and the rear wheel tire E coefficient.

[0023] In step S3, the track and the car are mathematically modeled using a dual-track seven-degree-of-freedom vehicle kinematic model, where the vehicle's longitudinal, lateral and yaw movements and the rotational movement of the four tires constitute seven degrees of freedom. For the dual-track vehicle model, the following can be obtained:

[0024] speed:

[0025]

[0026] Acceleration:

[0027]

[0028] Yaw:

[0029]

[0030] Among them, v x Longitudinal speed, v y Lateral velocity, ω z Yaw angular velocity, a x Longitudinal acceleration, a y Lateral acceleration, δ front wheel turning angle, m vehicle mass, a distance from center of mass to front axle, b distance from center of mass to rear axle, F xij (fl, fr, rl, rr) longitudinal force on each wheel, F yij (fl, fr, rl, rr) lateral force on each wheel, I z Moment of inertia, T f Front wheelbase, T r Rear axle wheelbase.

[0031] In step S4, the trajectory solution problem of the rigid body system is considered to minimize the time required to complete a given trajectory; the starting point and the end point are fixed, the trajectory boundary constraints are rigid constraints, the boundaries of the control quantity and the state quantity are specified, and the change of the state quantity is a flexible constraint.

[0032] The objective function is:

[0033] Where x(t f ) is the vehicle state at the end moment, x(t) is the real-time state of the vehicle during driving, and u(t) is the real-time control quantity of the vehicle during driving.

[0034]

[0035] u={Δ,f d ,f b}

[0036] Where v is the vehicle speed, β is the vehicle heading angle, is the angle between the vehicle head and the x-axis of the world coordinate system, n is the tangential acceleration of the vehicle's driving path, θ is the angle between the lane coordinate system and the world coordinate system, Δ is the steering wheel angle, and f d is the vehicle braking torque, f b is the vehicle acceleration torque.

[0037] The track constraints are:

[0038] Where N is the track width, since we have assumed that the widths of the left and right sides of the track centerline are equal, That is half the track width, b is the vehicle width, and a safety distance b is set safeIt is convenient to adjust the aggressiveness of the vehicle's driving.

[0039] The control rate constraint is:

[0040] Where v is the vehicle speed, Δ is the steering wheel angle, and f b is the vehicle acceleration torque, f d is the vehicle braking torque, m is the vehicle mass, is the steering wheel angular velocity, R is the radius the vehicle is allowed to pass, and a is the vehicle acceleration.

[0041] Vehicle dynamics constraints:

[0042]

[0043] ρ=ρ(α,V)

[0044] Where ρ is the radius of the acceleration circle, a x and a y represents the lateral acceleration and longitudinal acceleration, α is a x and a y The angle V is the vehicle speed.

[0045] In step S5, the interior point method is used to solve the nonlinear optimization problem, and the objective function is rewritten in the form of the interior point method:

[0046]

[0047] Among them, I_ is the barrier term of the interior point method, which is used to prevent the iterative solution from going out of the feasible domain. When it is within the constraint range, the I_ function value is 0; when it is outside the constraint range, the I_ function value is infinite.

[0048] In step S5, the interior point method is used to solve the optimal track line within the constraint range of step S4.

[0049] In step S6, the obtained optimal track line is discretized with a discrete step length of 0.2 m, and the continuous track is discretized into multiple points containing vehicle state information and control information.

[0050] In step S7, a cost function consisting of six aspects, namely, trajectory error, heading error, steering error, speed loss, turning angle change rate and speed change rate, is constructed.

[0051] The cost function expression is:

[0052]

[0053] ω cte ,ω epsi ,ω δ ,ω v,ω rates and They are the weight coefficients of six aspects, ω cte ,ω epsi ,ω δ ,ω v ,ω rates and They are the weight coefficients of six aspects, cte i Represents the vehicle displacement deviation, epsi i Represents the vehicle heading angle deviation, δ i Represents the vehicle turning angle deviation, v i is the speed of the vehicle at time i, v ref is the vehicle reference speed, v i -v ref represents the speed change, δ fi+1 is the turning angle of the vehicle at time i+1, δ fi is the turning angle of the vehicle at time i, δ fi+1 -δ fi Represents the change in angle, v i+1 is the speed of the vehicle at time i+1, v i is the speed of the vehicle at time i, v i+1 -v i Represents the change in speed.

[0054] The minimum value of the cost function is solved by a computer, and the solution to the minimum value is the optimal control rate.

[0055] The present invention has the following beneficial effects: the present invention adopts the optimal curve method to solve the algorithm of the path planning problem, and uses the racing car dynamics as an additional boundary constraint to establish a more accurate tire dynamics formula, thereby avoiding the optimal path obtained from exceeding the vehicle dynamics limit, and using the optimal control theory (OCP) to seek to obtain the optimal path of a given global track in a shorter time, that is, to find the most suitable vehicle longitudinal and lateral acceleration at a suitable position on the track, so as to exert the limit of the vehicle. When the unmanned vehicle obtains the path it needs to execute through a series of algorithms, the controller is designed using the model predictive control (MPC) to simulate the input (steering wheel and pedal) of the real driver to the vehicle to drive the vehicle's actuator to move along the preset optimal trajectory. The optimal route planned by the present invention can also act on the racing car design link, and has a guiding role in the selection of racing car parameters, such as tire type, transmission ratio and aerodynamics; and the optimal path is used to train the racing car driver to improve the driving ability of the racing car driver; the planned optimal path can also be used for deep learning and neural network processing, so as to obtain appropriate weights for the learning of automatic driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 Magic formula calculation process.

[0057] Figure 2 Schematic diagram of the race track and the car.

[0058] Figure 3 Kinematic model of a dual-track seven-degree-of-freedom vehicle.

[0059] Figure 4 GG diagram of the test vehicle at low speed.

[0060] Figure 5 Racing ggv picture

[0061] Figure 6 Optimal track line.

[0062] Figure 7 Monorail vehicle model.

[0063] Figure 8 Figure eight track following simulation.

[0064] Fig. 9 The y deviation of the actual car on the tracking track.

[0065] Fig.10 Track tracking real car x deviation.

[0066] Fig.11 The actual vehicle heading angle deviation on the tracking track.

[0067] Fig.12 Actual vehicle speed deviation on the tracking track.

[0068] Fig.13 Comparison of actual car speed and simulated speed on the tracking track.

[0069] Fig.14 MPC vs. pure pursuit speed comparison. DETAILED DESCRIPTION

[0070] The present invention will be further described below in conjunction with the accompanying drawings and embodiments, but the present invention is not limited thereto.

[0071] The present invention provides a path planning and control method for an unmanned formula racing car, comprising the following steps:

[0072] S1 maps the parameters of the driverless racing car.

[0073] Table 1 Main parameters of driverless racing car

[0074]

[0075] Structural analysis and model building of S2 driverless racing car.

[0076] In the development of racing dynamics, the tire model is always the most critical part of the whole vehicle. This is because any interaction between the car and the road is transmitted through the tires, and all the performance of the car is realized through the tires, so the performance of the tire will directly affect the upper limit of the car's performance. Usually, what we call tire dynamics performance is mainly composed of the tire's cornering force and sideslip angle under different vertical loads, slip rate and sideslip angle, and tire positioning parameters such as camber and toe angle. In short, the tire model is the entry and focus of racing research. The Magic Formula tire model is not a conventional mathematical derivation model. This is due to the complexity of pneumatic rubber tires, so empirical formulas are often used. Because of its small fitting error and wide fitting range, the magic formula is the most common model in the establishment of tire models. The magic formula calculation process is as follows Figure 1 As shown, the longitudinal slip rate κ, the sideslip angle α, the camber angle γ, and the wheel vertical load F Z Enter the magic formula as input variable and get the output variable longitudinal force F X , lateral force F Y , overturning moment M X , rolling resistance distance M Y , return torque M Z , the magic formula can be used:

[0077] Y(x)=Dsin[Carctan{Bx-E(Bx-arctan(Bx))}]

[0078] Wherein, B is the curve stiffness factor, C is the curve shape factor, D is the curve peak factor, and E is the curve curvature factor. When x represents the tire's side slip angle or longitudinal slip rate, Y(x) can be the lateral force, the self-aligning moment, or the longitudinal force.

[0079] By analyzing the tires from the combined longitudinal force under steady-state conditions, the combined lateral force under steady-state conditions, and the combined self-aligning torque under steady-state conditions, the magic formula coefficients of the front and rear wheels can be obtained. The magic formula coefficients include the front wheel tire B coefficient, the front wheel tire C coefficient, the load dependence of the front tire coefficient D, the front wheel tire E coefficient, the rear wheel tire B coefficient, the rear wheel tire C coefficient, the load dependence of the rear tire coefficient D, and the rear wheel tire E coefficient.

[0080] According to the test parameters provided in the tire manual (the tire model is Continental C16 / C17), the combined longitudinal force under steady-state conditions, the combined lateral force under steady-state conditions and the combined aligning torque under steady-state conditions are calculated, and then substituted into the magic formula to determine the magic formula coefficients as shown in Table 2.

[0081] Table 2 Magic formula coefficients for front and rear wheels

[0082]

[0083] S3 mathematically models the track and the car.

[0084] There are three coordinate systems in the driving of driverless racing cars, namely the vehicle coordinate system, the lane coordinate system and the global coordinate system. The three coordinate systems can be converted to each other through mathematical relationships. Figure 2 The relationship between the car and the track is shown in the figure. S is the center line of the track, R is the radius of curvature of the track, N l \N r is the distance from the center line to the left and right boundaries. The nt coordinate system is a coordinate system with the point on the center line of the track corresponding to the vehicle as the origin, the normal and tangent as the coordinate axes, and the xy coordinate system is a coordinate system with the center of mass of the vehicle as the origin, the horizontal as the x-axis direction, and the vertical as the y-axis direction. V is the direction of the car's speed, ψ represents the vehicle's heading angle, β represents the vehicle's center of mass side slip angle, and θ represents the angle between the two coordinate systems. It can be described by the following formula:

[0085]

[0086] in is the tangential speed of the car along the track, is the normal speed of the car along the track, is the rate of change of the angle between the front of the car and the track, ξ is the angle between the front of the car and the track, v is the speed of the car, ω z is the yaw rate, β is the sideslip angle of the car's center of mass, n is the track width, and k is the track curvature.

[0087] The racing car is modeled using a dual-track seven-degree-of-freedom vehicle model, such as Figure 3 As shown in the figure, the model has seven degrees of freedom, ignoring the roll motion of the vehicle. The model has good nonlinear performance and the model complexity is moderate, which is suitable for this part of the study. The longitudinal, lateral and yaw motions of the vehicle and the rotational motions of the four tires constitute seven degrees of freedom. First, we determine the vehicle's body coordinate system: the origin of the body coordinate system is the center of mass of the vehicle, the center line of the longitudinal plane of the vehicle is the x-axis, the center line of the transverse plane is the y-axis, the forward direction of the vehicle is the positive x direction, the left side of the vehicle is the positive y direction, and all angles and moments are negative in the clockwise direction. In order to simplify the model and facilitate subsequent research, reasonable assumptions are made for the model:

[0088] (1) Assume that the road surface is flat, with no slope or inclination;

[0089] (2) The mechanical properties of each tire are the same;

[0090] (3) The effect of the vehicle suspension system and the effect of the vehicle's own gravity in the vertical direction are not considered. The differential equation of motion of the vehicle is as follows:

[0091] speed:

[0092]

[0093] Acceleration:

[0094]

[0095] Yaw:

[0096]

[0097] Table 3 Meaning of magic formula symbols

[0098]

[0099]

[0100] S4 sets constraints and solution goals based on the track and car performance.

[0101] Consider the trajectory solution problem of the rigid body system to minimize the time required to complete a given trajectory. The starting point and the end point are fixed, the trajectory boundary constraints are rigid constraints, the boundaries of the control quantity and the state quantity are specified, and the change of the state quantity is a flexible constraint.

[0102] The objective function is:

[0103] Where x(t f ) is the vehicle state at the end moment, x(t) is the real-time state of the vehicle during driving, and u(t) is the real-time control quantity of the vehicle during driving.

[0104]

[0105] u={Δ,f d ,f b}

[0106] Where v is the vehicle speed, β is the vehicle heading angle, is the angle between the vehicle head and the x-axis of the world coordinate system, n is the tangential acceleration of the vehicle's path, θ is the angle between the lane coordinate system and the world coordinate system. Δ is the steering wheel angle, f d is the vehicle braking torque, f b is the vehicle acceleration torque.

[0107] The track constraints are:

[0108] Where N is the track width, since we have assumed that the widths of the left and right sides of the track centerline are equal, That is half the track width, b is the vehicle width, and a safety distance b is setsafe It is convenient to adjust the aggressiveness of the vehicle's driving.

[0109] The control rate constraint is:

[0110] Where v is the vehicle speed, Δ is the steering wheel angle, and f b is the vehicle acceleration torque, f d is the vehicle braking torque, m is the vehicle mass, is the steering wheel angular velocity, R is the radius the vehicle is allowed to pass, and a is the vehicle acceleration.

[0111] Table 4 Limitation parameters

[0112]

[0113] Vehicle dynamics constraints:

[0114] If the vehicle does not have the force of air downforce, most of the vehicle's dynamic performance will not be affected by speed, but because the maximum power is constant, an increase in speed will inevitably lead to a decrease in acceleration. In modern racing, aerodynamic kits have become an indispensable part. Because aerodynamic kits can generate huge downforce, the acceleration and deceleration and cornering limits in the GG limit diagram will be expanded to a certain extent. However, it is inevitable that the increase in speed will cause an increase in driving resistance, which will cause the acceleration performance limit to decrease. The two-dimensional GG diagram describes the relationship between the longitudinal and lateral acceleration of the car at a certain speed, so the traditional GG diagram ( Figure 3 ) cannot accurately describe the limit of the car, and the ggv image needs to be introduced ( Figure 4 ) to describe the limits of a car. The three-dimensional GGV diagram shows the effect of changes in downforce and air resistance due to speed on the GG limit circle.

[0115] like Figure 3 and Figure 4 At low speeds, the boundary of the GG graph is mainly determined by the tire friction characteristics, so replacing tires with higher limits can effectively improve the car's limit at low speeds, and the car's performance at low speeds will be better. However, at high speeds, when tire friction tends to saturation, the influence of aerodynamics begins to dominate due to the square growth relationship between air resistance and speed; it is not difficult to see that Figure 5 When the speed reaches 80km / h, the increased air resistance makes the longitudinal acceleration of the car approach 0, while the longitudinal deceleration can reach a maximum value of -1.5g. Figure 1 It is generally elliptical in shape, and the graph can be placed in a polar coordinate system to represent the relationship between longitudinal and lateral acceleration:

[0116]

[0117] ρ=ρ(α,V)

[0118] Where ρ is the radius of the acceleration circle, a x and a y represents the lateral acceleration and longitudinal acceleration, α is a x and a y angle, V is the speed of the car.

[0119] The task in path planning is to control the performance of the car as close to the surface of the ggv image as possible, so as to obtain the best vehicle dynamics performance.

[0120] S5 uses optimal control theory to solve the optimal curved track.

[0121] The present invention rewrites the objective function using the interior point method as follows:

[0122]

[0123] Among them, I_ is the interior point method barrier term, which is used to prevent the iterative solution from going out of the feasible domain. When it is within the constraint range, the I_ function value is 0; when it is outside the constraint range, the I_ function value is infinite. This effectively prevents the iterative solution from going out of the constraint limit, so that it is possible to find the optimal solution within the feasible domain. The barrier term function form is as follows

[0124]

[0125] Where u is the control quantity of the car.

[0126] Commonly used obstacle functions are as follows:

[0127]

[0128] Where u is the car control rate and t is a parameter used to adjust the degree of approximation.

[0129]

[0130] Where i is the discretized time point, m is the total number of discretized time points, f0(x) represents the cost function caused by the state quantity of the vehicle at the starting time, and f i (x) represents the cost function caused by the state quantity of the vehicle at each discrete point in time.

[0131] Among them, the objective function is a convex function, we define x * (t) is the optimal solution of the above formula, and p * is the minimum value of the objective function.

[0132] The following formula can be obtained

[0133]

[0134] From the above formula, we can see that as t gradually increases, x * (t) can eventually converge to the minimum point. This minimum point is the optimal state of the vehicle we want to solve.

[0135] S6 discretizes the planned track.

[0136] The nonlinear model can well characterize the vehicle's motion state, allowing us to plan a route with better performance. Comparing the nonlinear model predictive control with the linear model predictive control, the nonlinear model has worse timeliness, and the model establishment, discretization, and programming are more difficult. For college student driverless formula racing, the pursuit of speed and timeliness obviously makes the linear model better able to meet our requirements. Equivalent linearization and approximate linearization are the main means of linearizing nonlinear models, and approximate linearization is the method used in this study.

[0137] In model predictive control, we use a single-track bicycle model such as Figure 7 shown.

[0138]

[0139] X=[x,y,θ] T

[0140] U=[v,δ f ] T

[0141] Where x and y are the x-axis and y-axis coordinates of the vehicle's center of mass, θ is the vehicle's heading angle, and δ f is the front wheel angle.

[0142] In X r Taylor expansion at , and ignoring second-order and higher-order small terms

[0143]

[0144] in:

[0145]

[0146] Where L is the wheelbase of the car, v is the speed of the car; X r is the equilibrium point of the vehicle state and the origin of Taylor expansion; X is

[0147] The discrete state point of the vehicle at any time; X r and X both contain the x-axis coordinate, y-axis coordinate, and heading angle; and represents the rate of change between discrete state points and state equilibrium points; v represents the vehicle speed at discrete moments; v r represents the vehicle speed at the equilibrium point; δ represents the actual turning angle of the vehicle; δ r Represents the transfer of vehicle balance point; f function represents Taylor expansion of .

[0148] S7 uses model predictive control to plan each discrete point according to the constraints.

[0149] In the linear MPC of the present invention, a cubic polynomial should be used to fit the optimal track line, and the state points of the car at the next six moments after being discretized are used to approximate the cubic polynomial.

[0150] y=f(x i )=a3x i 3 +a2x i 2 +a1x i +a0

[0151]

[0152] Among them, a0, a1, a2, and a3 are the coefficients of the cubic polynomial constant term, the linear term, the quadratic term, and the cubic term, respectively. The coefficients change with the input of the discrete point x of the horizontal coordinate of the optimal track line. i changes and keeps changing; y is the vertical coordinate of the optimal track line; Ψ is the track line at x i The heading angle of the point.

[0153] The deviation function is constructed as follows:

[0154] Tracking Deviation:

[0155] cte i+1 =cte i+1 +v i+1 sin(Ψ i+1 )Δt

[0156] cte i =f(x i )-y i

[0157] where y i is the actual position of the driverless car at time i, i+1 is the heading angle of the car at time i+1, Δt is the time interval, v i+1 is the speed of the car at time i+1, cte i and cte i+1 Represents the trajectory deviation of the car at time i and time i+1.

[0158] Course deviation:

[0159]

[0160] epsi i =arctan(f′(x i ))-Ψ i

[0161] Among them Ψ i is the actual heading angle of the driverless car at time i, epsi i and epsilon i+1 represents the course deviation of the car at time i and time i+1, δ fi is the front wheel turning angle, L is the wheelbase of the car, and Δt is the time interval.

[0162] Construct a cost function consisting of six aspects: trajectory error, heading error, steering error, speed loss, turning angle change rate and speed change rate.

[0163]

[0164] ω cte ,ω epsi ,ω δ ,ω v ,ω rates and They are the weight coefficients of six aspects, ω cte ,ω epsi ,ω δ ,ω v ,ω rates and They are the weight coefficients of six aspects, cte i Represents the vehicle displacement deviation, epsi i Represents the vehicle heading angle deviation, δ i Represents the vehicle turning angle deviation, v i is the speed of the vehicle at time i, v ref is the vehicle reference speed, v i -v ref represents the speed change, δ fi+1 is the turning angle of the vehicle at time i+1, δ fi is the turning angle of the vehicle at time i, δ fi+1 -δ fi Represents the change in angle, v i+1 is the speed of the vehicle at time i+1, v i is the speed of the vehicle at time i, v i+0 -v i Represents the change in speed.

[0165] The computer is used to solve the minimum value of the cost function, and the solution to the minimum value is the optimal control rate we need. S8 solves the vehicle control rate and sends the steering wheel angle and pedal opening.

[0166] The vehicle control rate calculated by the model predictive control in S7 is sent out by the CAN signal, and the steering signal is sent to the steering control board composed of stm32f103c8t6, ​​and then sent to the steering motor after PWM wave modulation. The acceleration signal is sent to the vehicle electronic control unit composed of stm32f407, and then sent to the motor after processing. The brake signal is an analog model, which is sent to the vehicle electronic control unit composed of stm32f407 after passing through the power amplifier board, and then converted into the motor torque value, which is sent to the motor controller by the CAN message, and the motor rotates to pull the brake pedal.

[0167] The S9 actuator executes the control rate, and the driverless car moves according to the planned path and control rate.

[0168] The driverless car moves according to the planned path and control rate.

[0169] Test verification

[0170] 1. Simulation comparison of different algorithms

[0171] Four types of track lines, namely the shortest path, the minimum curvature, the center line and the optimal track line adopted by the present invention, are compared, and various results of different track lines are shown in Table 5. Figure 6 It is the optimal track line planned by this algorithm. The thick line is the vehicle's driving trajectory, and the dots are the track boundaries.

[0172] Table 5 Comparison of results of various track lines

[0173]

[0174] Judging from the simulation results, the driving distance of the optimal track line is between the minimum curvature track line and the shortest path track line, but the optimal track line reduces the lap time by 0.73 seconds and 3.92 seconds respectively than the minimum curvature track line and the shortest path track line. Therefore, it can be considered that the optimal track line planning algorithm is superior to the minimum curvature track line and the shortest path track line algorithms.

[0175] 2. Software Simulation Verification

[0176] The vehicle participating in the college student driverless formula competition was used for verification. The specific parameters of the vehicle are shown in Table 1. The driverless perception part of the vehicle consists of a laser radar and a binocular vision camera; the vehicle's position and posture are determined by combining an inertial navigation system and using differential GPS. The vehicle originally used triangulation along the centerline path planning and pure tracking path following methods.

[0177] Figure 8 This is the algorithm verification result. The solid line in the figure is the expected track line, the dotted line is the actual track line of the car, and the solid line with stars emitted from the vehicle position is the preview distance of the car. It can be seen from the figure that the following effect is good and the displacement deviation is small.

[0178] 3. Real vehicle test verification

[0179] Fig. 9 and Fig.10 To track the difference between the actual vehicle trajectory and the expected trajectory in the y and x directions of the actual vehicle on the track, it can be seen that there is a deviation of -1.00m in the y direction at the starting position. The vehicle was corrected more aggressively, causing the vehicle to quickly reach a deviation of +0.75m, and then the vehicle fluctuated within ±0.1m. The x direction became unstable at 220m, but it was quickly corrected, and the final deviation also fluctuated within ±0.1m.

[0180] Fig.11 is the heading angle deviation of the actual vehicle in the test, and Fig.10 The trends are basically the same. The vehicle loses stability at around 210m, which is manifested in the increase of the heading angle deviation, which in turn leads to the increase of the x-direction deviation at around 220m.

[0181] Fig.12 This is the deviation between the actual vehicle speed and the simulated speed. Most of the deviations are controlled within ±0.3m / s. Fig.13 This is a comparison between the actual vehicle speed and the simulated speed on the tracking track. The solid line is the actual vehicle speed data obtained by the vehicle inertial navigation system using differential GPS, and the dotted line is the ideal simulated speed. It can be seen that at around 25m, the actual vehicle speed is slightly lower than the ideal speed by about 1m / s. This is because the simulation process is relatively idealized. In the 50-300m section, the actual vehicle speed and the simulated speed are basically consistent, and the control effect is better.

[0182] Fig.14 In the actual vehicle test, the path following of the MPC algorithm and the pure tracking algorithm are compared on the same track. It can be seen that the average speed of the pure tracking algorithm is lower than that of the MPC algorithm, and the MPC algorithm can regulate the vehicle speed in a larger range, which gives full play to the vehicle's dynamic performance.

Claims

1. A path planning and control method for an unmanned formula car, characterized in that: The following steps are involved: S1 maps the parameters of driverless racing cars; Structural analysis and model building of S2 driverless racing car; S3 mathematically models the track and the car; S4 sets constraints and solution goals based on the track and car performance; S5 uses optimal control theory to solve the optimal curve track; S6 discretizes the planned track; S7 uses model predictive control to plan each discrete point according to the constraints; S8 solves the vehicle control rate and sends the steering wheel angle and pedal opening; The S9 actuator executes the control rate, and the driverless car moves according to the planned path and control rate; In the step S4, the trajectory solution problem of the rigid body system is considered to minimize the time required to complete a given trajectory; the starting point and the end point are fixed, the trajectory boundary constraint is a rigid constraint, the boundaries of the control quantity and the state quantity are specified, and the change of the state quantity is a flexible constraint; The objective function is: Where x(t f ) is the vehicle state at the end point, x(t) is the real-time state of the vehicle during driving, and u(t) is the real-time control quantity of the vehicle during driving; u={Δ,f d ,f b } Where v is the vehicle speed, β is the vehicle heading angle, is the angle between the vehicle head and the x-axis of the world coordinate system, n is the tangential acceleration of the vehicle's driving path, θ is the angle between the lane coordinate system and the world coordinate system, Δ is the steering wheel angle, and f d is the vehicle braking torque, f b is the vehicle acceleration torque; The track constraints are: Where N is the track width, b is the vehicle width, and b safe For safe distance; The control rate constraint is: Where m is the vehicle mass, is the steering wheel angular velocity, R is the radius that the vehicle is allowed to pass, and a is the vehicle acceleration; Vehicle dynamics constraints: ρ=ρ(α,V) Where ρ is the radius of the acceleration circle, a x and a y represents the lateral acceleration and longitudinal acceleration, α is a x and a y The angle between them, V is the vehicle speed; In step S5, the interior point method is used to solve the nonlinear optimization problem, and the objective function is rewritten in the form of the interior point method: Among them, f0(x) represents the cost function caused by the state quantity of the vehicle at the starting time, f i (x) represents the cost function caused by the state quantity of each discrete point of the vehicle, m is the total number of discretized time points, i is the discretized time point, I_ is the interior point method obstacle term, its function is to prevent the iterative solution from going out of the feasible domain, within the constraint range, the I_ function value is 0; outside the constraint range, the I_ function value is infinite; In the step S7, a cost function consisting of six aspects including trajectory error, heading error, steering error, speed loss, angle change rate and speed change rate is constructed; The cost function expression is: ω cte ,ω epsi ,ω δ ,ω v ,ω rates and They are the weight coefficients of six aspects, cte i Represents the vehicle displacement deviation, epsi i Represents the vehicle heading angle deviation, δ i Represents the vehicle turning angle deviation, v i is the speed of the vehicle at time i, v ref is the vehicle reference speed, v i -v ref represents the speed change, δ fi+1 is the turning angle of the vehicle at time i+1, δ fi is the turning angle of the vehicle at time i, δ fi+1 -δ fi Represents the change in angle, v i+1 is the speed of the vehicle at time i+1, v i is the speed of the vehicle at time i, v i+1 -v i Represents the change in speed.

2. The path planning and control method of an unmanned formula racing car according to claim 1, characterized in that: In step S1, the mapping parameters include the vehicle curb weight m, center of mass height cog, wheelbase l, distance from center of mass to front axle a, wheel rolling radius, front axle track w l 、Rear axle track w r , rolling resistance coefficient, vehicle width b, vehicle length L, yaw inertia, front axle lift coefficient, rear axle lift coefficient, front axle braking force distribution coefficient, rear axle braking force distribution coefficient and vehicle air resistance coefficient.

3. The path planning and control method of an unmanned formula racing car according to claim 1, characterized in that: In step S2, the tire performance is analyzed using the magic formula, which is as follows: Y(x)=Dsin[Carctan{Bx-E(Bx-arctan(Bx))}] Wherein, B is the curve stiffness factor, C is the curve shape factor, D is the curve peak factor, E is the curve curvature factor, when x represents the tire's side slip angle or longitudinal slip rate, Y(x) is the lateral force, the righting moment or the longitudinal force; By analyzing the tires from the combined longitudinal force under steady-state conditions, the combined lateral force under steady-state conditions, and the combined self-aligning torque under steady-state conditions, the magic formula coefficients of the front and rear wheels can be obtained. The magic formula coefficients include the front wheel tire B coefficient, the front wheel tire C coefficient, the load dependence of the front tire coefficient D, the front wheel tire E coefficient, the rear wheel tire B coefficient, the rear wheel tire C coefficient, the load dependence of the rear tire coefficient D, and the rear wheel tire E coefficient.

4. The path planning and control method of an unmanned formula racing car according to claim 1, characterized in that: In step S3, the track and the car are mathematically modeled using a dual-track seven-degree-of-freedom vehicle kinematic model, where the vehicle's longitudinal, lateral and yaw movements and the rotational movement of the four tires constitute seven degrees of freedom.

5. The path planning and control method of an unmanned formula racing car according to claim 1, characterized in that: In step S5, the interior point method is used to solve the optimal track line within the constraint range of step S4.

6. The path planning and control method of an unmanned formula racing car according to claim 1, characterized in that: In step S6, the obtained optimal track line is discretized with a discrete step length of 0.2 m, and the continuous track is discretized into multiple points containing vehicle state information and control information.

7. The path planning and control method of an unmanned formula racing car according to claim 1, characterized in that: Solve the minimum value of the cost function, and the solution to the minimum value is the optimal control rate.

Images