CN105197010A - Auxiliary parking system and auxiliary parking control method - Google Patents

Abstract

The invention discloses a parking path planning method and parking motion control method of an auxiliary parking system. A multi-stage smooth parking path which is designed through the parking path planning method based on the B-spline theory meets multiple non-linear constraints such as a vehicle obstacle avoidance constraint, a minimum turning radius constraint and a maximum steering speed constraint; through the path curvature continuity, a pivot steering phenomenon is avoided, and tire wear and steering motor loads are reduced; through the multi-stage smoothness of the path, vehicle tracking is easy to achieve. According to the parking motion control strategy, the requirements of the parking system for the parking speed of a driver are lowered, and the driver only needs to control a vehicle to be driven at low speed. For the phenomenon that in the parking process, the vehicle deviates from the target path and cannot park in the correct position due to the fact that the vehicle speed is too high, a vehicle position posture adjusting strategy based on fixed-point tracking control is presented, and therefore the parking success rate is increased; in addition, fixed-point tracking control is adopted, therefore, the vehicle can be parked to be parallel to a parking space, and vehicle parking standardization is achieved.

Description

Assisted parking system and auxiliary parking control method

technical field

The invention relates to a parking device and method, in particular to an auxiliary parking system and an auxiliary parking control method.

Background technique

As the number of vehicles increases, the limited space in the city becomes gradually crowded, and the reduction of parking space increases the difficulty of parking, so people's demand for assisted parking systems is increasing.

The existing parking control methods mainly include: (1) methods based on empirical knowledge; (2) methods based on path planning.

The method based on empirical knowledge uses fuzzy control, neural network, genetic algorithm, etc. to simulate the driver's parking operation behavior. This method is difficult to obtain sample data, difficult to obtain control logic structure, and difficult to train control rules.

The path planning based method consists of path planning and path tracking control. The path planning method mainly uses the arc-line method to plan the parking path. Due to the discontinuous curvature of the parking path composed of arcs and straight lines, it is necessary to stop and turn during parking. The park steer phenomenon causes tire wear and accelerates the steering control motor to increase the workload. During the parking process, when the vehicle follows the arc-line path and reaches the connection point between the arc and the straight line or the junction between the arc and the arc, the driver needs to reduce the speed or even stop the vehicle to cooperate with the steering wheel. Excessive speed or incomplete parking will cause the parking system to lose the best steering timing, and the vehicle will deviate from the target path, which will eventually lead to parking failure. During the parking process, the driver's speed control is highly required.

In the actual parking operation, when the vehicle cannot be parked once, the driver will drive the vehicle forward for a small distance and continue to park backwards. For the situation that the vehicle cannot be parked once, the existing parking system has no remedial measures.

For the phenomenon that the vehicle is not finally parked parallel to the parking space, the existing parking system does not have parking adjustment measures.

Contents of the invention

The technical problem to be solved by the present invention is to provide an auxiliary parking system and an auxiliary parking control method,

In order to solve the above problems, the present invention adopts the following technical solutions to provide an auxiliary parking system, including a path memory, a parking system sensing unit, a parking motion controller, a steering actuator, and a parking system human-computer interaction system. The path memory is used to store the parking path data of the minimum parking space determined by means of a B-spline curve according to the structural parameters of the vehicle; the parking system sensing unit is used to sense the specific position of the vehicle relative to the parking space, and The data is transmitted to the parking motion controller; the parking motion controller is used to determine the motion control strategy and calculate the steering wheel according to the data provided by the path memory and the specific location of the parking system provided by the parking system perception unit. Corner value. The steering actuator is used to receive the signal from the parking motion controller to drive the vehicle steering system to realize the steering wheel angle value calculated by the parking motion controller; the parking system human-computer interaction system is used to detect the parking space The results, shift operation display, and the existence of surrounding obstacles are displayed to the driver, and the driver's operation instructions are transmitted to the parking system. The human-computer interaction system of the parking system communicates with the parking system through the CAN communication mode, receives the prompt information sent by the parking system and displays it in the form of an image, obtains the driver's operation instruction through the touch screen, and sends the instruction through the CAN communication method to the controller.

In order to solve the problems existing in the prior art, the present application also provides an auxiliary parking control method, which includes the following steps:

Step 1, parking path planning; determine the minimum parking space space required by the vehicle according to the vehicle structure parameters, establish a global coordinate system, and use B-spline curves to design the parking path of the minimum parking space required by the vehicle; the parking space of the minimum parking space The path is a path curve, and the path curve is solidified in the parking path memory in the form of data.

Step 2: Parking space sensing prompt; turn on the parking system, the parking system sensing unit starts to detect the right parking space, and displays the lateral distance between the vehicle and the right parking space on the display screen. When it is detected that the parking space on the right meets the minimum parking space requirement, the parking system judges whether the parking space is valid according to the lateral distance between the vehicle and the parking space. For a valid parking space, when the distance between the rear end of the vehicle and the starting point of the parking space is greater than 7m, the human-computer interaction screen displays Detected Get to the parking space and prompt to stop.

