Automatic driving method and system and readable storage medium

Abstract

The invention provides an automatic driving method and system and a readable storage medium, and the method comprises the steps: S1, obtaining point cloud data through a laser radar, and outputting the boundary information of an obstacle; s2, acquiring vehicle position information based on a Beidou positioning module; s3, the obstacle boundary information and the vehicle position information are input into a decision-making module, driving behaviors are determined through a preset state machine, and the driving behaviors comprise driving in a lane, lane-borrowing overtaking and decelerating for giving way; s4, track planning is carried out according to the driving behavior; s5, obtaining a track for control based on the smooth path and the planned speed; s6, generating a steering wheel turning angle by adopting an LQR algorithm based on a kinematic model on the basis of a track for control, and generating an acceleration compensation instruction by adopting a double-PID controller in which a position PID and a speed PID are connected in series; and S7, the vehicle executing mechanism controls the vehicle to run through the steering wheel rotation angle and the acceleration compensation instruction.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of automatic driving, and particularly relates to an automatic driving method, system and readable storage medium. BACKGROUND

[0002] With the continuous development of automatic driving technology, intelligent transportation systems are increasingly applied in various scenarios, especially in closed areas such as industrial parks, logistics parks and commercial parks, where automatic driving technology has shown great potential. Automatic driving vehicles can complete complex driving tasks without human intervention by using sensors, computer vision, deep learning and other technologies, thereby improving work efficiency, reducing human errors, optimizing traffic flow and reducing accident rates. Automatic driving vehicles in parks often need to travel in a relatively complex environment. These environments usually include different traffic flows, complex traffic signs, obstacles, and possibly pedestrians or other non-motor vehicles. Although existing technologies have made some progress in automatic driving perception, path planning and decision-making, there are still many challenges in scenarios specific to parks.

[0003] A related technology provides an automatic driving teaching system, method and device, relating to the technical field of automatic driving, wherein the automatic driving teaching method includes selecting a teaching mode, then determining the configuration parameters of the vehicle coordinate system conversion module, the sensor driving module, the perception module, the map positioning module and the road planning module based on the teaching mode, and finally driving the vehicle to automatically drive based on the configuration parameters of the coordinate system conversion module, the sensor driving module, the perception module, the map positioning module and the planning module.

[0004] A related technology provides a three-dimensional environment perception algorithm for multiple sensors in an automatic driving scenario, mainly aiming at the obstacle recognition problem in complex environments. The algorithm effectively reduces noise and sampling density by performing multi-stage preprocessing on point cloud data, including straight-through filtering, statistical filtering and voxel filtering. In addition, a ground plane fitting algorithm is used to filter out ground point clouds, reducing the interference of invalid information on the detection result. By converting the processed point cloud data into an aerial view and combining a ResNet model to extract key features.

[0005] A related technology provides a virtual driving vehicle test system with high immersion human-computer interaction, which simulates various interactive scenarios, including emergency and non-standard operations. The test system can help discover and correct design flaws that may cause safety problems.

[0006] However, the above technical solution is difficult to cope with high-density and rapidly changing traffic flow. In addition, the existing redundancy and safety mechanism has insufficient fault tolerance capability in extreme cases, which may lead to the system's response being not timely in the event of a failure or an emergency. Therefore, how to improve the stability, real-time performance and safety of the autonomous driving system in the park environment is still a major problem faced by current technology. SUMMARY

[0007] The present application provides an autonomous driving method, system and readable storage medium, which can solve the technical problem that the above technical solution is difficult to cope with high-density and rapidly changing traffic flow. In addition, the existing redundancy and safety mechanism has insufficient fault tolerance capability in extreme cases, which may lead to the system's response being not timely in the event of a failure or an emergency.

[0008] The technical solution provided by the embodiments of the present application is as follows: In one aspect, the embodiments of the present application provide an autonomous driving method, characterized in that it comprises: S1. Obtain point cloud data by laser radar, and output obstacle boundary information; S2. Obtain vehicle position information based on a Beidou positioning module; S3. Input the obstacle boundary information and the vehicle position information into a decision module, and determine a driving behavior by a preset state machine, wherein the driving behavior includes driving in a lane, overtaking by borrowing a lane and slowing down to yield; S4. Plan a trajectory according to the driving behavior, including dynamically scattering points on a SL graph according to a curvature change rate, generating an initial path by dynamic programming, and then smoothing the initial path by quadratic programming to obtain a smooth path; and planning a speed based on the speed state of the vehicle on an ST graph, including the expected speed, the speed on a curve, the vehicle speed limit and the terminal speed, to obtain a planned speed; S5. Obtain a trajectory available for control based on the smooth path and the planned speed; S6. Generate a steering wheel turning angle based on the trajectory available for control by using a LQR algorithm based on a kinematic model, and generate an acceleration compensation instruction by using a double-PID controller in series with a position PID and a speed PID; S7. Send the steering wheel turning angle and the acceleration compensation instruction to a vehicle control module through a CAN bus, and control the vehicle to drive by the steering wheel turning angle and the acceleration compensation instruction through a vehicle execution mechanism.

