CN112793562B - A planning and tracking control method for an automatic parking path, a planning device, a storage medium and a computer device - Google Patents

Abstract

The invention discloses a planning and tracking control method, a planning device, a storage medium and computer equipment for an automatic parking path. And the planning method adopts a preview tracking algorithm, closed-loop steering control and speed control, tracks points are tracked in a segmented manner, motion accumulated errors are eliminated in real time, and the track tracking precision is ensured. After parking is finished, the distance deviation is not more than 20cm, the angle between the center of the car body and the center line of the parking space is not more than 2 degrees, and the performance requirement of a parking system is completely met.

Description

A planning and tracking control method for an automatic parking path, a planning device, and a storage medium quality and computer equipment

technical field

The invention relates to the technical field of automobile assisted driving, in particular to a planning and tracking control method of an automatic parking path, a planning device, a storage medium and a computer device.

Background technique

With the rapid development of the automobile industry, the functions of automobile auxiliary driving are becoming more and more perfect; the automatic parking function is also one of the functions of automobile auxiliary driving. The current automatic parking system is mainly composed of environment perception module, control decision-making module and execution module. The parking process can be divided into three parts: parking space detection, trajectory planning and trajectory tracking.

At present, the realization of parking trajectory planning is mainly based on the vehicle motion model and the Ackerman angle principle to decompose the parking process into straight lines, arcs, and curves, and form the parking trajectory through segmented splicing, so as to calculate the corresponding expected vehicle speed, expected Steering wheel angle and gear position. However, the existing parking path planning is mainly static planning, and the accuracy of its segmented trajectory is greatly affected by boundary constraints, vehicle kinematics models, and calibration parameters. Moreover, static planning cannot ensure that the parking trajectory is dynamically adjusted when the vehicle encounters obstacles or changes in environmental conditions during parking, and the failure rate of parking is high.

Contents of the invention

The technical problem solved by the present invention is to provide a planning and tracking control method, planning device, storage medium and computer equipment of an automatic parking path, which can be based on the real-time position of the vehicle, parking space information, vehicle dynamics constraints and parking boundary constraints Etc. Real-time calculation of parking trajectory, high parking accuracy.

In order to solve the problems of the technologies described above, the content of the technical solution adopted in the present invention is specifically as follows:

A method for planning and tracking control of an automatic parking path, comprising the steps of:

Acquire parking space information and vehicle information, the parking space information includes the coordinates of the four vertices of the parking space; the vehicle information includes the initial position coordinates of the vehicle, the way the vehicle is parked, the length of the vehicle body, the width of the vehicle body, the front of the vehicle Overhang length and rear overhang length of the vehicle;

Process the acquired data, calculate the boundary constraints, calculate the target parking space and the actual position of the current vehicle through coordinate conversion, and determine the target parking point;

Obtain the parking trajectory according to the parking space information, vehicle information and obstacle information;

Use the tracking algorithm to control the vehicle and complete the tracking of the planned trajectory.

As an optimization of the above scheme, obtaining the parking trajectory according to the parking space information, vehicle information and obstacle information includes the following steps:

Determine the target parking point of the vehicle according to the vehicle information and parking space information;

In the P coordinate system, with the target parking point as the end point and the initial vehicle position as the starting point, the trajectory curve combination between the target parking point and the initial vehicle position is determined by using the Dubins path planning method;

Get the vehicle profile parameters according to the vehicle information;

Get the parking space boundary according to the parking space information;

According to the vehicle contour parameters and the parking space boundary, the trajectory curves in the trajectory curve combination are verified by boundary constraints and dynamic constraints in turn, and the trajectory curves that meet the boundary constraints and dynamic constraints are confirmed as parking curves;

Use the Bessel formula to smooth the parking curve;

For the smoothed parking curve, the vehicle motion model is used to decompose the vehicle motion information, that is, the parking trajectory is obtained.

As a preference of the above scheme, determining the target parking spot of the vehicle according to the vehicle information and the parking space information includes the following steps:

Determine the rear axle center of the vehicle according to the length of the vehicle body, the width of the vehicle body, the length of the front overhang of the vehicle and the length of the rear overhang of the vehicle;

According to the coordinates of the four vertices of the parking space, determine the projection point of the center of the rear axle of the vehicle on the parking space when the vehicle is parked in the parking space, and the projection point is the target parking point.