Step 3, parking motion control; the parking system reads the parking path, and then enters the motion control; in the parking motion control, it first enters the path tracking control state. When the vehicle X coordinate is greater than x _flag , the parking system always takes the path Tracking control, when the X coordinate is equal to x _flag , the parking system judges to start the fixed-point control or continue to use the path following control according to the deviation between the vehicle and the target path. When the tracking deviation is less than the set value, continue to use the path following control. If the tracking deviation is greater than the set value, the tracking fixed-point control will be started. Wherein, x _flag =l _c -K _flag ·(L _r +L), the value range of K _flag is: 0.4～0.7, the smaller the actual parking space, the smaller the value of K _flag should be.

Step 4, vehicle parking adjustment; after the vehicle enters the parking space, if the vehicle is not parallel to the parking space, the parking system adjusts the vehicle to be parallel to the parking space using a fixed-point control rate.

Further, said step one includes the following steps:

(1) Determine the minimum parking space length required by the vehicle according to the vehicle structure parameters

${\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; csafe</span>}}$

and the minimum parking space width;

${\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; +</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; ksafe</span>}}$

Among them: l _cmin is the required minimum length of the parking space, l _kmin is the required minimum width of the parking space, L _r is the distance from the rear axle of the vehicle to the rear end of the vehicle, R _min is the minimum turning radius of the midpoint of the rear axle of the vehicle, L _k is the width of the vehicle, L is the wheelbase of the vehicle, L _f is the distance from the front axle of the vehicle to the front end of the vehicle, l _csafe is the safety margin of the length of the parking space, and l _ksafe is the safety margin of the width of the parking space.

(2) Establish a global coordinate system with the starting point outside the parking space as the coordinate origin, select the spline curve of the 4th degree 5th order B-spline control points to design the parking path, set the constraint conditions, select the number of control points, and calculate through the optimization algorithm Generate a B-spline curvature-minimizing path curve.

Further, the constraints include: 1) The vehicle travels along the minimized path curve and does not collide with the three sides of the parking space; 2) When the vehicle reaches the end of the path, it completely enters the parking space and is parallel to the parking space; 3) The maximum curvature of the curve is not greater than 1/ R _min ; 4) When the vehicle travels along the path curve at v _max speed, the equivalent front axle rotation angle The rotational speed ω is not greater than ω _max , v _max is the allowable maximum vehicle speed during parking, the value is 0.5-1.5m/s, and ω _max is the maximum steering angular velocity of the steering shaft of the vehicle.

Further, in the second step, the lateral distance is kept at 0.5-1.5m.

Further, the target path deviation in the step 3 includes lateral distance deviation and heading angle deviation, when the lateral distance deviation between the vehicle and the target path |yy _r |≤Δy _o and the heading angle deviation |θ-θ _r |≤Δθ _o , the parking control strategy continues to use path tracking control, if the lateral distance deviation between the vehicle and the target path |yy _r |>Δy _o or the heading angle deviation |θ-θ _r |>Δθ _o , the parking control strategy adopts vehicle over-fixed point control , to adjust the vehicle pose; where: y is the Y-axis coordinate of the midpoint of the rear axle of the vehicle, y _r is the Y-axis coordinate of the intersection point of the line passing through the midpoint of the rear axle of the vehicle and perpendicular to the X-coordinate axis and the path, and the maximum side of Δy _o θ is the angle between the vehicle symmetry line and the X-axis of the coordinate system, _θr is the intersection point between the path tangent and the X-coordinate axis at the point where the line passing through the midpoint of the rear axle of the vehicle and perpendicular to the X-coordinate axis intersects the path angle, Δθ _o is the heading angle deviation threshold.

Further, the lateral distance deviation threshold: Δy _o =(l _c -l _cmin )/K ₂ , the value range of K ₂ is: 10-100, the smaller the l _c , the larger the value; the heading angle deviation valve Value: Δθ _o ＝(l _c -l _cmin )/K ₃ , the value range of K ₃ is 50~100, the smaller the l _c is, the larger the value should be; where l _c is the actual length of the parking space, and l _cmin is the length of the vehicle Minimum bay length required.

Further described path following control, its vehicle control rate is:

In the formula, x is the X coordinate of the midpoint 106 of the rear axle of the vehicle 101, y is the Y-axis coordinate of the midpoint 106 of the rear axle of the vehicle 101, θ is the angle between the vehicle symmetry line and the X-axis of the coordinate system, that is, the vehicle heading angle, y _r is the Y-axis coordinate of the point P _r on the target path corresponding to the x-coordinate of the vehicle, θ _r is the angle between the tangent line at the point P _r and the X-axis of the coordinate system, that is, the target heading angle of the vehicle, and ρ _r is the distance between the parking path 301 at the point P _r Curvature, L is the distance from the rear axle to the front axle of the vehicle 101, k ₁ is the path tracking lateral distance deviation coefficient, k ₂ is the path tracking heading angle deviation coefficient, both k ₁ and k ₂ take negative values when the vehicle is moving forward, and the vehicle moves backward When driving, k ₁ takes a negative value, k ₂ takes a positive value, is the equivalent rotation angle of the front axle, the parking controller obtains the steering wheel steering angle according to the steering transmission ratio of the steering actuator, and the steering is realized by the steering actuator. The steering wheel turns right for positive values, and the steering wheel turns left for negative values.