[0009] In an optional embodiment, planning a trajectory according to the driving behavior comprises: If the driving behavior is to drive in a lane, it is determined whether there is an obstacle that does not affect normal driving in the current lane; If yes, avoid the obstacle in the lane, and if no, drive in the current lane; If the obstacle cannot be avoided in the current lane, determine whether the obstacle can be bypassed to overtake; If the obstacle cannot be bypassed to overtake, drive in the current lane at a reduced speed, and if the obstacle can be bypassed to overtake, enter a bypass overtaking driving state.

[0010] In an optional embodiment, trajectory planning according to the driving behavior further comprises: If the driving behavior is bypass overtaking, determine whether there is an obstacle in the right lane that hinders normal driving; If no, drive to the right lane and jump into a lane driving state; If yes, continue overtaking.

[0011] In an optional embodiment, if an obstacle appears in the left lane during the process of continuing overtaking, determine whether the vehicle can accelerate to eliminate the influence; If no, determine whether the vehicle can drive at a reduced speed to the right lane, and if no, stop.

[0012] In an optional embodiment, S1. The point cloud data is obtained by a laser radar, and the obstacle boundary information is output, comprising: obtaining the point cloud data by a laser radar, sequentially performing voxel filtering processing, ground point cloud elimination processing and Euclidean clustering processing to obtain the obstacle boundary information.

[0013] In an optional embodiment, the grid size of the voxel filtering processing is 0.2±0.05m, and the distance threshold of the Euclidean clustering is 0.5±0.1m; the ground point cloud elimination adopts a RANSAC plane fitting algorithm.

[0014] In an optional embodiment, if the driving behavior is driving in the lane and there is no obstacle that hinders normal driving, the distance between the obstacle and the vehicle is determined, and when the distance between the obstacle and the vehicle is ≤5m and the obstacle occupies a lane width ≥30%, the vehicle drives in the lane to avoid the obstacle; If the obstacle cannot be avoided in the current lane, determine whether there is no obstacle within 200m of the opposite lane and whether the road marking is a dashed line, if there is no obstacle within 200m of the opposite lane and the road marking is a dashed line, bypass overtaking is performed; If there is an obstacle within 200m of the opposite lane and the road marking is a solid line, the vehicle is stopped at a reduced speed.

[0015] In an optional embodiment, a double-PID controller in series connection of a position PID and a speed PID is used to generate an acceleration compensation instruction, comprising: Obtain the current actual speed; The position PID is used to obtain a position error, and a speed compensation is obtained according to the position error; The speed PID is used to obtain a speed error; A deviation between a current actual speed and an expected speed is obtained; The acceleration compensation instruction is obtained based on the deviation between the current actual speed and the expected speed and the speed compensation.

[0016] In still another aspect, a readable storage medium is provided, and the readable storage medium has stored thereon the automatic driving method according to any one of the preceding aspects.

[0017] In yet another aspect, an automatic driving system is provided, and the automatic driving system comprises the readable storage medium according to the preceding aspect and a positioning module configured to obtain point cloud data by using a laser radar and output obstacle boundary information; A perception module is configured to obtain vehicle position information based on the Beidou positioning module; A decision module is configured to input the obstacle boundary information and the vehicle position information into the decision module, determine a driving behavior by using a preset state machine, and the driving behavior comprises driving in a lane, overtaking by using a lane and slowing down to give way; A planning module is configured to perform trajectory planning according to the driving behavior, including dynamically scattering points on a SL graph according to a curvature change rate, generating an initial path by using dynamic programming, and obtaining a smooth path by using quadratic programming smoothing; performing speed planning on an ST graph based on a speed state of the vehicle, including an expected speed, a curve speed, a vehicle speed limit and a terminal speed, to obtain a planned speed; obtaining a controllable trajectory based on the smooth path and the planned speed; generating a steering wheel turning angle by using a kinematic model-based LQR algorithm based on the controllable trajectory, and generating an acceleration compensation instruction by using a double-PID controller in series with a position PID and a speed PID; A control module is configured to control vehicle driving by using the steering wheel turning angle and the acceleration compensation instruction.