As a preference of the above solution, the vehicle profile parameter is the coordinate values of the four vertices of the vehicle relative to the center of the rear axle of the vehicle.

As an optimization of the above scheme, using a tracking algorithm to control the vehicle to realize parking trajectory tracking includes the following steps:

Perform lateral control on the vehicle to obtain the steering wheel angle of the vehicle;

Perform longitudinal control on the vehicle to obtain the control mode, longitudinal distance and longitudinal speed control module of the vehicle;

According to the steering wheel angle, the control mode of the vehicle, the longitudinal distance and the longitudinal speed control module, the parking trajectory is corrected to obtain the parking path.

As an optimization of the above scheme, performing lateral control on the vehicle to obtain the steering wheel angle of the vehicle includes the following steps:

According to the body parameters of the vehicle, the coefficients of the quadratic term and the first term of the preview distance formula of the pure tracking algorithm are determined to be 1/6 and 1/5 respectively;

Determine the preview distance of the tracking algorithm, obtain the preview point, and the calculation formula of the preview distance ld is

Calculate the coordinates of the preview point in the vehicle coordinate system, and the ordinate of the preview point in the vehicle coordinate system is the preview deviation;

Taking the preview deviation as the control quantity, the PID control algorithm is used to control the steering wheel angle in closed loop.

The present invention also provides a planning device for an automatic parking path, including an acquisition module and a path planning module, the acquisition module acquires parking space information and vehicle information, and the parking space information includes the coordinates of four vertices of the parking space; the vehicle information includes The initial position coordinates of the vehicle, the parking mode of the vehicle, the body length of the vehicle, the width of the vehicle body, the front overhang length of the vehicle and the rear overhang length of the vehicle; the path planning module obtains parking track.

As a preference of the above scheme, the acquisition module includes a first acquisition unit, a first determination unit, a second determination unit, a third determination unit, a fourth determination unit and a smoothing processing unit, and the first acquisition unit is based on vehicle information and The parking space information determines the target parking point of the vehicle; in the P coordinate system, the first determination unit takes the target parking point as the end point and the initial position of the vehicle as the starting point, and uses the Dubins path planning method to determine the distance between the target parking point and the initial position of the vehicle. The trajectory curve combination; the second determination unit obtains the vehicle contour parameters according to the vehicle information; the third determination unit obtains the parking space boundary according to the parking space information; The trajectory curve of the trajectory curve is verified by boundary constraints and dynamic constraints in turn, and the trajectory curve that meets the boundary constraints and dynamic constraints is confirmed as the parking curve; the smoothing processing unit uses the Bessel formula to smooth the parking curve deal with.

The present invention also provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the planning method are realized.

The present invention also provides a computer device, including a memory and a processor connected to the memory, the memory stores a computer program, and when the computer program is executed by the processor, the steps of the planning method are implemented.

Compared with prior art, the beneficial effect of the present invention is:

The automatic parking path planning and tracking control method provided by the present invention adopts a dynamic parking trajectory planning method to update the parking trajectory in real time according to positioning information and obstacle information, under the premise of satisfying boundary constraints and vehicle dynamic constraints Adapt to various parking environments to the greatest extent. Moreover, the planning method adopts a preview tracking algorithm, closed-loop steering control and speed control, tracks track points in segments, eliminates motion accumulation errors in real time, and ensures track tracking accuracy. The distance deviation after parking is not greater than 20cm, and the angle between the center of the vehicle body and the centerline of the parking space is not more than 2°, fully meeting the performance requirements of the parking system.

The above description is only an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention, it can be implemented according to the contents of the description, and in order to make the above and other purposes, features and advantages of the present invention more obvious and understandable , the following preferred embodiments are specifically cited below, and are described in detail as follows in conjunction with the accompanying drawings.

Description of drawings

Fig. 1 is an application environment diagram of the planning and tracking control method of the automatic parking path of a more preferred embodiment;

Fig. 2 is a schematic flowchart of a method for planning and tracking control of an automatic parking path in a more preferred embodiment;

Fig. 3 is a structural block diagram of a planning device for an automatic parking path in a more preferred embodiment;

Fig. 4 is the structural block diagram of the computer equipment of more preferred embodiment;

Fig. 5 is a parking space information parameter map;

Wherein, the reference numerals of each accompanying drawing are:

1. Terminal; 2. Server; 3. Acquisition module; 4. Path planning module; 5. First acquisition unit; 6. First determination unit; 7. Second determination unit; 8. Third determination unit; 9. The first Four determination unit; 10, smoothing processing unit.

Detailed ways

In order to further elaborate the technical means and effects that the present invention adopts for reaching the intended invention purpose, below in conjunction with the accompanying drawings and preferred embodiments, the specific implementation, structure, features and effects of the present invention are described in detail as follows:

Embodiment one

As shown in Figure 1 is the application environment diagram of the planning and tracking control method of the automatic parking path of the present invention, the planning and tracking control method of the automatic parking path is applied to the planning system of the automatic parking path, the automatic parking The path planning system includes a terminal 1 and a server 2, the terminal 1 and the server 2 are connected through a network, the terminal 1 may be a desktop terminal or a mobile terminal, and the mobile terminal may be a mobile phone, a tablet computer, a notebook computer, a portable At least one of wearable devices, etc., the server 2 can be implemented by an independent server or a server cluster composed of multiple servers.

As shown in FIG. 2 , in one embodiment, the present invention provides a method for planning and tracking control of an automatic parking path, which is described by taking the method applied to the server 2 in FIG. 1 as an example, including:

Acquire parking space information and vehicle information, the parking space information includes the coordinates of the four vertices of the parking space; the vehicle information includes the initial position coordinates of the vehicle, the way the vehicle is parked, the length of the vehicle body, the width of the vehicle body, the front of the vehicle Overhang length and rear overhang length of the vehicle;

Process the acquired data, calculate the boundary constraints, calculate the target parking space and the actual position of the current vehicle through coordinate conversion, and determine the target parking point;

Obtain the parking trajectory according to the parking space information, vehicle information and obstacle information;

Use the tracking algorithm to control the vehicle and complete the tracking of the planned trajectory.

Specifically, taking the parking space shown in Figure 5 as an example, A, B, C, and D are the four vertices of the parking space, and the coordinates of the four points A, B, C, and D in the earth coordinate system are respectively (x _A , y _A ), (x _B , y _B ), (x _C , y _C ) and (x _D , y _D ), the formula for calculating the width Wid of the parking space is: Wid=x _B -x _A ＝x _C -x _D ; the calculation formula of the depth Dep of the parking space is Dep=y _D -y _A =y _C -y _B .

As an optimization of the above scheme, obtaining the parking trajectory according to the parking space information, vehicle information and obstacle information includes the following steps:

Determine the target parking point of the vehicle according to the vehicle information and parking space information; in the geodetic coordinate system, the coordinates of the target parking point x _o =Wid/2=(x _B -x _A )/2, y _o =Dep/2-l _r - l _s = (y _D -y _A )/2-l _r -l _s ; where, l _r is the length of the rear overhang of the vehicle, and l _s is the reserved safety distance behind.

In the P coordinate system, with the target parking point as the end point and the vehicle’s initial position as the starting point, the Dubins path planning method is used to determine the trajectory curve combination between the target parking point and the vehicle’s initial position. Specifically, the CSC type (ie, curve + straight line + curve) combination, assuming that the starting point is S=(x _s ,y _s ,α1), the end point g=(x _g ,y _g ,α2), and the minimum turning radius is r _min , then the calculation formulas of the three sections of paths are respectively :

Then the path L _CSC =p1+p2+p3.

Get the vehicle profile parameters according to the vehicle information;

Get the parking space boundary according to the parking space information;

According to the vehicle contour parameters and the parking space boundary, the trajectory curves in the trajectory curve combination are verified by boundary constraints and dynamic constraints in turn, and the trajectory curves that meet the boundary constraints and dynamic constraints are confirmed as parking curves;

Use the Bessel formula to smooth the parking curve;

For the smoothed parking curve, the vehicle motion model is used to decompose the vehicle motion information, that is, the parking trajectory is obtained.

Specifically, the point vector on the above-mentioned path curve at any moment is (x _r (t), y _r (t), α), where x _r (t) is the abscissa of the trajectory, and y _r (t) is the ordinate of the trajectory , α is the tangential angle of the trajectory. Assuming that the center point of the rear axle of the vehicle tracks the path curve, the coordinates of the center point of the rear axle of the vehicle at any time are also the point coordinates on the curve. According to the vehicle kinematics equation, without considering the sideslip of the vehicle at low speeds, there are:

Among them, L is the wheelbase of the front and rear axles of the vehicle, V is the vehicle speed (constant speed), and β is the front wheel steering angle.

As a preference of the above scheme, determining the target parking spot of the vehicle according to the vehicle information and the parking space information includes the following steps:

Determine the center of the rear axle of the vehicle according to the length of the vehicle body, the width of the vehicle body, the length of the front overhang of the vehicle, and the length of the rear overhang of the vehicle. Specifically, taking the center of the outer side of the left front wheel as the origin, the coordinates of the rear axle point x _r = L _C /2, y _r =L _l -L _r -L _f , wherein, L _C is the width of the vehicle, L _l is the length of the vehicle, L _r and L _f are the lengths of the rear suspension and the front suspension of the vehicle respectively;

According to the coordinates of the four vertices of the parking space, determine the projection point of the center of the rear axle of the vehicle on the parking space when the vehicle is parked in the parking space, and the projection point is the target parking point.

As a preference of the above solution, the vehicle profile parameter is the coordinate values of the four vertices of the vehicle relative to the center of the rear axle of the vehicle.

As an optimization of the above scheme, using a tracking algorithm to control the vehicle to realize parking trajectory tracking includes the following steps:

Perform lateral control on the vehicle to obtain the steering wheel angle of the vehicle;

Perform longitudinal control on the vehicle to obtain the control mode, longitudinal distance and longitudinal speed control module of the vehicle;

According to the steering wheel angle, the control mode of the vehicle, the longitudinal distance and the longitudinal speed control module, the parking trajectory is corrected to obtain the parking path.

As an optimization of the above scheme, performing lateral control on the vehicle to obtain the steering wheel angle of the vehicle includes the following steps:

其中，a_max为最大减速度，本案例中取3m/s^2；B为反应时间，本发明中B的取值为0.2s。According to the body parameters of the vehicle, the quadratic coefficient A and the primary coefficient B of the preview distance formula of the pure tracking algorithm are determined to be 1/6 and 1/5 respectively. When calculating:

Wherein, a _max is the maximum deceleration, which is 3m/s^2 in this case; B is the reaction time, and the value of B among the present invention is 0.2s.

具体地，Cons通常为车辆的最小转弯半径r_min，本发明采用工程化方法标定出Cons，通过对不同车位形式，不同泊入泊出方式，不同阶段下的车辆运动轨迹实车测试标定而得；Determine the preview distance of the tracking algorithm, obtain the preview point, and the calculation formula of the preview distance ld is

Specifically, Cons is usually the minimum turning radius r _min of the vehicle. The present invention adopts an engineering method to calibrate Cons, which is obtained through actual vehicle test calibration of vehicle motion trajectories in different parking spaces, different parking modes, and different stages. ;

Calculate the coordinates of the preview point in the vehicle coordinate system. The ordinate of the preview point in the vehicle coordinate system is the preview deviation. Specifically, the coordinates of the preview point in the earth coordinate system are converted to In the vehicle coordinate system, and the coordinate conversion formula is:

Among them: x', y' are the coordinates of the preview point in the converted vehicle coordinate system, x _lp and y _lp are the coordinates of the preview point in the earth coordinate system, θ is the coordinate rotation angle, x _car and y _car are the coordinates of the vehicle in the earth Coordinates under the system; y′ is the preview deviation

Taking the preview deviation as the control amount, the PID control algorithm is used to control the steering wheel angle in a closed loop. Specifically, according to the preview deviation, the wheel angle of the vehicle to be parked is calculated by PID, and the calculation formula of the wheel angle is:

Among them: δ _wheel is the wheel angle; err is the preview deviation; kp is the proportional control coefficient; kd is the differential control coefficient;

The requested steering wheel angle can be obtained by looking up the table according to the wheel angle.

It should be understood that although the various steps in the flow chart of FIG. 2 are displayed sequentially as indicated by the arrows, these steps are not necessarily executed sequentially in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and these steps can be executed in other orders. Moreover, at least some of the steps in FIG. 2 may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but may be executed at different times. The execution of these sub-steps or stages The order is not necessarily performed sequentially, but may be performed alternately or alternately with at least a part of other steps or sub-steps or stages of other steps.

Embodiment two