Further, the tracking fixed-point control, its vehicle tracking fixed-point steering control rate is:

In the formula, x is the X coordinate of the midpoint of the rear axle of the vehicle, y is the coordinate of the Y axis of the midpoint of the rear axle of the vehicle, θ is the angle between the vehicle axis and the X axis, that is, the heading angle of the vehicle, x _p and y _p are the positions of the fixed point of the tracking target, θ _p is the heading angle at the fixed point of the target. L is the vehicle wheelbase, k ₃ is the fixed-point tracking position deviation coefficient, and k ₄ is the fixed-point tracking heading angle deviation coefficient. When the vehicle is moving forward, k ₃ takes a positive value, and k ₄ takes a positive value. When the vehicle runs backwards, k ₃ takes Upright, k ₄ takes a negative value, is the equivalent rotation angle of the front axle 104, The steering wheel turns right for positive values, and the steering wheel turns left for negative values.

The present invention can obtain following technical effect according to above-mentioned technical scheme:

(1) Using the spline curve to plan the multi-order smooth parking path method avoids the phenomenon of parking and turning, and reduces the tire wear and motor work load.

(2) The path tracking control method based on non-time reference has low requirements on vehicle speed control, requiring the driver to drive smoothly at a speed not higher than v _max , which is easier to operate and relieves the driver's nervousness during parking.

(3) In view of the situation that the vehicle deviates from the target path and cannot be parked at one time, the over-fixed control strategy is used to adjust the vehicle pose, so that the vehicle can drive along the predetermined path again, which improves the success rate of parking.

(4) In view of the phenomenon that the vehicle is not parked parallel to the parking space, the fixed-point control strategy is adopted to adjust the vehicle pose to realize the parallel parking of the vehicle and the parking space

Description of drawings

Fig. 1 is the structure and parameter diagram of the vehicle to be parked in the present invention

Figure 2 is a schematic diagram of parking path planning based on B-spline curves

Figure 3 is an analysis diagram of the steering control rate of the vehicle following the parking path

Figure 4 is an analysis diagram of vehicle tracking fixed-point steering control rate

Figure 5 is a schematic diagram of the adjustment strategy for a vehicle that cannot be successfully parked once.

Figure 6 is a schematic diagram of the auxiliary parking system

Figure 7 is a flowchart of the assisted parking system

Fig. 8 is a flow chart of parking motion control

Fig. 9 is a flow chart of vehicle parking adjustment control

Among them: 101, the vehicle to be parked; 102, the steering wheel; 103, the steering system; 104, the front axle of the vehicle: 105, the rear axle of the vehicle; 106, the midpoint of the rear axle; 107, the symmetrical axis of the vehicle; 201～212, the B-spline curve Control point; 301, parking path; 401, actual driving path of the vehicle; 402, vehicle driving direction indication; A, object behind the parking space; B, road; C, object in front of the parking space; D boundary line of the parking space.

Detailed ways

The auxiliary parking control method will be further described below in conjunction with the accompanying drawings, and accompanying drawing 7 shows a working flow chart of the auxiliary parking control method. First, the parking path planning curve is solidified in the parking path memory in the form of data, the parking system is turned on, and the parking system sensing unit starts to detect the right parking space surrounded by the parking space boundary line D as shown in Figure 2, And the lateral distance between the vehicle and the right parking space is displayed on the screen. When it is detected that the parking space on the right meets the minimum parking space requirement stored in the parking path memory, the parking system judges whether the parking space is valid according to the lateral distance between the vehicle and the parking space. When the distance from the starting point is greater than 7m, the human-computer interaction screen will display that a parking space has been detected and prompt to stop. Then, the parking motion control is performed; the vehicle is driven into the parking space. If the vehicle is parallel to the parking space, the parking control ends; otherwise, the vehicle parking adjustment is started, and the parking system adjusts the vehicle to be parallel to the parking space by using a fixed-point control rate.

Accompanying drawing 1, 2 have shown the parking route planning of this control method. Firstly, according to the vehicle parameters in Figure 1, determine the minimum parking space required by the vehicle to be parked. Determine the minimum parking space length required by the vehicle according to the vehicle structure parameters

${\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; csafe</span>}}$

and the minimum parking space width;

${\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; +</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; ksafe</span>}}$

Where: in the formula, L _k is the width of the vehicle 101, L _r is the length from the rear axle 105 to the rear of the vehicle, L is the distance from the front axle 104 to the rear axle 105 of the vehicle 101, L _f is the distance from the front axle 104 to the front end of the vehicle 101, and R _min is The minimum turning radius at the midpoint 106 of the rear axle 105, is the maximum equivalent rotation angle of the front axle, ω _max is the maximum rotation speed of the front axle equivalent rotation angle, v _max is the set maximum speed during parking, v _max determines the maximum parking speed according to the maximum rotation speed of the front axle, and the value ranges from 0.1 to 1.5 m/s, l _ksafe is the safety margin of the minimum parking space width, and the value is greater than or equal to 0.1m.