[0018] The automatic driving method provided by the embodiments of the present application has at least the following beneficial effects: The automatic driving method provided by the embodiments of the present application can cope with high-density and rapidly changing traffic flow, and the driving behavior is determined by using a preset state machine, and the trajectory planning is performed according to the driving behavior, so that the response capability of the system in the event of a fault or an unexpected situation is improved, and the stability, real-time performance and safety of the automatic driving in a park environment are improved. BRIEF DESCRIPTION OF DRAWINGS

[0019] The above and other objects, features and advantages of the present disclosure will become more apparent from the following detailed description taken in conjunction with the accompanying drawings, in which like reference characters refer to the like parts throughout the figures, and in which the exemplary embodiments of the present disclosure are shown.

[0020] Figure 1 An automatic driving method flowchart is provided for the embodiments of the present application.

[0021] Figure 2 A flowchart for processing point cloud data.

[0022] Figure 3 A decision module decision flowchart.

[0023] Figure 4 A path planning flowchart.

[0024] Figure 5 A control module control diagram.

[0025] Figure 6 An automatic driving system structure diagram. DETAILED DESCRIPTION

[0026] Embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure is more thorough and complete, and to fully convey the scope of the present disclosure to those skilled in the art.

[0027] The term "comprising" and variations thereof as used herein mean "including but not limited to". Unless otherwise specified, the term "or" means "and / or". The term "based on" means "based at least in part on". The terms "one example embodiment" and "an embodiment" mean "at least one example embodiment". The term "another embodiment" means "at least one additional embodiment". The terms "first", "second", and the like can refer to different or the same objects. Other explicit and implicit definitions can also be included below.

[0028] Please refer to Figure 1 In one aspect, the embodiments of the present application provide an automatic driving method, comprising steps S1-S2.

[0029] S1. Obtain point cloud data by laser radar, and output obstacle boundary information.

[0030] S2. Obtain vehicle position information based on a Beidou positioning module.

[0031] S3. Input the obstacle boundary information and the vehicle position information into a decision module, determine a driving behavior through a preset state machine, and the driving behavior includes driving in a lane, passing by borrowing a lane, and slowing down to yield.

[0032] S4. Trajectory planning according to driving behavior, including dynamic scattering on SL graph according to curvature change rate, generating initial path through dynamic planning, and smoothing through quadratic programming to obtain smooth path; speed planning on ST graph based on vehicle speed state, including expected speed, curve speed, vehicle speed limit and terminal speed, to obtain planned speed.

[0033] S5. Generating trajectory for control based on smooth path and planned speed.

[0034] S6. Generating steering wheel angle based on trajectory for control through LQR algorithm based on kinematic model, and generating acceleration compensation instruction through double PID controller in series of position PID and speed PID.

[0035] S7. Sending steering wheel angle and acceleration compensation instruction to vehicle control module through CAN bus, and controlling vehicle driving through steering wheel angle and acceleration compensation instruction.

[0036] The automatic driving method provided by the embodiment has at least the following beneficial effects: The automatic driving method provided by the embodiment can cope with high-density and rapidly changing traffic flow, and driving behavior is determined through preset state machine, trajectory planning is performed according to driving behavior, which can improve the response capability of the system in the presence of faults or sudden conditions, and improve the stability, real-time performance and safety of automatic driving in the park environment.

[0037] In step S1, point cloud data is acquired through laser radar, and obstacle boundary information is output.

[0038] Please refer to Figure 2 In an optional embodiment, S1. The point cloud data is acquired through laser radar, and the obstacle boundary information is output, including: acquiring point cloud data through laser radar, sequentially performing voxel filtering processing, ground point cloud removal processing and Euclidean clustering processing, to obtain obstacle boundary information.

[0039] After acquiring the point cloud of the radar, voxel filtering is first performed to reduce the density of the point cloud, then the ground point cloud is removed to prevent affecting the clustering effect, and then Euclidean clustering is used for obstacle fitting, and finally the boundary information of the obstacle is output.

[0040] In an optional embodiment, the grid size of the voxel filtering processing is 0.2±0.05m, and the distance threshold of the Euclidean clustering is 0.5±0.1m; the RANSAC plane fitting algorithm is used for ground point cloud removal.