As shown in Figure 3, the present embodiment provides a planning device for an automatic parking path, including an acquisition module 3 and a path planning module 4, the acquisition module 3 acquires parking space information and vehicle information, and the parking space information includes four parking spaces. The coordinates of the vertex, the vehicle information includes the initial position coordinates of the vehicle, the parking mode of the vehicle, the body length of the vehicle, the width of the vehicle body, the front overhang length of the vehicle and the rear overhang length of the vehicle; the path planning module 4 according to Parking space information, vehicle information and obstacle information are used to obtain the parking trajectory.

As a further preferred solution, the acquisition module 3 includes a first acquisition unit 5, a first determination unit 6, a second determination unit 7, a third determination unit 8, a fourth determination unit 9 and a smoothing processing unit 10, the first An acquisition unit 5 determines the target parking point of the vehicle according to the vehicle information and the parking space information; in the P coordinate system, the first determination unit 6 takes the target parking point as the end point and the initial position of the vehicle as the starting point, and uses the Dubins path planning method to determine The trajectory curve combination between the target parking point and the initial position of the vehicle; the second determination unit 7 obtains the vehicle profile parameters according to the vehicle information; the third determination unit 8 obtains the parking space boundary according to the parking space information; the fourth determination unit 9 According to the vehicle profile parameters and the parking space boundary, the trajectory curve in the trajectory curve combination is verified by boundary constraint verification and dynamic constraint in turn, and the trajectory curve that meets the boundary constraint and dynamic constraint is confirmed as the parking curve; the smoothing process Unit 10 uses the Bessel formula to smooth the parking curve.

It should be noted that each module in the automatic parking path planning device can be fully or partially realized by software, hardware or a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, and can also be stored in the memory of the computer device in the form of software, so that the processor can invoke and execute the corresponding operations of the above-mentioned modules.

Embodiment Three

This embodiment provides a computer device. The computer device may be a server. As shown in FIG. 4 , the computer device includes a processor, a memory, a network interface, and a database connected through a system bus. Wherein, the processor of the computer device is used to provide calculation and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, computer programs and databases. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The database of the computer device is used to store operation behavior data, commodity information data and the like. The network interface of the computer device is used to communicate with an external terminal via a network connection. When the computer program is executed by the processor, the steps of the method for finding items are realized.

Those skilled in the art can understand that the structure shown in Figure 4 is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation to the computer equipment on which the solution of the application is applied. The specific computer equipment can be More or fewer components than shown in the figures may be included, or some components may be combined, or have a different arrangement of components.

In other embodiments, a computer device is provided, including a memory and a processor, the memory stores a computer program, and the processor implements the following steps when executing the computer program:

Acquire parking space information and vehicle information, the parking space information includes the coordinates of the four vertices of the parking space; the vehicle information includes the initial position coordinates of the vehicle, the way the vehicle is parked, the length of the vehicle body, the width of the vehicle body, the front of the vehicle The overhang length and the rear overhang length of the vehicle; the parking trajectory is obtained according to the parking space information, vehicle information and obstacle information; the vehicle is controlled by the tracking algorithm to realize the parking trajectory tracking.

In some other embodiments, when the processor executes the computer program, the step of obtaining the parking track according to the parking space information, the vehicle information and the obstacle information specifically includes the following steps: determining the target parking point of the vehicle according to the vehicle information and the parking space information; In the P coordinate system, with the target parking point as the end point and the initial position of the vehicle as the starting point, the Dubins path planning method is used to determine the trajectory curve combination between the target parking point and the initial position of the vehicle; the vehicle profile parameters are obtained according to the vehicle information; according to the parking space information Obtain the parking space boundary; perform boundary constraint verification and dynamic constraint verification on the trajectory curve in the trajectory curve combination according to the vehicle contour parameters and the parking space boundary, and confirm the trajectory curve that meets the boundary constraint and dynamic constraint as the parking curve; The Bessel formula is used to smooth the parking curve; for the smoothed parking curve, the vehicle motion model is used to decompose the vehicle's motion information, that is, the parking trajectory is obtained.

In some other embodiments, when the processor executes the computer program, the step of determining the target parking spot of the vehicle according to the vehicle information and the parking space information specifically includes the following steps: and the rear overhang length of the vehicle to determine the center of the rear axle of the vehicle; according to the coordinates of the four vertices of the parking space, determine the projection point of the center of the rear axle of the vehicle on the parking space when the vehicle is parked in the parking space, and the projection point is the target parking point.