Secondly, as shown in Figure 2, the global coordinate system is established with the starting point outside the parking space as the coordinate origin, and the spline curve of the 4th degree 5th order B-spline control points is selected to design the parking path. The number of control points is n, and n takes a value Range: 12~15, path control points are recorded as: Q ₁ , Q ₂ ,…, Q _n . The parking path curve can be obtained from the coordinates of n control points Q ₁ -Q _n according to the formula (a). According to the joint formulas (a), (b), (e) and (f) of the coordinates of n control points Q ₁ -Q _n , the trajectory of the four vertices of the vehicle profile can be obtained when the vehicle is driving along the parking path. From the coordinates of n control points Q ₁ -Q _n , (a), (b) and (e) can be combined to obtain the heading angle of the vehicle body at each point of the path when the vehicle is driving along the parking path. The curvature of the path curve is obtained according to the coordinates of the control points Q ₁ -Q _n in combination with formulas (a), (b), (c) and (g). When the vehicle is traveling along the path at the vehicle speed v _max , the steering angular velocity ω of the equivalent steering angle of the front axle is obtained according to the coordinates of the control points Q ₁ ~ Q _n and formulas (a), (b), (c), (d), (h) .

The present invention selects 12 control points to plan the parking route, as shown in FIG. 2 . The genetic algorithm is used to take the coordinates of the control points 201-212 as optimization variables, aiming at minimizing the curvature of the B-spline curve, and optimizing the parking path with the following constraints: 1) The vehicle 101 travels along the path and interacts with surrounding objects A, B, and C No collision; 2) When the vehicle reaches the end of the path, it completely enters the parking space and is parallel to the parking space; 3) The maximum curvature of the curve is not greater than 1/R _min ; 4) When the vehicle travels along the parking path at v _max speed, the equivalent rotation angle of the front axle of the vehicle The rotational speed is not greater than the maximum rotational speed ω _max of the vehicle steering system. ω _max is the maximum speed of the steering wheel of the front axle 105, v _max is the set maximum speed during parking, the value is 0.5-1.5m/s, L is the wheelbase of the vehicle, and ρ is the curvature of the parking path.

In order to reduce the amount of calculation for the path planning of the parking controller, the planned B-spline parking path is stored in the control system. When it is detected that the parking space meets the minimum parking space requirement of the vehicle, the parking control system reads the target path and controls the vehicle to track the target. path.

$\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccccc}\mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 4</span>}& \mathrm{<span\; class="notranslate">\; 6</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 4</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 4</span>}& \mathrm{<span\; class="notranslate">\; 12</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 12</span>}& \mathrm{<span\; class="notranslate">\; 4</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 6</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 6</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 6</span>}& \mathrm{<span\; class="notranslate">\; 6</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 4</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 12</span>}& \mathrm{<span\; class="notranslate">\; 12</span>}& \mathrm{<span\; class="notranslate">\; 4</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 11</span>}& \mathrm{<span\; class="notranslate">\; 11</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]$

Q＝[Q _i Q _i+1 Q _i+2 Q _i+3 Q _i+4 ]′, i=1,2,n-4

i represents the i-th segment of the B-spline curve, n is the number of control points, Q _i ~ Q _i+4 are the coordinates of the 5 points that generate the i-th segment of the B-spline curve, and the B-spline path generated by n control points It is composed of n-4 segments of B-spline smooth connections.

u∈[0,1] is the dimensionless variable of each B-spline curve.

$\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; x</span>}& \mathrm{<span\; class="notranslate">\; the\; y</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; twenty\; four</span>}}\left[\begin{array}{ccccc}{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; 4</span>}}& {\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}& \mathrm{<span\; class="notranslate">\; u</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right]<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; Q</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; )</span>$

In formula (a), x is the coordinate of the spline curve on the X axis, y is the coordinate of the spline curve on the Y axis, u is the variable of the B-spline curve, and Q is the control point of the B-spline curve.

$\left[\begin{array}{cc}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}& \stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; twenty\; four</span>}}\left[\begin{array}{ccccc}{\mathrm{<span\; class="notranslate">\; 4</span>}\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}& \mathrm{<span\; class="notranslate">\; 3</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}& \mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; u</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; Q</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; b</span>}<span\; class="notranslate">\; )</span>$

In formula (b) is the first derivative of the X-axis coordinate of the path to u, is the first derivative of the path Y-axis coordinates to u, u is the variable of the B-spline curve, and Q is the control point of the B-spline curve.

$\left[\begin{array}{cc}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}& \stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; twenty\; four</span>}}\left[\begin{array}{ccccc}\mathrm{<span\; class="notranslate">\; 12</span>}{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}& \mathrm{<span\; class="notranslate">\; 6</span>}\mathrm{<span\; class="notranslate">\; u</span>}& \mathrm{<span\; class="notranslate">\; 2</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; Q</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; c</span>}<span\; class="notranslate">\; )</span>$