[0041] Further, as an example, the point cloud space can be divided into a three-dimensional cubic grid (voxel), the grid size is 0.2±0.05m, for all points in each voxel, the weighted centroid is calculated, and based on the output centroid point, the down-sampling point cloud is output.

[0042] The ground point cloud rejection adopts a RANSAC plane fitting algorithm, as an example, 3 points are randomly selected to generate an initial plane model: $ax + by + cz + d = 0$, the distance of all points to the plane is calculated, the inliers (points with a distance of ≤0.05m) are screened, and the plane is iterated 100 times, the plane ground point cloud segmentation is retained, the points within 0.1m below and 0.3m above the plane are extracted as ground points, the ground point cloud is deleted, and the obstacle candidate point set is retained.

[0043] The distance threshold of the Euclidean clustering processing is 0.5±0.1m, it should be noted that the voxel filtering processing, the ground point cloud rejection processing and the Euclidean clustering processing are prior art, and a person skilled in the art can operate according to the processing mode in the prior art, and the present application will not be described one by one.

[0044] In step S2, the vehicle position information can be obtained based on the Beidou positioning module, and the vehicle position information can include the longitude and latitude information of the vehicle, as long as the position of the vehicle can be displayed.

[0045] In step S3, the obstacle boundary information and the vehicle position information are input into the decision module, and the driving behavior is determined through the preset state machine, and the driving behavior includes driving in the lane, overtaking by borrowing the lane and slowing down to give way.

[0046] The method provided by the embodiment of the present application is suitable for similar parks, such as closed areas such as industrial parks, logistics parks and commercial parks, therefore, the embodiment of the present application presets the driving behavior, including driving in the lane, overtaking by borrowing the lane and slowing down to give way. And set the path planning in these states, provide the basis for path planning in a specific state (closed state) in a specific area (industrial park, logistics park and commercial park). Specifically, the obstacle boundary information and the vehicle position information can be input into the decision module, and the state machine is preset in the decision module, and the obstacle boundary information and the vehicle position information can be compared with the preset state machine to determine the driving behavior as driving in the lane, overtaking by borrowing the lane or slowing down to give way.

[0047] In step S4, trajectory planning is performed according to the driving behavior, including dynamically scattering points on the SL graph according to the curvature change rate, and generating a smooth path through secondary planning after generating an initial path through dynamic planning; on the ST graph, speed planning is performed based on the speed state of the vehicle, including the expected speed, the speed on a curve, the vehicle speed limit and the terminal speed, to obtain the planned speed.

[0048] Further, the step of dynamic programming in the embodiment of the present application comprises: Step 1: Establish SL coordinate system and reference line.

[0049] Wherein, the definition of SL coordinate system is: taking the road reference line (such as the lane center line) as the reference, the S axis is the arc length (longitudinal distance) along the reference line, and the L axis is the lateral distance (lateral distance) perpendicular to the reference line, so as to convert the trajectory planning problem into a two-dimensional path search in the SL plane.

[0050] Determine the reference line: according to the high-precision map or the real-time perceived road boundary, determine the reference line (such as the current lane center line) of the current scene as the reference for trajectory planning.

[0051] Step 2: Discretization of state space (dynamic scattering).

[0052] Dynamic programming needs to discretize the continuous SL space into a limited number of "nodes" (states), and the distribution of nodes needs to be dynamically adjusted in combination with the curvature rate of change (i.e. "dynamic scattering"): State definition: the state of each node includes the discrete position of the node in the S direction (arc length along the reference line); the discrete position in the L direction (lateral distance); and the lateral velocity (optional, used to reflect the smoothness of driving behavior).

[0053] Dynamic scattering rule: in the S direction: adjust the node density in the S direction according to the curvature rate of change of the reference line (or the longitudinal velocity demand of the driving behavior), if the curvature rate of change of the current S section is large (such as a curve) or the driving behavior is biased towards being aggressive (such as accelerating through), the S direction node is more dense; In the L direction: adjust the node density in the L direction according to the lateral curvature rate of change (or the lateral flexibility of the driving behavior), if the lane needs to be changed frequently (the lateral curvature changes greatly) or the driving behavior is biased towards flexibility (such as sudden steering), the L direction node is more dense; Step 3: Define state transition constraints.

[0054] State transition refers to the feasibility constraints from the current node to the next node, which needs to match the vehicle dynamics and driving behavior preference: dynamics constraints: curvature constraints: the curvature of the transition path needs to be within the physical limits of the vehicle (such as the curvature corresponding to the maximum steering angle); curvature rate of change constraints: the curvature rate of change between adjacent nodes needs to be less than a threshold (to avoid sudden steering and reflect driving stability); lateral acceleration constraints can be ignored.