In some other embodiments, when the processor executes the computer program, the step of using the tracking algorithm to control the vehicle and realize the tracking of the parking trajectory includes the following steps: performing lateral control on the vehicle to obtain the steering wheel angle of the vehicle; For longitudinal control, the control mode, longitudinal distance and longitudinal speed control module of the vehicle are obtained; the parking trajectory is corrected according to the steering wheel angle, the control mode of the vehicle, the longitudinal distance and the longitudinal speed control module, and the parking path is obtained.

采用工程化方法标定出Cons，通过对不同车位形式，不同泊入泊出方式，不同阶段下的车辆运动轨迹实车测试标定而得；计算预瞄点在车辆坐标系下的坐标，预瞄点在车辆坐标系下的纵坐标即为预瞄偏差；以预瞄偏差作为控制量，使用PID控制算法闭环控制方向盘转角。In some other embodiments, when the processor executes the computer program, the step of laterally controlling the vehicle and obtaining the steering wheel angle of the vehicle specifically includes the following steps: determining the quadratic term of the preview distance formula of the pure tracking algorithm according to the body parameters of the vehicle The first-order coefficients are 1/6 and 1/5 respectively; determine the preview distance of the tracking algorithm, obtain the preview point, and the calculation formula of the preview distance ld is

The Cons is calibrated by engineering methods, which are obtained through the actual vehicle test calibration of different parking spaces, different parking in and out methods, and vehicle motion trajectories at different stages; calculate the coordinates of the preview point in the vehicle coordinate system, and the preview point The ordinate in the vehicle coordinate system is the preview deviation; the preview deviation is used as the control amount, and the PID control algorithm is used to control the steering wheel angle in a closed loop.

Embodiment Four

This embodiment provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the following steps are implemented: obtaining parking space information and vehicle information, and the parking space information includes four vertices of the parking space The coordinates of the vehicle; the vehicle information includes the initial position coordinates of the vehicle, the way the vehicle is parked, the length of the vehicle body, the width of the vehicle body, the length of the front overhang of the vehicle and the length of the rear overhang of the vehicle; according to the parking space information, vehicle information and obstacles The parking trajectory is obtained from the object information; the vehicle is controlled by the tracking algorithm to realize the parking trajectory tracking.

In some other embodiments, the computer program is executed by the processor to realize the step of obtaining the parking trajectory according to the parking space information, the vehicle information and the obstacle information, which specifically includes the following steps: determining the target parking point of the vehicle according to the vehicle information and the parking space information; In the P coordinate system, with the target parking point as the end point and the initial position of the vehicle as the starting point, the Dubins path planning method is used to determine the trajectory curve combination between the target parking point and the initial position of the vehicle; the vehicle profile parameters are obtained according to the vehicle information; according to the parking space information Obtain the parking space boundary; perform boundary constraint verification and dynamic constraint verification on the trajectory curve in the trajectory curve combination according to the vehicle contour parameters and the parking space boundary, and confirm the trajectory curve that meets the boundary constraint and dynamic constraint as the parking curve; The Bessel formula is used to smooth the parking curve; for the smoothed parking curve, the vehicle motion model is used to decompose the vehicle's motion information, that is, the parking trajectory is obtained.

In some other embodiments, the computer program is executed by the processor to realize the step of determining the target parking spot of the vehicle according to the vehicle information and the parking space information, which specifically includes the following steps: according to the vehicle body length, vehicle body width, and vehicle front overhang length and the rear overhang length of the vehicle to determine the center of the rear axle of the vehicle; according to the coordinates of the four vertices of the parking space, determine the projection point of the center of the rear axle of the vehicle on the parking space when the vehicle is parked in the parking space, and the projection point is the target parking point.

In some other embodiments, the computer program is executed by the processor to realize the steps of using the tracking algorithm to control the vehicle and realize the tracking of the parking trajectory, which specifically includes the following steps: performing lateral control on the vehicle to obtain the steering wheel angle of the vehicle; For longitudinal control, the control mode, longitudinal distance and longitudinal speed control module of the vehicle are obtained; the parking trajectory is corrected according to the steering wheel angle, the control mode of the vehicle, the longitudinal distance and the longitudinal speed control module, and the parking path is obtained.