In formula (c) is the second derivative of the X-axis coordinate of the path to u, is the second derivative of the path Y-axis coordinates to u, u is the variable of the B-spline curve, and Q is the control point of the B-spline curve.

$\left[\begin{array}{cc}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}& \stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\end{array}\right]<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; twenty\; four</span>}}\left[\begin{array}{ccccc}\mathrm{<span\; class="notranslate">\; twenty\; four</span>}\mathrm{<span\; class="notranslate">\; u</span>}& \mathrm{<span\; class="notranslate">\; 6</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; G</span>}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; Q</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; )</span>$

In formula (d) is the third derivative of the X-axis coordinate of the path to u, is the third derivative of path Y-axis coordinates to u, u is the variable of B-spline curve, and Q is the control point of B-spline curve.

$\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arctan</span>}<span\; class="notranslate">\; (</span>\frac{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; e</span>}<span\; class="notranslate">\; )</span>$

In formula (e), θ is the angle between the tangent line of the B-spline curve and the X-axis, is the first derivative of the X-axis coordinate of the path, is the first derivative of the Y-axis coordinate of the path

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; zq</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; ;</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; zq</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; ;</span>\\ {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; yq</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; ;</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; yq</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; ;</span>\\ {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; yh</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; ;</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; yh</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; ;</span>\\ {\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; en</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; ;</span>\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; en</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; ;</span>\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; )</span>$

In formula (f), x is the coordinate of the spline curve on the X axis, y is the coordinate of the spline curve on the Y axis, θ is the angle between the tangent of the spline curve and the X axis, L is the wheelbase of the vehicle, and L _f is the front axle of the vehicle Distance to the front end, L _r is the distance from the rear axle of the vehicle to the rear end of the vehicle, L _k is the width of the vehicle, x _zq is the X-axis coordinate of the left front end of the vehicle, y _zq is the Y-axis coordinate of the left front end of the vehicle, x _yq is the X-axis of the right front end of the vehicle Coordinates, y _yq is the Y-axis coordinates of the right front end of the vehicle, x _yh is the X-axis coordinates of the right rear end of the vehicle, y _yh is the Y-axis coordinates of the right rear end of the vehicle, x _zh is the X-axis coordinates of the left rear end of the vehicle, y _zh is the left side of the vehicle The Y-axis coordinate of the rear end.

$\mathrm{<span\; class="notranslate">\; \&rho;</span>}<span\; class="notranslate">\; =</span>\frac{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}{{<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; g</span>}<span\; class="notranslate">\; )</span>$

In formula (g), ρ is the path curvature, The first derivative of the X-axis coordinates of the path, is the first derivative of the Y-axis coordinate of Road King is the second derivative of the path X-axis coordinates, It is the second derivative of the Y-axis coordinate of the path.

$\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; max</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 3</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>}{{<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>$

In the formula (h), v _max is the maximum parking speed, ω is the equivalent rotation angle of the front axle when the vehicle travels along the spline curve at the speed of v _max , The first derivative of the X-axis coordinates of the path, is the first derivative of the Y-axis coordinate of Road King is the second derivative of the path X-axis coordinates, It is the second derivative of the Y-axis coordinate of the path.

Finally, the completed path curve is solidified in the parking path memory in the form of data.

Accompanying drawing 3-5, 8 have shown the parking movement control of the step 3 of this method. As shown in Figure 3, the reference quantity for selecting the motion control rate of the parking system: x _flag = l _c - K _flag (L _r + L), the value range of K _flag is: 0.4 ~ 0.7, the smaller the actual parking space, the smaller the K flag The value of _flag should be smaller. When the X coordinate of the vehicle is greater than x _flag , the parking system always adopts path tracking control. When the X coordinate is equal to x _flag , the parking system judges whether to start fixed-point control or continue to use path tracking control according to the deviation between the vehicle and the target path. When tracking If the deviations are smaller than the set value, continue to use path tracking control, otherwise, start tracking fixed-point control. The target path deviation includes lateral distance deviation and heading angle deviation. When the lateral distance deviation |yy _r |≤Δy _o and the heading angle deviation |θ-θr| _{≤Δθ o} between the vehicle and the target path, the parking control strategy continues to use Tracking control, if the lateral distance deviation between the vehicle and the target path |yy _r |>Δy _o or the heading angle deviation |θ-θ _r |>Δθ _o , the parking control strategy adopts the vehicle passing fixed point control to adjust the vehicle pose; where: y is the Y-axis coordinate of the midpoint of the rear axle of the vehicle, y _r is the Y-axis coordinate of the intersection point of a straight line passing through the midpoint of the rear axle of the vehicle and perpendicular to the X coordinate axis and the path, Δy _o is the lateral distance deviation threshold, and θ is the vehicle The angle between the symmetry line and the X-axis of the coordinate system, _θr is the angle between the path tangent and the X-coordinate axis at the point where the line passing through the midpoint of the rear axle of the vehicle and perpendicular to the X-coordinate axis intersects the path, and the X-coordinate axis, Δθ _o is the heading angle deviation valve value.

The vehicle tracking bias is set as follows:

Threshold of lateral distance deviation: Δy _o = (l _c -l _cmin )/K ₂ , the value range of K ₂ is: 10~100, the smaller l _c should be, the larger the value should be;

Heading angle deviation threshold: Δθ _o = (l _c -l _cmin )/K ₃ , value range of K ₃ : 50-100, the smaller l _c is, the larger the value should be;

In the formula, l _c is the actual length of the parking space, and l _cmin is the minimum length of the parking space required by the vehicle.

The vehicle travels backwards from the starting point of parking. When the X-axis coordinate of the rear axle midpoint 106 of the vehicle 101 is greater than x _flag , the parking controller performs path tracking control. The tracking deviation is shown in FIG. 3 , and the vehicle motion control rate is:

In the formula, x is the X coordinate of the midpoint 106 of the rear axle of the vehicle 101, y is the Y-axis coordinate of the midpoint 106 of the rear axle of the vehicle 101, θθ is the angle between the vehicle symmetry line and the X-axis of the coordinate system, that is, the vehicle heading angle, y _r is the Y-axis coordinate of the point P _r on the target path corresponding to the X coordinate of the vehicle, θ _r is the angle between the tangent line at the point P _r and the X-axis of the coordinate system, that is, the target heading angle of the vehicle, and ρ _r is the distance between the parking path 301 at the point P _r Curvature, L is the distance from the rear axle 105 to the front axle 104 of the vehicle 101, k ₁ is the path tracking position deviation coefficient, k ₂ is the path tracking heading angle deviation coefficient, both k ₁ and k ₂ take negative values when the vehicle is moving forward, and the vehicle moves backward When driving, k ₁ takes a negative value, k ₂ takes a positive value, is the equivalent rotation angle of the front axle 05, the parking controller obtains the steering angle of the steering wheel 102 according to the steering transmission ratio of the steering actuator 103, and the steering is realized by the steering actuator 103, Steering wheel turns right for a positive value, and turns left for a negative value;

Fig. 4 is a schematic diagram of vehicle fixed-point tracking deviation, point P is the point to be tracked after the parking system starts the fixed-point tracking control. The tracking fixed point steering control rate is:

In the formula, x is the X coordinate of the midpoint of the rear axle of the vehicle, y is the coordinate of the Y axis of the midpoint of the rear axle of the vehicle, θ is the angle between the vehicle axis and the X axis, that is, the heading angle of the vehicle, x _p and y _p are the positions of the fixed point of the tracking target, θ _p is the heading angle at the fixed point of the target. L is the wheelbase of the vehicle 101, k ₃ is the fixed-point tracking position deviation coefficient, and k ₄ is the fixed-point tracking heading angle deviation coefficient. When the vehicle is moving forward, k ₃ takes a positive value, and k ₄ takes a positive value. When the vehicle moves backward, k ₃ Take the positive value, k ₄ takes the negative value, is the equivalent rotation angle of the front axle 104, The steering wheel turns right for positive values, and the steering wheel turns left for negative values.

As shown in Figure 5. Record the point on the target path whose X-axis coordinates are equal to x _flag as P ₂ , select a point on the tangent line of P ₂ and record it as P ₁ , the pose at P ₂ is expressed as (x ₂ , _y2 ,θ ₂ ), and the position of P ₁ The pose is (x ₂ +K ₁ ·cosθ ₁ ,y ₂ +K ₁ ·sinθ ₁ ,θ ₂ ), and the value range of K ₁ is 0.5～2.

Note that the end point of the target path is P ₃ , P ₄ is a point directly in front of P ₃ , and the distance from point P ₄ to object C is not less than L+L _f +Δ _safe , Δ _safe is the safe parking distance in the parking space, reverse the car according to the parking system Radar minimum range and measurement accuracy determine safe values. When the X-axis coordinates of the midpoint 106 of the rear axle of the vehicle 101 are equal to x _flag , if the vehicle position deviation |yy _r |≤Δy _o and |θ-θ _r |≤Δθ _o continue to use path tracking control, otherwise the parking system prompts the driver through the screen When driving forward, the parking system controls the vehicle to track the fixed point P ₁ . When the X-axis coordinate of the vehicle is greater than the X-axis coordinate of point P ₁ or the Y-axis coordinate of the vehicle is greater than the Y-axis coordinate of P ₁ point, the parking system prompts the driver to drive backward and park the car. The system controls the vehicle to track the point P2, and judges whether to use path tracking control or fixed _- point tracking control according to the tracking deviation when the vehicle's X-axis coordinate is equal to x _flag .

As shown in Figure 9, when the vehicle reaches point P ₃ along the target path or the distance of the object A behind is less than _Δsafe , if the vehicle fails to parallel, the parking system prompts the driver to move forward and controls the vehicle to track P _4. When the vehicle When the X coordinate is greater than the X coordinate of point P ₄ or the distance between the front end of the vehicle and object C is less than Δ _safe , if the vehicle is still not parallel to the road C, the parking system prompts the driver to drive backwards and controls the vehicle to track P ₃ point. _The parking system adjusts the heading angle of the vehicle 101 by tracking the points _P3 and P4 so that the vehicle 101 is parallel to the road B.

As shown in Figure 6, the auxiliary parking system of the present invention includes a path memory, a parking system sensing unit, a parking motion controller, a steering actuator, and a parking system human-computer interaction system. Structural parameters, adopting B-spline curve method to determine the parking path data of the minimum parking space; the parking system sensing unit is used to sense the specific position of the vehicle relative to the parking space, and transmit the sensing data to the parking motion control The parking motion controller is used for judging the driving direction of the vehicle and calculating the steering wheel angle value according to the data provided by the path memory and the specific location required for parking provided by the parking system sensing unit. The steering actuator is used to receive the signal from the parking motion controller to drive the steering system of the vehicle to realize the value of the steering wheel angle; The existence of obstacles is displayed to the driver, and the driver's operation instructions are transmitted to the parking system. The human-computer interaction system of the parking system communicates with the parking system through the CAN communication mode, receives the prompt information sent by the parking system and displays it in the form of an image, obtains the driver's operation instruction through the touch screen, and sends the instruction through the CAN communication method to the controller.

What is described in this specification is only a specific embodiment of the present invention, and the above embodiments are only used to illustrate the technical solution of the present invention rather than limit the present invention. All technical solutions obtained by those skilled in the art through logical analysis, reasoning or limited experiments according to the concept of the present invention shall fall within the scope of the present invention.

Claims (9)

1. An auxiliary parking control method, characterized in that: comprising the following steps Step 1, parking path planning; determine the minimum parking space space required by the vehicle according to the vehicle structure parameters, establish a global coordinate system, and use B-spline curves to design the parking path of the minimum parking space required by the vehicle; the parking space of the minimum parking space The path is a path curve, and the path curve is solidified in the parking path memory in the form of data; Step 2: Parking space sensing prompt; turn on the parking system, the parking system sensing unit starts to detect the right parking space, and displays the lateral distance between the vehicle and the right parking space on the display screen. When it is detected that the parking space on the right meets the minimum parking space requirement, the parking system judges whether the parking space is valid according to the lateral distance between the vehicle and the parking space. Get to the parking space and prompt to stop. Step 3, parking motion control; the parking system reads the parking path, and then enters the motion control; in the parking motion control, it first enters the path tracking control state. When the vehicle X coordinate is greater than x _flag , the parking system always takes the path Tracking control, when the X coordinate is equal to x _flag , the parking system judges to start the fixed-point control or continue to use the path following control according to the deviation between the vehicle and the target path. When the tracking deviation is less than the set value, continue to use the path following control, otherwise , start tracking fixed-point control. Wherein, x _flag =l _c -K _flag ·(L _r +L), the value range of K _flag is: 0.4～0.7, the smaller the actual parking space, the smaller the value of K _flag should be. Step 4, vehicle parking adjustment; after the vehicle enters the parking space, if the vehicle is not parallel to the parking space, the parking system adjusts the vehicle to be parallel to the parking space using a fixed-point control rate. 2. The auxiliary parking control method according to claim 1, wherein said step 1 comprises the following steps: (1) Determine the minimum parking space length required by the vehicle according to the vehicle structure parameters ${\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; +</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; csafe</span>}}$ and the minimum parking space width; ${\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; +</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; L</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; ksafe</span>}}$ Among them: l _cmin is the required minimum length of the parking space, l _kmin is the required minimum width of the parking space, L _r is the distance from the rear axle of the vehicle to the rear end of the vehicle, R _min is the minimum turning radius of the midpoint of the rear axle of the vehicle, L _k is the width of the vehicle, L is the wheelbase of the vehicle, L _f is the distance from the front axle of the vehicle to the front end of the vehicle, l _csafe is the safety margin of the length of the parking space, and l _ksafe is the safety margin of the width of the parking space. (2) Establish a global coordinate system with the starting point outside the parking space as the coordinate origin, select the spline curve of the 4th degree 5th order B-spline control points to design the parking path, set the constraint conditions, select the number of control points, and calculate through the optimization algorithm Generate a B-spline curvature-minimizing path curve. 3. The auxiliary parking control method according to claim 2, characterized in that: the constraints include: 1) The vehicle traveling along the minimized path curve does not collide with the three sides of the parking space; 2) When the vehicle reaches the end of the path, it completely enters the parking space and is parallel to the parking space; 3) The maximum curvature of the curve is not greater than 1/R _min ; 4) When the vehicle travels along the path curve at v _max speed, the equivalent front axle rotation angle The rotational speed ω is not greater than ω _max , v _max is the allowable maximum vehicle speed during parking, the value is 0.5-1.5m/s, and ω _max is the maximum steering angular velocity of the steering shaft of the vehicle. 4. A parking assistance control method according to claim 1, characterized in that: in the second step, the lateral distance is kept at 0.3-2m. 5. A parking assistance control method according to claim 3, characterized in that: the target path deviation in step 3 includes a lateral distance deviation and a heading angle deviation, when the lateral distance deviation between the vehicle and the target path| yy _r | ≤ _{Δy o} and heading angle deviation |θ _{- θ r} | |θ-θ _r |>Δθ _o , the parking control strategy adopts the vehicle passing fixed-point control to adjust the vehicle pose; where: y is the Y-axis coordinate of the midpoint of the rear axle of the vehicle, y _r is the midpoint of the rear axle of the vehicle and is related to X Y-axis coordinates at the intersection of a straight line perpendicular to the coordinate axis and the path, Δy _o lateral distance deviation threshold, θ is the angle between the vehicle symmetry line and the X-axis of the coordinate system, θ _r is the midpoint of the rear axle of the vehicle and the X-coordinate The angle between the path tangent and the X coordinate axis at the intersection point of the straight line perpendicular to the axis and the path, Δθ _o is the heading angle deviation threshold. 6. A parking assistance control method according to claim 5, characterized in that: said lateral distance deviation: Δy _o =(l _c -l _cmin )/K ₂ , the value range of K ₂ is: 10 ~100, the smaller the l _c is, the larger the value should be; heading angle deviation: Δθ _o = (l _c -l _cmin )/K ₃ , the value range of K ₃ : 50~100, the smaller the l _c should be, the larger the value ; where l _c is the actual length of the parking space, and l _cmin is the minimum length of the parking space required by the vehicle. 7. A kind of assisted parking control method according to claim 6, is characterized in that: described path following control, its vehicle control ratio: In the formula, x is the X coordinate of the midpoint 106 of the rear axle of the vehicle 101, y is the Y-axis coordinate of the midpoint 106 of the rear axle of the vehicle 101, θ is the angle between the vehicle symmetry line and the X-axis of the coordinate system, that is, the vehicle heading angle, y _r is the Y-axis coordinate of the point P _r on the target path corresponding to the x-coordinate of the vehicle, θ _r is the angle between the tangent line at the point P _r and the X-axis of the coordinate system, that is, the target heading angle of the vehicle, and ρ _r is the distance between the parking path 301 at the point P _r Curvature, L is the distance from the rear axle to the front axle of the vehicle 101, k ₁ is the path tracking lateral distance deviation coefficient, k ₂ is the path tracking heading angle deviation coefficient, both k ₁ and k ₂ take negative values when the vehicle is moving forward, and the vehicle moves backward When driving, k ₁ takes a negative value, k ₂ takes a positive value, is the equivalent rotation angle of the front axle, the parking controller obtains the steering wheel steering angle according to the steering transmission ratio of the steering actuator, and the steering is realized by the steering actuator. The steering wheel turns right for positive values, and the steering wheel turns left for negative values. 8. A kind of auxiliary parking control method according to claim 7, characterized in that: said tracking fixed-point control, its vehicle tracking fixed-point steering control rate is: In the formula, x is the X coordinate of the midpoint of the rear axle of the vehicle, y is the coordinate of the Y axis of the midpoint of the rear axle of the vehicle, θ is the angle between the vehicle axis and the X axis, that is, the heading angle of the vehicle, x _p and y _p are the positions of the fixed point of the tracking target, θ _p is the heading angle at the fixed point of the target. L is the wheelbase of the vehicle, k ₃ is the fixed-point tracking position deviation coefficient, and k ₄ is the fixed-point tracking heading angle deviation coefficient. When the vehicle is moving forward, k ₃ takes a positive value, and k ₄ takes a positive value. When the vehicle moves backward, k ₃ takes a positive value. , k ₄ takes a negative value, is the equivalent rotation angle of the front axle 104, and the steering wheel rotation angle is obtained by calculating the equivalent rotation angle of the front axle according to the angular transmission ratio of the steering system of the vehicle, The steering wheel turns right for positive values, and the steering wheel turns left for negative values. 9. An auxiliary parking system running the auxiliary parking control method according to claim 1, comprising a path memory, a parking system sensing unit, a parking motion controller, a steering actuator, and a parking system human-computer interaction system , characterized by: The path memory is used to store the parking path data for determining the minimum parking space by means of a B-spline curve according to vehicle structural parameters; The parking system sensing unit is used to sense the specific position of the vehicle relative to the parking space, and transmit the sensing data to the parking motion controller; The parking motion controller is used to select a motion control strategy and calculate the steering wheel angle value according to the data provided by the path memory and the specific location required for parking provided by the parking system sensing unit. The steering actuator is used to receive the signal from the parking motion controller, and drive the steering system of the vehicle to realize the steering wheel angle value calculated by the parking motion controller; The man-machine interaction system of the parking system is used to display the detection result of the parking space, the shift operation display, and the existence of surrounding obstacles to the driver, and transmit the driver's operation instructions to the parking system. The human-computer interaction system of the parking system communicates with the parking system through CAN communication, receives the prompt information sent by the parking system and displays it in the form of an image, obtains the driver's operation instructions through the touch screen, and sends the instructions through CAN communication to the controller.