[0055] Step 4: Construct the cost function.

[0056] The cost function is used to evaluate the "good or bad" of the path, which needs to integrate safety, smoothness, driving behavior preference and other goals. The formula can be expressed as: (Cost = w_1 cdot Cost_{safety} + w_2cdot Cost_{smoothness} + w_3cdot Cost_{behavior} + w_4 cdot Cost_{efficiency}).

[0057] Where each item and meaning: (Cost_{safety}): distance from obstacles (the smaller the distance, the greater the cost); (Cost_{smoothness}): integral of curvature rate of change (the greater the value, the more bumpy the path, the greater the cost); (Cost_{behavior}): deviation from the typical driving behavior trajectory (such as according to historical data, aggressive driving is more inclined to large L value lane changing, and conservative driving is closer to the reference line); (Cost_{efficiency}): S-direction distance to the target point (avoid detours, the cost increases moderately with the increase of S); (w_1\sim w_4): weight, adjusted according to driving behavior preference (such as aggressive driving can reduce (w_2) and increase (w_4)).

[0058] After generating the initial path based on the above cost function, the smooth path is obtained by quadratic programming smoothing; based on the speed state of the vehicle on the ST graph, including the expected speed, the speed on the curve, the vehicle speed limit and the terminal speed, the planned speed is obtained.

[0059] It should be noted that in Apollo and other autonomous driving systems, the SL graph is a two-dimensional coordinate system for path planning in the Frenet coordinate system, and the coordinate system is defined as follows: the horizontal axis (S): the longitudinal path distance of the vehicle along the reference line. The vertical axis (L): the lateral offset of the vehicle relative to the reference line. The core function is to project the vehicle driving path onto a two-dimensional plane. It can cooperate with the ST graph (time-distance coordinate system) to realize dynamic path optimization. Support obstacle projection and avoidance decision. Determine the initial path through dynamic planning, etc.

[0060] In kinematic analysis, the ST graph intuitively displays the motion state of an object through the coordinate system: the horizontal axis is time (t) and the vertical axis is distance (s). Uniform straight line motion is represented as a tilted straight line passing through the origin, and the slope corresponds to the speed value. Application functions: calculate the distance change in a certain time period. Determine the time required to pass a certain distance. Compare the motion speeds of different objects by slope. The ST graph only reflects the relationship between motion parameters, not the actual motion trajectory. The expected speed refers to the expected speed that can be reached during driving, See Figure 3In an optional embodiment, trajectory planning according to driving behavior includes: if the driving behavior is driving in the lane, determining whether there is an obstacle in the current lane that does not affect normal driving; If yes, avoid the obstacle in the lane, and if no, drive in the current lane; If the obstacle cannot be avoided in the current lane, determine whether it can be overtaken by borrowing a lane; If it cannot be overtaken by borrowing a lane, drive in the current lane at a reduced speed, and if it can be overtaken by borrowing a lane, enter the lane borrowing overtaking driving state.

[0061] In an optional embodiment, trajectory planning according to driving behavior further includes: If the driving behavior is overtaking by borrowing a lane, determine whether there is an obstacle in the right lane that affects normal driving; If no, drive to the right lane and jump into the lane driving state; If yes, continue overtaking.

[0062] See 4, in an optional embodiment, after generating an initial path through dynamic planning, a smooth path is obtained through quadratic programming smoothing, including: after obtaining the output result of the decision module, planning the trajectory according to the current positioning information and perception information. Path planning first scatters points on the SL graph, obtains the path with the minimum value (i.e. the initial path of the present application), and then uses QP to smooth it to obtain a smooth path.

[0063] Wherein, QP refers to quadratic programming, which is a mathematical tool for smoothing the path through optimization method. The role of QP in path smoothing is to adjust the path parameters (such as curvature, steering angle, etc.) through quadratic programming to make the generated trajectory meet the following conditions: curvature continuity: avoid sudden changes in steering wheel, ensure smooth transition of driving trajectory; constraint conditions: meet traffic rules (such as lane line restrictions), obstacle avoidance and other constraints. The implementation is: taking the OSQP library as an example, the QP problem is usually converted into a convex optimization problem for solution, and the objective function and constraint conditions are realized through adjustment of parameters to achieve path smoothing. For example, the objective function may include path length, steering angle change rate and other indicators, and the constraint conditions involve lane width, obstacle boundary and other restrictions.