采用工程化方法标定出Cons，通过对不同车位形式，不同泊入泊出方式，不同阶段下的车辆运动轨迹实车测试标定而得；计算预瞄点在车辆坐标系下的坐标，预瞄点在车辆坐标系下的纵坐标即为预瞄偏差；以预瞄偏差作为控制量，使用PID控制算法闭环控制方向盘转角。In some other embodiments, the computer program is executed by the processor to realize the lateral control of the vehicle and the step of obtaining the steering wheel angle of the vehicle, which specifically includes the following steps: determining the quadratic term of the preview distance formula of the pure tracking algorithm according to the body parameters of the vehicle The first-order coefficients are 1/6 and 1/5 respectively; determine the preview distance of the tracking algorithm, obtain the preview point, and the calculation formula of the preview distance ld is

The Cons is calibrated by engineering methods, which are obtained through the actual vehicle test calibration of different parking spaces, different parking in and out methods, and vehicle motion trajectories at different stages; calculate the coordinates of the preview point in the vehicle coordinate system, and the preview point The ordinate in the vehicle coordinate system is the preview deviation; the preview deviation is used as the control amount, and the PID control algorithm is used to control the steering wheel angle in a closed loop.

Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be realized through computer programs to instruct related hardware, and the programs can be stored in a non-volatile computer-readable storage medium When the program is executed, it may include the processes of the embodiments of the above-mentioned methods. Wherein, any reference to memory, database or other media used in the various embodiments provided by the application may include non-volatile memory and/or volatile memory, wherein: (1) non-volatile memory Memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory; (2) volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms such as Static RAM (SRAM), Dynamic RAM (DRAM), Synchronous DRAM (SDRAM), Double Data Rate SDRAM (DDRSDRAM), Enhanced SDRAM (ESDRAM), Synchronous Chain Synchlink DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

The above-mentioned embodiment is only a preferred embodiment of the present invention, and cannot be used to limit the protection scope of the present invention. Any insubstantial changes and substitutions made by those skilled in the art on the basis of the present invention belong to the scope of the present invention. Scope of protection claimed.

Claims (3)

1. A method for planning and tracking control of an automatic parking path, characterized in that, comprising the steps of: Obtain parking space information and vehicle information, the parking space information includes the coordinates of the four vertices of the parking space; the vehicle information includes the initial position coordinates of the vehicle, the way the vehicle is parked, the length of the vehicle body, the width of the vehicle body, the front of the vehicle Overhang length and rear overhang length of the vehicle; Process the acquired data, calculate the boundary constraints, calculate the target parking space and the actual position of the current vehicle through coordinate conversion, and determine the target parking point; Obtain the parking trajectory according to the parking space information, vehicle information and obstacle information; Use the tracking algorithm to control the vehicle and complete the tracking of the planned trajectory; Acquisition of parking space information and vehicle information includes the following steps: Obtain the parking space information through the cloud server end, the information includes the coordinates of the four vertices of the parking space and the parking mode instruction; read the vehicle information of the car stored in the local storage medium: A, B, C, and D are the four vertices of the parking space, and the coordinates of the four points A, B, C, and D in the earth coordinate system are (x _A ,y _A ), (x _B ,y _B ) _{. _ _ _ _ _ _ _} Dep = y _D -y _A = y _C -y _B ; Obtaining the parking trajectory according to the parking space information, vehicle information and obstacle information includes the following steps: Determine the target parking point of the vehicle according to the vehicle information and parking space information; in the geodetic coordinate system, the coordinates of the target parking point x _o =Wid/2=(x _B -x _A )/2, y _o =Dep/2-l _r - l _s ＝(y _D -y _A )/2-l _r -l _s ; among them, l _r is the length of the rear overhang of the vehicle, and l _s is the reserved safety distance at the rear; In the P coordinate system, with the target parking point as the end point and the initial vehicle position as the starting point, the DuBins path planning method is used to determine the trajectory curve combination between the target parking point and the initial position of the vehicle. Specifically, the CSC type is curve + straight line +Curve combination, assuming that the starting point is S=(x _s ,y _s ,α1), the end point g=(x _g ,y _g ,α2), and the minimum turning radius is r _min , then the calculation formulas of the three sections of paths are:

Then the path L _CSC =p1+p2+p3; Get the vehicle profile parameters according to the vehicle information; Get the parking space boundary according to the parking space information; According to the vehicle contour parameters and the parking space boundary, the trajectory curves in the trajectory curve combination are verified by boundary constraints and dynamic constraints in turn, and the trajectory curves that meet the boundary constraints and dynamic constraints are confirmed as parking curves; Use the Bessel formula to smooth the parking curve; For the smoothed parking curve, the vehicle motion model is used to decompose the vehicle motion information, that is, the parking trajectory is obtained; The point vector on the above parking curve at any time is (x _r (t), y _r (t), α), where x _r (t) is the abscissa of the trajectory, y _r (t) is the ordinate of the trajectory, α is the tangential angle of the trajectory; Assuming that the center point of the rear axle of the vehicle tracks the parking curve, the coordinates of the center point of the rear axle of the vehicle at any time are also the point coordinates on the curve. According to the vehicle kinematics equation, without considering the side slip of the vehicle at low speeds, then:

Among them, L is the wheelbase of the front and rear axles of the vehicle, V is the vehicle speed, and β is the steering angle of the front wheels; Determining the target parking point of the vehicle according to the vehicle information and the parking space information includes the following steps: Determine the center of the rear axle of the vehicle according to the length of the vehicle body, the width of the vehicle body, the length of the front overhang of the vehicle, and the length of the rear overhang of the vehicle. Specifically, taking the center of the outer side of the left front wheel as the origin, the coordinates of the rear axle point x _r = L _C /2, y _r =L _l -L _r -L _f , wherein, L _C is the width of the vehicle, L _l is the length of the vehicle, L _r and L _f are the lengths of the rear suspension and the front suspension of the vehicle respectively; According to the coordinates of the four vertices of the parking space, determine the projection point of the rear axle center of the vehicle on the parking space when the vehicle is parked in the parking space, and this projection point is the target parking point; The coordinate value of the center of the rear axle; Using the tracking algorithm to control the vehicle and realize the tracking of the parking trajectory includes the following steps: Perform lateral control on the vehicle to obtain the steering wheel angle of the vehicle; Perform longitudinal control on the vehicle to obtain the control mode, longitudinal distance and longitudinal speed control module of the vehicle; According to the steering wheel angle, the control mode of the vehicle, the longitudinal distance and the longitudinal speed control module, the parking trajectory is corrected to obtain the parking path; Carrying out lateral control to the vehicle, obtaining the steering wheel angle of the vehicle includes the following steps: According to the body parameters of the vehicle, the quadratic coefficient A and the primary coefficient B of the preview distance formula of the pure tracking algorithm are determined to be 1/6 and 1/5 respectively. When calculating:

Among them, a _max is the maximum deceleration, which is 3m/s^2 in this case; B is the reaction time; Determine the preview distance of the tracking algorithm, obtain the preview point, and the calculation formula of the preview distance ld is

Cons is usually the minimum turning radius r _min of the vehicle. Cons is calibrated by engineering methods. It is obtained through actual vehicle test calibration of vehicle motion trajectories in different parking spaces, different parking modes, and different stages; Calculate the coordinates of the preview point in the vehicle coordinate system. The ordinate of the preview point in the vehicle coordinate system is the preview deviation. Specifically, the coordinates of the preview point in the earth coordinate system are converted to In the vehicle coordinate system, and the coordinate conversion formula is:

Among them: x', y' are the coordinates of the preview point in the converted vehicle coordinate system, x _lp and y _lp are the coordinates of the preview point in the earth coordinate system, θ is the coordinate rotation angle, x _car and y _car are the coordinates of the vehicle in the earth coordinates under the system; y′ is the preview deviation; Taking the preview deviation as the control amount, the PID control algorithm is used to control the steering wheel angle in a closed loop. Specifically, according to the preview deviation, the wheel angle of the vehicle to be parked is calculated by PID, and the calculation formula of the wheel angle is:

Among them: δ _wheel is the wheel angle; err is the preview deviation; k _p is the proportional control coefficient; k _d is the differential control coefficient; The requested steering wheel angle can be obtained by looking up the table according to the wheel angle. 2. A computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the automatic parking path planning and tracking control method according to claim 1 is implemented. 3. A computer device, characterized in that it comprises the computer-readable storage medium according to claim 2 and a processor connected to the readable storage medium, the readable storage medium stores a computer program, the When the computer program is executed by the processor, the automatic parking path planning and tracking control method according to claim 1 is realized.

Images