[0064] In an optional embodiment, if an obstacle appears in the left lane during the process of continuing overtaking, determine whether the vehicle can accelerate to eliminate the impact; If no, determine whether it can drive at a reduced speed to the right lane, and if no, stop.

[0065] In an alternative embodiment, if the driving behavior is driving in the lane, and there is no obstacle affecting normal driving, the distance between the obstacle and the vehicle is determined, and if the distance between the obstacle and the vehicle is ≤5m and the lane width occupied by the obstacle is ≥30%, the vehicle drives in the lane to avoid the obstacle; If the obstacle cannot be avoided in the current lane, it is determined whether there is no obstacle within 200m of the opposite lane and whether the road marking is a dashed line, and if there is no obstacle within 200m of the opposite lane and the road marking is a dashed line, the vehicle performs a lane-changing overtaking; If there is an obstacle within 200m of the opposite lane and the road marking is a solid line, the vehicle slows down and stops.

[0066] In step S5, a trajectory available for control is obtained based on the smoothed path and the planned speed.

[0067] After the path is smoothed in QP, the calculation of speed is performed in ST graph, wherein the speed includes: vehicle expected speed, curve speed, vehicle speed limit, and terminal speed.

[0068] Further, an ordered point sequence in SL or XY coordinate system is Pi = (si, li) or (xi, yi), wherein si is the arc length (cumulative distance) along the reference line. Usually this path is already a dense enough point sequence (e.g. one point per 0.1-0.5 meter).

[0069] Planned speed: a speed profile v(s) (speed as a function of path arc length s) or v(t) (speed as a function of time t) defined on the ST graph. More commonly used and matched with the path is v(s).

[0070] a. Associate speed with path point: because path point P_i has corresponding arc length s_i, the planned speed v_i = v(s_i) of this point can be directly looked up or interpolated.

[0071] b. Calculate time stamp: from the starting point s=0 (corresponding to P_0, t_0 = 0), the time stamp t_i of each path point P_i is calculated by backward integration of speed. The commonly used integration method is the trapezoidal rule: t_i = t_{i-1} + Δs_i / ( (v_i + v_{i-1}) / 2 ) wherein Δs_i = s_i - s_{i-1} is the arc length increment of adjacent path points. If the path points are equi-arc-length spaced (Δs constant) and the speed changes smoothly, t_i = t_{i-1} + Δs / v_{i-1} (backward Euler) or t_i = t_{i-1} + Δs / v_i (forward Euler) can also be used, but the trapezoidal rule has better accuracy and stability.

[0072] c. Calculate acceleration: For the need of subsequent controller (e.g. LQR), it is usually required to calculate the acceleration a_i of each trajectory point. This can be obtained by numerical differentiation of the velocity sequence v_i: a_i = (v_i - v_{i-1}) / (t_i - t_{i-1}) (backward difference) or more smooth, using central difference (requires v_{i+1} and v_{i-1}): a_i = (v_{i+1} - v_{i-1}) / (t_{i+1} - t_{i-1}) (for points inside the path points) Note: the acceleration of the start and end points need special treatment (e.g. using forward / backward difference or extrapolation).

[0073] d. Calculate heading angle: Calculate the heading angle θ_i (tangent direction) of the path point P_i according to its position (x_i, y_i): θ_i = atan2(y_i - y_{i-1}, x_i - x_{i-1}) (use the previous point to calculate) For smoother result, one can use multiple points before and after to smooth or fit a local curve to calculate the tangent direction.

[0074] e. Calculate curvature: (optional but recommended, especially for LQR lateral control) Calculate the curvature κ_i based on the path points: κ_i = (θ_{i+1} - θ_i) / Δs (requires path points to be equi-arc-length or interpolated) or fit a local circular arc with three adjacent points (P_{i-1}, P_i, P_{i+1}).

[0075] f. Assemble trajectory points: Add the calculated time stamp t_i, velocity v_i, acceleration a_i, heading angle θ_i, curvature κ_i (optional) to each path point P_i to form a complete trajectory point T_i = (x_i, y_i, θ_i, v_i, a_i, t_i, [κ_i]). This sequence of T_i is the final trajectory.

[0076] With the above method, the existing path and velocity planning results are fully utilized, and the calculation efficiency is high.

[0077] The initial velocity planning is obtained by dynamic programming, and the velocity obtained by planning is not smooth enough. The QP is used to smooth it, and the final trajectory that can be used for control is obtained.

[0078] Please refer to Figure 5 , in step S6, based on the trajectory available for control, the LQR algorithm based on the kinematic model is used to generate the steering wheel angle, and the double PID controller using position PID and velocity PID in series is used to generate the acceleration compensation instruction.

[0079] In an optional embodiment, a double PID controller in series with position PID and speed PID is used to generate acceleration compensation instructions, including: Obtain the current actual speed; Use the position PID to obtain the position error, and obtain the speed compensation according to the position error; Use the speed PID to obtain the speed error; Obtain the deviation between the current actual speed and the expected speed; Based on the deviation between the current actual speed and the expected speed and the speed compensation, obtain the acceleration compensation instruction.

[0080] PID (Proportional-Integral-Derivative) control is a commonly used core control algorithm in the field of autonomous driving, mainly used for vehicle path tracking and driving stability control. Its core function is to dynamically adjust vehicle state parameters (such as steering angle, throttle / brake force, etc.) to make the actual driving trajectory of the vehicle track the expected path. Specific application scenarios include:‌ Path tracking control: According to the deviation between the current position of the vehicle and the target path, the steering angle or throttle / brake instruction is calculated through the PID algorithm to correct the driving direction in real time.

[0081] Navigation stability control: maintain stable driving of the vehicle under complex road conditions, such as avoiding rollover or deviating from the lane. Dynamic response optimization: through the combination of proportional, integral, and differential links, balance the response speed and system stability, and adapt to different driving scenarios (such as sudden acceleration / braking, curves, etc.). The algorithm can Real-time error adjustment: continuously calculate the deviation between the actual position and the expected path, and quickly adjust the control instruction. Multi-link coordination: the proportional link quickly responds to the deviation, the integral link eliminates steady-state error, and the differential link predicts future trends to suppress overshoot.

[0082] In step S7, the steering wheel angle and acceleration compensation instruction are sent to the vehicle actuator through the CAN bus, and the vehicle actuator controls the vehicle driving through the steering wheel angle and acceleration compensation instruction.

[0083] In still another aspect, a readable storage medium is provided, and the readable storage medium has any one of the above autonomous driving methods stored thereon.

[0084] See Figure 6 In still another aspect, an autonomous driving system is provided, including the above readable storage medium and a positioning module for obtaining point cloud data through a laser radar and outputting obstacle boundary information; The perception module is configured to obtain vehicle position information based on the Beidou positioning module. a decision module, configured to input the obstacle boundary information and the vehicle position information into the decision module, determine a driving behavior by a preset state machine, the driving behavior including driving in a lane, overtaking by borrowing a lane, and slowing down to give way; a planning module, configured to plan a trajectory according to the driving behavior, including dynamically scattering points on a SL graph according to a curvature change rate, generating an initial path by dynamic programming, and then smoothing the initial path by quadratic programming to obtain a smooth path; planning a speed based on a speed state of the vehicle, including an expected speed, a speed on a curve, a vehicle speed limit, and a terminal speed, on an ST graph to obtain a planned speed; obtaining a controllable trajectory based on the smooth path and the planned speed; generating a steering wheel turning angle based on the controllable trajectory by using an LQR algorithm based on a kinematic model, and generating an acceleration compensation instruction by using a double-PID controller in series of a position PID and a speed PID; a control module, configured to control the vehicle to drive by using the steering wheel turning angle and the acceleration compensation instruction.

[0085] The above has described various embodiments of the present disclosure, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and changes are obvious to those skilled in the art without departing from the scope and spirit of the described embodiments. The choice of terms used herein is intended to best explain the principles, practical application, or technical improvement in the market of the embodiments, or to enable other ordinary skilled persons in the art to understand the embodiments disclosed herein.

Claims

1. An automatic driving method, characterized by, The method comprises the following steps: S1. Obtain point cloud data by laser radar, and output obstacle boundary information; S2. Obtain vehicle position information based on a Beidou positioning module; S3. Input the obstacle boundary information and the vehicle position information into a decision module, and determine a driving behavior by a preset state machine, wherein the driving behavior comprises driving in a lane, overtaking by borrowing a lane, and slowing down to give way; S4. Plan a trajectory according to the driving behavior, including dynamically scattering points on an SL graph according to a curvature change rate, generating an initial path by dynamic programming, and then smoothing the initial path by quadratic programming to obtain a smooth path; and planning a speed based on a speed state of the vehicle on an ST graph, including an expected speed, a curve speed, a vehicle speed limit, and a terminal speed, to obtain a planned speed; S5. Obtain a trajectory available for control based on the smooth path and the planned speed; S6. Generate a steering wheel turning angle by an LQR algorithm based on a kinematic model based on the trajectory available for control, and generate an acceleration compensation instruction by a double-PID controller in series with a position PID and a speed PID; S7. Send the steering wheel turning angle and the acceleration compensation instruction to a vehicle control module through a CAN bus, and control the vehicle to drive by the steering wheel turning angle and the acceleration compensation instruction through a vehicle actuator.

2. The automatic driving method of claim 1, wherein, The trajectory planning according to the driving behavior comprises the following steps: If the driving behavior is driving in a lane, it is determined whether there is an obstacle that does not affect normal driving in the current lane; If yes, avoid the obstacle in the lane, and if no, drive in the current lane; If the obstacle cannot be avoided in the current lane, it is determined whether the vehicle can overtake by borrowing a lane; If the vehicle cannot overtake by borrowing a lane, the vehicle slows down in the current lane, and if the vehicle can overtake by borrowing a lane, the vehicle enters the state of overtaking by borrowing a lane.

3. The method of claim 2, wherein, The trajectory planning according to the driving behavior further comprises the following steps: If the driving behavior is overtaking by borrowing a lane, it is determined whether there is an obstacle that affects normal driving in the right lane; If no, drive to the right lane and jump into the state of driving in a lane; If yes, continue overtaking.

4. The method of claim 3, wherein, If an obstacle appears in the left lane during the overtaking process, it is determined whether the vehicle can accelerate to eliminate the influence; If no, it is determined whether the vehicle can slow down in the right lane, and if no, the vehicle stops.

5. The automatic driving method of claim 1, wherein, S1. Obtain point cloud data by laser radar, and output obstacle boundary information, including: obtaining point cloud data by laser radar, sequentially performing voxel filtering processing, ground point cloud elimination processing, and Euclidean clustering processing to obtain the obstacle boundary information.

6. The method of claim 5, wherein, The grid size of the voxel filtering processing is 0.2±0.05 m, and the distance threshold of the Euclidean clustering is 0.5±0.1 m; the ground point cloud elimination adopts a RANSAC plane fitting algorithm.

7. The method of claim 2, wherein, If the driving behavior is driving in a lane and there is an obstacle that does not affect normal driving, it is determined whether the distance between the obstacle and the vehicle is ≤5 m and whether the obstacle occupies a lane width ≥30%; if yes, drive in the lane to avoid the obstacle; and if no, drive in the current lane. If the obstacle cannot be avoided in the current lane, it is determined whether there is no obstacle in the opposite lane within 200m and the road marking is a dashed line. If there is no obstacle in the opposite lane within 200m and the road marking is a dashed line, the vehicle is driven in the opposite lane to overtake; If there is an obstacle in the opposite lane within 200m and the road marking is a solid line, the vehicle is slowed down and stopped.

8. The automatic driving method of claim 1, wherein, The double-PID controller in series of position PID and speed PID is used to generate acceleration compensation instructions, including: the current actual speed is obtained; the position error is obtained by using the position PID, and the speed compensation is obtained according to the position error; the speed error is obtained by using the speed PID; the deviation between the current actual speed and the expected speed is obtained; the acceleration compensation instruction is obtained based on the deviation between the current actual speed and the expected speed and the speed compensation.

9. A readable storage medium, characterized by, The readable storage medium has the automatic driving method in any one of claims 1-8.

10. An autonomous driving system, characterized by, The readable storage medium in claim 9 is included, and a positioning module is used to obtain point cloud data by laser radar and output obstacle boundary information; a perception module is used to obtain vehicle position information based on the Beidou positioning module; a decision module is used to input the obstacle boundary information and the vehicle position information into the decision module, determine the driving behavior by a preset state machine, and the driving behavior includes driving in the lane, overtaking in the opposite lane and slowing down to give way; a planning module is used to plan a trajectory according to the driving behavior, including dynamically scattering points on the SL graph according to the curvature change rate, generating an initial path by dynamic programming, and then smoothing the path by quadratic programming to obtain a smooth path; on the ST graph, speed planning is performed based on the speed state of the vehicle, including expected speed, curve speed, vehicle speed limit and terminal speed, to obtain the planned speed; a trajectory available for control is obtained based on the smooth path and the planned speed; a steering wheel angle is generated based on the trajectory available for control by using the LQR algorithm based on the kinematic model, and an acceleration compensation instruction is generated by using the double-PID controller in series of position PID and speed PID; a control module is used to control the vehicle driving by the steering wheel angle and the acceleration compensation instruction.

Images