CN112148002B - A local trajectory planning method, system and device - Google Patents
A local trajectory planning method, system and device Download PDFInfo
- Publication number
- CN112148002B CN112148002B CN202010900948.8A CN202010900948A CN112148002B CN 112148002 B CN112148002 B CN 112148002B CN 202010900948 A CN202010900948 A CN 202010900948A CN 112148002 B CN112148002 B CN 112148002B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- path
- speed
- acceleration
- safe
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000012544 monitoring process Methods 0.000 claims abstract description 41
- 230000003993 interaction Effects 0.000 claims abstract description 18
- 230000008447 perception Effects 0.000 claims abstract description 8
- 230000001133 acceleration Effects 0.000 claims description 68
- 230000008569 process Effects 0.000 claims description 21
- 238000004364 calculation method Methods 0.000 claims description 11
- 230000006399 behavior Effects 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 5
- 230000006870 function Effects 0.000 claims description 4
- 238000012886 linear function Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 2
- 239000002699 waste material Substances 0.000 abstract description 3
- 230000004888 barrier function Effects 0.000 description 12
- 238000004422 calculation algorithm Methods 0.000 description 7
- 230000008859 change Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 3
- 230000014759 maintenance of location Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000001172 regenerating effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/0285—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using signals transmitted via a public communication network, e.g. GSM network
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种局部轨迹规划方法、系统及装置，方法为：获取感知信息和当前位姿，基于所述感知信息和当前位姿生成一条最优路径；基于所述感知信息对所述最优路径附加速度属性，根据所述最优路径与障碍物的交互情况，采取超越模式或等待模式，获取超越模式和等待模式下安全的行驶速度，如果不能获取沿最优路径行驶的安全速度，则重新生成最优路径；将所述最优路径与所述安全速度融合，生成车辆行驶轨迹，同时车辆进入监控状态行驶；并进入状态保持模式，不仅可以提升车辆在行驶过程中的舒适性，而且可以确保其快速响应交通场景的变化；同时，该框架能够避免频繁的重规划带来了计算资源的大量浪费，降低计算机运行自动驾驶系统时的计算负荷。
The invention discloses a local trajectory planning method, system and device. The method comprises the following steps: acquiring perception information and a current posture, generating an optimal path based on the perception information and the current posture; The additional speed attribute of the optimal path, according to the interaction between the optimal path and the obstacle, adopt the overtaking mode or the waiting mode, and obtain the safe driving speed in the overtaking mode and the waiting mode. If the safe driving speed along the optimal path cannot be obtained, Then the optimal path is regenerated; the optimal path is fused with the safe speed to generate the vehicle driving trajectory, and the vehicle enters the monitoring state driving; and enters the state maintaining mode, which can not only improve the comfort of the vehicle during driving, but also And it can ensure that it responds quickly to changes in traffic scenarios; at the same time, the framework can avoid a lot of waste of computing resources caused by frequent re-planning, and reduce the computing load when the computer runs the automatic driving system.
Description
Technical Field
The invention belongs to the technical field of unmanned vehicles, and particularly relates to a local trajectory planning method, system and device.
Background
Local trajectory planning is an important loop in autonomous driving systems. Under the guidance of global navigation, a track which can be driven by a vehicle is generated. The trajectory must satisfy the following condition: the vehicle dynamic constraint is met, the collision is avoided, and the vehicle dynamic constraint is efficient and comfortable. Local trajectory planning provides agility and safety of an autonomous driving system in dynamic traffic scenarios
Since the unmanned system is faced with a dynamic traffic scene, it needs to be able to react swiftly to changes in the environment. For the problem, the current planning framework only performs trajectory re-planning at a fixed time frequency, and then continuously outputs the re-planned trajectory to the control system. This approach is too simple and does not take into account the possible problems of variability between plans. And frequent re-planning brings about a great waste of computing resources and excessive dependence on the time overhead of the planning algorithm. In conclusion, such a frame is certainly unsuitable, which can lead to problems in terms of travel path, speed and comfort of the vehicle.
Disclosure of Invention
In order to solve the problems in the prior art, the invention provides a local trajectory planning method, a system and a device, and the proposed planning framework can not only improve the comfort of the vehicle in the driving process, but also ensure the vehicle to quickly respond to the change of a traffic scene, thereby realizing environment self-adaptive re-planning.
In order to achieve the above object, the present invention adopts a technical solution that a local trajectory planning method includes the following steps:
s100, obtaining the perception information of the vehicle to the external environment and the current pose,
s200, generating an optimal path based on the perception information and the current pose;
s300, adding a speed attribute to the optimal path of S200 based on the sensing information and the current pose of S100, adopting an override mode or a waiting mode according to the interaction condition of the optimal path and the barrier, acquiring safe running speeds in the override mode and the waiting mode based on a uniform acceleration motion model, and regenerating the optimal path according to S200 if the safe running speed along the optimal path cannot be acquired; when the speed is generated, the error between the actual speed of the vehicle and the expected speed of the track is used as a compensation item and added to the calculation process;
s400, fusing the optimal path of S200 and the safe speed of S300 to generate a vehicle running track, and enabling the vehicle to run in a monitoring state; and enters a state retention mode.
S200, generating a candidate path group by adopting an end point state sampling method, and selecting an optimal path from the candidate paths, wherein the method specifically comprises the following steps:
s201, the longitudinal deviation S of the planned distance on a reference line and the current speed of the vehicle form a linear function, a reference point R can be obtained, and different transverse deviation d is formed at the position of the reference point RiDifferent target states G can be obtained by samplingiSolving for GiPose in cartesian coordinates;
s202, obtain the initial pose I ═ x from the vehicle(init),y(init),θ(init),κ(init)) To GiParameter curve Path ofi(ii) a Generating diversified paths to obtain a path group to be selected;
s203, considering static and low-speed obstacles, judging whether each path collides with the obstacle or not to obtain the collision risk of each path, comprehensively considering the maximum curvature of the path and the transverse offset from the reference line, calculating the collision risk, the maximum curvature and the transverse offset loss from the reference line, and then performing weighted summation, wherein the path with the minimum loss is the optimal path.
In S300, a feasible interval of acceleration in the interaction process of each obstacle is obtained according to the information of the dynamic obstacles on the optimal path, the feasible interval of the final acceleration is the intersection of the feasible intervals of the acceleration generated in the interaction process of each dynamic obstacle, the feasible interval of the acceleration is obtained, the optimal acceleration is found from the feasible intervals, and the speed curve of the vehicle is calculated according to the uniform acceleration motion model by adopting the optimal acceleration.
S300 specifically comprises the following steps:
s301, obtaining a feasible interval of acceleration in the interaction process of each obstacle according to the dynamic obstacle information, obtaining collision points of the vehicle and the dynamic obstacles, and enabling the vehicle to enter an overrunning mode or a waiting mode at the collision points;
acquiring a feasible acceleration interval under an overtaking mode and a waiting mode when the vehicle encounters each dynamic obstacle based on the information of each dynamic obstacle and the information of the collision point;
and calculating the loss of any acceleration in the acceleration interval according to the expected acceleration corresponding to the current acceleration of the vehicle, the current acceleration and the boundary of the acceleration interval, wherein the acceleration with the minimum loss is the optimal acceleration, and obtaining the speed curve of the vehicle by utilizing a uniform acceleration motion model according to the optimal acceleration.
When the vehicle is in a monitoring state driving and state maintaining mode, judging whether the current speed curve meets the conditions of an overtaking mode or a waiting mode in real time, and if not, giving an alarm; and calculating the distance between the current position of the vehicle and the optimal track target point in real time, and when the distance is smaller than a set threshold value, replanning a new optimal path.
A local track planning system comprises a control module, a path planning module, a speed generation module, a safety monitoring module and a driving monitoring module, wherein the path planning module generates a path according to information provided by a vehicle-mounted sensing system; the speed generation module adds a speed attribute to the generated path; the speed generation module acquires the generated path and barrier information and inputs the path and barrier information into the speed generation module at the same time, the safe driving speed is determined according to the interaction condition of the optimal path and the barrier, if the safe speed for driving along the path cannot be obtained, a path planning instruction is generated, and if the safe speed is obtained, the path and the speed are combined to complete the generation of the track; the safety monitoring module is used for judging whether the vehicle is in a dangerous state or not; the driving monitoring module is used for judging whether the target point of the planned path is reached; the safety monitoring module and the driving monitoring module also store and output the track of the vehicle, namely output the optimal path; and the control module generates corresponding behaviors according to the optimal path so that the vehicle starts to run along the optimal path.
The speed generation module comprises a speed generation unit and a low-pass filtering unit, the speed generation unit can continuously update the speed attribute of the track at the frequency of 0.5 Hz, and the low-pass filtering unit is used for acquiring the error between the actual speed of the vehicle and the expected speed of the track and feeding the error back to the speed generation unit.
The safety monitoring module continuously updates the current state of the vehicle and the barrier information given by the sensing module at the frequency of 10 Hz, judges the safety of the vehicle according to the current track, and generates and sends a corresponding signal if an emergency situation occurs when the vehicle continuously runs along the current track;
the driving monitoring module receives the current pose of the vehicle at the same frequency of 10 Hz, judges whether the vehicle is to reach a target point of the track according to the stored current track, and generates and sends a corresponding signal if the vehicle reaches the target point.
A computer device comprises one or more processors and a memory, wherein the memory is used for storing computer executable programs, the processors read part or all of the computer executable programs from the memory and execute the computer executable programs, and when the processors execute part or all of the computer executable programs, the local trajectory planning method can be realized.
A computer-readable storage medium, in which a computer program is stored, which, when being executed by a processor, is capable of implementing the local trajectory planning method according to the invention.
Compared with the prior art, the invention has at least the following beneficial effects:
the method of the invention obtains the generated path and barrier information and determines the safe driving speed according to the interaction condition of the planned path and barrier, if the safe speed for driving along the path can not be obtained, a continuous and smooth speed curve is formed, if the safe speed is obtained, the path and the speed are combined with the planning method provided by the invention, thus not only improving the comfort of the vehicle in the driving process, but also ensuring the vehicle to quickly respond to the change of a traffic scene; meanwhile, a large amount of waste of computing resources caused by frequent re-planning can be avoided, and the computing load of a computer when the automatic driving system runs is reduced;
the system of the invention completely decouples the path planning and the speed generation and operates as two independent modules, and the delay of the path planning does not cause the delay of the whole system; even the path planning algorithm with larger time overhead does not influence the real-time performance of the whole system; on the other hand, the current planning system is particularly serious in the consumption of computing resources in a mode of updating the track at a fixed frequency, but the planning system does not need to update the path at the fixed frequency, so that the expenditure of the computing resources is reduced to a great extent, and the computing load of a computer when the automatic driving system runs is reduced.
Drawings
Fig. 1 is a diagram of the proposed trajectory planning framework.
Fig. 2 is a schematic diagram of a path planning algorithm.
Fig. 3 is a schematic diagram of an override mode in barrier interaction.
Fig. 4 is a diagram illustrating a waiting mode in obstacle interaction.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings.
The speed generation module acquires the generated path and barrier information and inputs the path and barrier information into 'speed generation', determines a safe driving speed according to the interaction condition of the planned path and the barrier, generates a 'path planning' instruction if the safe speed for driving along the path cannot be obtained, and combines the path and the speed if the safe speed is obtained, so that the generation and planning process of the finished track can be divided into three layers: the first layer is path planning, the second layer is speed generation, and the third layer is planning scheduling; the path planning layer generates the most basic driving path, and the speed generation layer adds speed to the generated path to ensure the dynamic performance of the vehicle; and the planning and scheduling layer adaptively schedules the modules of the first two layers according to the environment and the vehicle running state.
The operation flow of the framework is as shown in fig. 1, after the automatic driving system receives an instruction for starting planning, the system generates a path in a path planning module by using an endpoint state sampling path planning algorithm based on five-time spline interpolation on the basis of the obstacle information provided by the sensing module, and the path satisfies G2And (3) continuous. The next step is to append a speed attribute to the generated path. And simultaneously inputting the generated path and the obstacle information into a speed planning module, and determining the safe driving speed according to the interaction condition of the planned path and the obstacle. And if the safe speed of driving along the path cannot be found, generating a re-path planning instruction, and triggering the path planning module to re-plan the path. Otherwise, combining the path with the speed to complete the generation of the track, and inputting the track into a vehicle state monitoring layer; the vehicle state monitoring layer comprises two parts, namely a safety monitoring module and a driving monitoring module, wherein the safety monitoring module is used for judging whether the vehicle is in a dangerous state, and the driving monitoring module is used for judging whether the vehicle reaches a target point of a planned path; the safety monitoring module and the driving monitoring module store the track generated by the speed generating module and then input the track into the control module. The control module generates corresponding behaviors according to the track, so that the vehicle starts to run along the track; the first cycle of programming is now complete and the next cycle of programming is not the second cycle but is to enter a state retention mode.
In the state maintaining mode, a path planning module does not plan a new path unless receiving a path re-planning instruction, the speed generating module comprises a speed generating unit and a low-pass filtering unit, and the speed generating unit continuously updates the speed attribute of the track at the frequency of 0.5 Hz; during the updating process, an error may exist between the actual speed of the vehicle and the expected speed of the track, and the error is fed back to the speed generating unit after passing through a low-pass filtering unit, and becomes one of the factors for updating the speed attribute. If the "speed generation" module cannot find a suitable speed attribute for the next path, the "speed generation" module is triggered to perform re-path planning. In addition, the safety monitoring module can continuously update the current state of the vehicle and the obstacle information given by the sensing module at the frequency of 10 Hz, and judge the safety of the vehicle according to the current track. Once the safety monitoring module considers that an emergency situation occurs when the vehicle continues to travel along the current track, the safety monitoring module sends a signal to the speed generation module to prompt the speed generation module to adjust the speed attribute of the current track so as to avoid danger. And the running monitoring module receives the current pose of the vehicle at the same frequency of 10 Hz and judges whether the vehicle is to reach a target point of the track according to the stored current track. If the vehicle reaches the target point, the "driving monitoring" module will send a signal to the "path planning" module to make it enter the next cycle of planning. The above process is repeated until the driving task of the automatic driving system is completed.
Through the process, the vehicle can always run from the starting point to the end point along the path generated by the planning algorithm, and the comfort in the running process is guaranteed. And in the process, the track speed attribute is continuously changed, so that the agility of the vehicle in a dynamic environment is realized. The reason for updating the speed attribute of the track at a low frequency of 0.5 hz is to reduce the consumption of computing resources on one hand and to avoid the problem that the control module is difficult to track the speed due to too frequent track updates on the other hand. Meanwhile, the safety monitoring module is updated at a high frequency of 10 Hz, so that the problem of untimely response to an emergency condition caused by too low updating frequency of the track speed is effectively solved. The method ensures that the automatic driving system can respond to emergency within 0.1 second time lag and exceeds the response speed of human beings, thereby effectively improving the agility of the system.
Besides the comfort and agility of vehicle running, the system of the invention has other advantages, and the time overhead of planning algorithm is one of the research difficulties. However, the system of the invention solves the problem, and the system operates as two independent modules through complete decoupling of path planning and speed generation, and the delay of the path planning does not cause the delay of the whole system; even the path planning algorithm with larger time overhead does not influence the real-time performance of the whole system; on the other hand, the current planning system is particularly serious in the consumption of computing resources in a mode of updating the track at a fixed frequency, but the planning system does not need to update the path at the fixed frequency, so that the expenditure of the computing resources is reduced to a great extent, and the computing load of a computer when the automatic driving system runs is reduced.
In addition, since the system is less likely to re-plan during travel along a planned route, it is difficult for a vehicle to appear during travel because the route does not satisfy G3The problem of understeer or lateral deviation overshoot occurs due to continuity, and on the other hand, the dynamic response of the vehicle mainly depends on speed generation, so that a planned and generated path is not tightly coupled with a sensing module, errors generated by the sensing module firstly influence the running speed of the vehicle, and the curvature change rate of the vehicle is not directly discontinuous.
When the system generates the speed, the error between the actual speed of the vehicle and the expected speed of the track is used as a compensation item to be added into the calculation process, and through the feedback of the compensation, the system can still better track the track even if the speed attribute of the track is updated at the frequency of 0.5 Hz; in conclusion, the system provided by the invention can better solve the problems existing in the current planning framework.
Each module in the framework is further described below:
a path planning module: firstly, generating a path to be selected by adopting an end point state sampling method, then selecting an optimal path, and calculating a corresponding point C of the optimal path on a reference line according to the current pose I of the vehicle after obtaining the reference line by global navigation as shown in FIG. 2;
then, the longitudinal deviation s of the planned distance on the reference line is a linear function of the current speed of the vehicle, so that a reference point R can be obtained, and different transverse deviation d is formed at the position of the reference point RiDifferent target states G can be obtained by samplingi；
Then, the solution G is obtainediPose in Cartesian coordinates, pose (x) of reference point R in Cartesian coordinates(r),y(r),θ(r),κ(r)) May be provided by reference lines each of which represents the x-coordinate, y-coordinate, orientation and curvature of R, respectively.
Thus, G can be calculated by the following expressioniPose in Cartesian coordinates
Next, the initial pose I ═ x of the vehicle is obtained(init)，y(init)，θ(init)，κ(init)) To GiParameter curve Path ofi. In order to ensure the curvature continuity between adjacent planning periods, quintic splines are adopted to form a parameter curve PathiDescription is given of { x(s), y(s) }, the expression is as follows:
wherein s isfThe arc length of the parametric curve is usually calculated by using the chord length as an estimate of the arc length; at this time, the unknown parameter, Path, in expression 2 is obtainediCan be uniquely determined; while the saidThe known parameters can be calculated by the boundary constraint condition of the parameter curve, and the expression is as follows:
substituting the expression 3 into the expression 2 can calculate the unknown parameters therein to obtain the final generation Path Pathi. Different GiDifferent Path can be obtainediThereby generating diverse paths, i.e., resulting sets of paths.
Next, optimal path selection is carried out; in the process, only static and low-speed obstacles are considered, whether each path collides with the obstacle or not is judged, so that the collision risk of each path is obtained, in addition, the maximum curvature of the path and the transverse offset from a reference line are the other two factors needing to be considered, the losses of the three factors are calculated and subjected to weighted summation, and the path with the minimum loss is the optimal path; so far, the function of the path generation module is completed.
For the speed generation module, assuming that the vehicle makes a uniform speed change movement in the next period of time, the key point of the speed generation is to obtain a proper acceleration, and the method for solving the problem is as follows:
and obtaining a feasible interval of the acceleration according to the dynamic obstacle information, and finding the optimal acceleration from the feasible interval.
When the vehicle runs along the optimal path, a feasible acceleration interval exists in the interaction process of the vehicle and each dynamic obstacle, and the feasible final acceleration interval is the intersection of the feasible acceleration intervals generated in the interaction process of each dynamic obstacle;
as an alternative example, the speed planning time period of the present invention is 2 s.
The calculation method of the feasible interval of each acceleration is as follows: first, the collision point of the vehicle with the dynamic obstacle is calculated, as shown in fig. 3 and 4, both the vehicle and the dynamic obstacle are described by rectangular frames; only judging whether two rectangular frames are sent or notWhether collision occurs can be judged by overlapping, and the judgment method of rectangular frame overlapping adopts the separation axis theorem; obtaining a vehicle collision point cp(ego)Collision point cp with obstacle(obs)Thereafter, the behavior pattern of the host vehicle can be divided into two types. The first is to override, i.e. to cross the collision point before the dynamic obstacle, as shown in fig. 3, and the second is to wait, i.e. to reach the collision point after the dynamic obstacle, as shown in fig. 4. Both behaviour modes ensure the safety of the vehicle.
For the override mode, the vehicle must travel to meet two conditions; the first condition is that the vehicle reaches the collision point cp(ego)While, the dynamic obstacle is away from the collision point cp(obs)There is also a safety distance distsafeThat is, the position is shown by the frame of the first scenario in fig. 3; the second condition is when the dynamic obstacle reaches cp(obs)When the vehicle has exceeded cp(ego)Safe distance distsafeThat is, the position is shown by the second frame in the scene two of fig. 3; safe distance distsafeThe calculation expression is as follows, and the calculation expression is related to the current speed of the vehicle and the speed of the dynamic obstacle:
wherein, dist1And dist2For a set distance constant, vobsAs the speed of the obstacle, acc(obs)Maximum acceleration of the obstacle, vegoAs the vehicle speed, acc(ego)Coef is a set parameter for the maximum acceleration of the vehicle, epsilon is a step function, the expression is divided into two terms, the first term shows that when the vehicle is emergently braked, the obstacle is emergently braked, and after the vehicle is finally stopped, the distance between the two terms is dist1The second term is the distance to the obstacle that is desired to be maintained during travel.
Subsequently, for each acceleration acc of the vehicle, it is determined whether the above-described two conditions are satisfied when running at that acceleration. For condition one, vehicle-to-cp can be solved by a variable acceleration motion formula(ego)Time overhead t and velocity v ofegoAnd calculating dist according to expression 4safeFinally, calculating the movement of the barrier in the time t, and judging whether the movement exceeds the position of the frame I or not, wherein if the movement does not exceed the position of the frame I, the acc meets the condition I; for condition two, the obstacle to cp can be calculated(obs)Then the final position and final speed v of the vehicle moving with acc during the time t are calculatedego(ii) a Dist is calculated according to expression 4safeJudging whether the final position of the vehicle exceeds cp(ego)Distance distsafe(ii) a If so, the acc satisfies condition two, and if both conditions are satisfied, the acc is feasible.
For the waiting mode, there are also two conditions, the first being when the dynamic obstacle reaches cp(obs)While, vehicle is away from cp(ego)There is also a safety distance distsafeThat is, the position is shown by the frame two in the scene one of fig. 4; the second condition is that the vehicle reaches cp(ego)When the dynamic obstacle exceeds the collision point cp(obs)Safe distance distsafeThat is, the position is shown by the frame of # two in the scenario of fig. 4. In this mode, the safe distance distsafeThe calculation expression is as follows:
where the meaning of the variables is the same as in expression 4 and Sgn is a sign function. The meaning of this expression is also consistent with expression 4. The following process is basically the same as the description in the overtaking behavior mode, whether the acceleration meets two conditions is judged, if yes, the acceleration is considered to be feasible, and then a feasible acceleration interval is obtained.
The next step is to select an optimal acceleration that takes into account a number of factors, including the desired acceleration a for the current speed(exp)(preset), current acceleration a(cur)And the boundary of the acceleration interval; the loss calculation of any acceleration a in the interval comprises a and a(exp)A distance from a(cur)And with an acceleration intervalThe reciprocal of the boundary distance, and the acceleration with the minimum loss is the optimal acceleration; after the optimal acceleration is obtained, the velocity curve can be naturally calculated.
The safety monitoring module and the driving monitoring module are relatively simple to realize; the safety monitoring module continuously judges whether the current speed curve can meet the conditions of the aforementioned overtaking mode or waiting mode, and if not, an alarm is given. The driving monitoring module calculates the distance between the current position of the vehicle and the optimal track target point, and sends a replanning signal once the distance is smaller than a threshold value.
Optionally, the present invention further provides a computer device, including but not limited to one or more processors and a memory, where the memory is used to store a computer-executable program, the processor reads a part or all of the computer-executable program from the memory and executes the computer-executable program, the processor can implement a part of or all of the steps of the local trajectory planning method according to the present invention when executing the part or all of the computer-executable program, and the memory can also be used to store vehicle-mounted sensor information, road information, and map information.
A computer-readable storage medium, in which a computer program is stored, which, when being executed by a processor, is capable of implementing the local trajectory planning method according to the invention.
The computer equipment can be an onboard computer, a notebook computer, a tablet computer, a desktop computer, a mobile phone or a workstation.
The invention also provides an output device for outputting the prediction result, wherein the output device is connected with the output end of the processor, and the output device is a display or a printer.
The processor may be a Central Processing Unit (CPU), a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), or an off-the-shelf programmable gate array (FPGA).
The memory of the invention can be an internal storage unit of a vehicle-mounted computer, a notebook computer, a tablet computer, a desktop computer, a mobile phone or a workstation, such as a memory and a hard disk; external memory units such as removable hard disks, flash memory cards may also be used.
Computer-readable storage media may include computer storage media and communication media. Computer storage media includes volatile and nonvolatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. The computer-readable storage medium may include: a Read Only Memory (ROM), a Random Access Memory (RAM), a Solid State Drive (SSD), or an optical disc. The Random Access Memory may include a resistive Random Access Memory (ReRAM) and a Dynamic Random Access Memory (DRAM).
Claims (10)
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010900948.8A CN112148002B (en) | 2020-08-31 | 2020-08-31 | A local trajectory planning method, system and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010900948.8A CN112148002B (en) | 2020-08-31 | 2020-08-31 | A local trajectory planning method, system and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112148002A CN112148002A (en) | 2020-12-29 |
| CN112148002B true CN112148002B (en) | 2021-12-28 |
Family
ID=73890418
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010900948.8A Active CN112148002B (en) | 2020-08-31 | 2020-08-31 | A local trajectory planning method, system and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112148002B (en) |
Families Citing this family (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110568847B (en) * | 2019-08-30 | 2022-09-23 | 驭势科技（北京）有限公司 | Intelligent control system and method for vehicle, vehicle-mounted equipment and storage medium |
| CN112721931A (en) * | 2021-01-18 | 2021-04-30 | 智马达汽车有限公司 | Vehicle meeting method, device, equipment and storage medium |
| CN112904858B (en) * | 2021-01-20 | 2022-04-22 | 西安交通大学 | Path planning method, system and equipment with continuous curvature |
| CN114815794B (en) * | 2021-01-28 | 2024-12-10 | 武汉智行者科技有限公司 | A local path planning method, system and storage medium |
| CN113074734B (en) * | 2021-03-23 | 2023-05-30 | 北京三快在线科技有限公司 | Track planning method and device, storage medium and electronic equipment |
| CN114701513A (en) * | 2021-04-16 | 2022-07-05 | 长城汽车股份有限公司 | Search advancing method and device for vehicle, storage medium and vehicle |
| CN115542888A (en) * | 2021-06-30 | 2022-12-30 | 未来机器人（深圳）有限公司 | Vehicle route generation method, device, computer equipment and storage medium |
| CN113611109B (en) * | 2021-07-26 | 2022-08-16 | 上海德衡数据科技有限公司 | Intelligent traffic control method and system based on fog calculation |
| CN113793520B (en) * | 2021-09-15 | 2023-09-01 | 苏州挚途科技有限公司 | Vehicle track prediction method and device and electronic equipment |
| CN113734202B (en) * | 2021-09-22 | 2023-12-01 | 驭势科技（北京）有限公司 | Multi-vehicle cooperation method, device, system, equipment, medium and product |
| CN113848869B (en) * | 2021-10-20 | 2023-03-07 | 北京三快在线科技有限公司 | Unmanned equipment control method and device, storage medium and electronic equipment |
| CN113867365B (en) * | 2021-10-28 | 2024-05-14 | 广州文远知行科技有限公司 | Method and device for determining variable acceleration of unmanned vehicle and related equipment |
| CN114274968B (en) * | 2021-11-09 | 2024-07-19 | 天津天瞳威势电子科技有限公司 | Vehicle control method, device, computing equipment and storage medium |
| CN115230719B (en) * | 2021-12-01 | 2023-09-26 | 上海仙途智能科技有限公司 | Driving track planning method and device |
| CN114594779B (en) * | 2022-03-30 | 2025-07-04 | 智道网联科技(北京)有限公司 | Edge cleaning trajectory planning method, device, electronic device, and storage medium |
| CN114815853B (en) * | 2022-06-21 | 2024-05-31 | 清华大学 | A path planning method and system considering road obstacle characteristics |
| CN115454056B (en) * | 2022-08-23 | 2025-05-27 | 北京机械设备研究所 | A method and system for unmanned driving path planning |
| CN115588299B (en) * | 2022-11-25 | 2023-02-28 | 陕西风润智能制造研究院有限公司 | Unmanned vehicle trajectory control system and method |
| CN118484000B (en) * | 2024-05-07 | 2025-01-17 | 浙江神州明月智能科技有限公司 | Robot autonomous navigation obstacle avoidance system and method based on multi-sensor fusion |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8442714B2 (en) * | 2007-04-12 | 2013-05-14 | Panasonic Corporation | Autonomous mobile device, and control device and program product for the autonomous mobile device |
| CN106372758A (en) * | 2016-09-13 | 2017-02-01 | 同济大学 | Path following method and apparatus of auxiliary parking system |
| CN106926844A (en) * | 2017-03-27 | 2017-07-07 | 西南交通大学 | A kind of dynamic auto driving lane-change method for planning track based on real time environment information |
| CN106931961A (en) * | 2017-03-20 | 2017-07-07 | 成都通甲优博科技有限责任公司 | A kind of automatic navigation method and device |
| CN107703948A (en) * | 2017-11-14 | 2018-02-16 | 上海理工大学 | Mobile robot local dynamic station paths planning method based on adaptive dynamic window |
| CN108073176A (en) * | 2018-02-10 | 2018-05-25 | 西安交通大学 | A kind of modified D*Lite vehicle dynamic path planing methods |
| CN108369777A (en) * | 2015-12-22 | 2018-08-03 | 爱信艾达株式会社 | Automatic Pilot support system, automatic Pilot support method and computer program |
| CN109765887A (en) * | 2018-12-21 | 2019-05-17 | 费子偕 | A kind of automatic Pilot control method |
| CN110196590A (en) * | 2019-04-23 | 2019-09-03 | 华南理工大学 | A kind of time optimal trajectory planning system and method for robot path tracking |
| CN110928297A (en) * | 2019-10-28 | 2020-03-27 | 中南大学 | Path planning method for intelligent public transport vehicles based on multi-objective dynamic particle swarm optimization |
| CN111390902A (en) * | 2020-01-06 | 2020-07-10 | 广州视源电子科技股份有限公司 | Trajectory planning method, trajectory planning device, equipment and storage medium |
| WO2020153985A1 (en) * | 2019-01-27 | 2020-07-30 | Navioptronix, Llc | Infrastructure, methods, and systems for high accuracy global positioning and self-driving vehicles |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106114507B (en) * | 2016-06-21 | 2018-04-03 | 百度在线网络技术（北京）有限公司 | Local path planning method and device for intelligent vehicle |
| CN108519773B (en) * | 2018-03-07 | 2020-01-14 | 西安交通大学 | Path planning method for unmanned vehicle in structured environment |
| KR102033924B1 (en) * | 2018-06-04 | 2019-10-18 | 이성식 | System for providing driving route of unmanned autonomous vehicle based on big data and method using the same |
| CN109976355B (en) * | 2019-04-26 | 2021-12-10 | 腾讯科技（深圳）有限公司 | Trajectory planning method, system, device and storage medium |
| CN111289008B (en) * | 2020-04-28 | 2021-04-13 | 南京维思科汽车科技有限公司 | A local path planning method for unmanned vehicles |
-
2020
- 2020-08-31 CN CN202010900948.8A patent/CN112148002B/en active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8442714B2 (en) * | 2007-04-12 | 2013-05-14 | Panasonic Corporation | Autonomous mobile device, and control device and program product for the autonomous mobile device |
| CN108369777A (en) * | 2015-12-22 | 2018-08-03 | 爱信艾达株式会社 | Automatic Pilot support system, automatic Pilot support method and computer program |
| CN106372758A (en) * | 2016-09-13 | 2017-02-01 | 同济大学 | Path following method and apparatus of auxiliary parking system |
| CN106931961A (en) * | 2017-03-20 | 2017-07-07 | 成都通甲优博科技有限责任公司 | A kind of automatic navigation method and device |
| CN106926844A (en) * | 2017-03-27 | 2017-07-07 | 西南交通大学 | A kind of dynamic auto driving lane-change method for planning track based on real time environment information |
| CN107703948A (en) * | 2017-11-14 | 2018-02-16 | 上海理工大学 | Mobile robot local dynamic station paths planning method based on adaptive dynamic window |
| CN108073176A (en) * | 2018-02-10 | 2018-05-25 | 西安交通大学 | A kind of modified D*Lite vehicle dynamic path planing methods |
| CN109765887A (en) * | 2018-12-21 | 2019-05-17 | 费子偕 | A kind of automatic Pilot control method |
| WO2020153985A1 (en) * | 2019-01-27 | 2020-07-30 | Navioptronix, Llc | Infrastructure, methods, and systems for high accuracy global positioning and self-driving vehicles |
| CN110196590A (en) * | 2019-04-23 | 2019-09-03 | 华南理工大学 | A kind of time optimal trajectory planning system and method for robot path tracking |
| CN110928297A (en) * | 2019-10-28 | 2020-03-27 | 中南大学 | Path planning method for intelligent public transport vehicles based on multi-objective dynamic particle swarm optimization |
| CN111390902A (en) * | 2020-01-06 | 2020-07-10 | 广州视源电子科技股份有限公司 | Trajectory planning method, trajectory planning device, equipment and storage medium |
Non-Patent Citations (7)
| Title |
|---|
| High-Definition Map Combined Local Motion Planning and Obstacle Avoidance for Autonomous Driving;Zhiqiang Jian等;《2019 IEEE Intelligent Vehicles Symposium (IV)》;20190612;第2180-2186页 * |
| Obstacle-guided informed planning towards robot navigation in cluttered environments;Zehui Meng等;《2017 IEEE International Conference on Robotics and Biomimetics (ROBIO)》;20171208;第332-337页 * |
| Path planning in environment with moving obstacles for mobile robot;Li Qing等;《Proceedings of the 31st Chinese Control Conference》;20120727;第5019-5024页 * |
| 一种基于共点映射的无人车可行驶区域检测方法;刘子熠，等;《Engineering》;20180831(第4期);第109-133页 * |
| 无人驾驶汽车局部路径规划算法研究;彭晓燕，等;《汽车工程》;20200131;第42卷(第01期);第1-10页 * |
| 煤矿救援机器人地图构建与路径规划研究;程新景;《中国优秀硕士学位论文全文数据库 工程科技I辑》;20170215(第02(2017)期);第B021-350页 * |
| 自主车路径跟踪控制中的参考输入优化与扰动补偿技术研究;朱琪;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20160315(第03(2016)期);第C035-157页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112148002A (en) | 2020-12-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112148002B (en) | A local trajectory planning method, system and device | |
| JP7345676B2 (en) | Adaptive control of autonomous or semi-autonomous vehicles | |
| US11661068B2 (en) | Vehicle system for recognizing objects | |
| CN108692734B (en) | A path planning method and device | |
| CN109941275B (en) | Lane changing method, lane changing device, electronic equipment and storage medium | |
| CN110834644B (en) | Vehicle control method and device, vehicle to be controlled and storage medium | |
| JP6494872B2 (en) | Method for controlling vehicle motion and vehicle control system | |
| JP2023175055A (en) | Autonomous vehicle planning | |
| US10571916B2 (en) | Control method for autonomous vehicles | |
| JP7520444B2 (en) | Vehicle-based data processing method, data processing device, computer device, and computer program | |
| CN109900289B (en) | Path planning method and device based on closed-loop control | |
| JP2022513808A (en) | Collision avoidance system using orbit verification | |
| JP2022516614A (en) | Collision avoidance system | |
| CN110834631A (en) | Pedestrian avoiding method and device, vehicle and storage medium | |
| WO2020003556A1 (en) | Control system of vehicle, method for controlling vehicle, and non-transitory computer readable memory | |
| JP7282271B2 (en) | Direct and indirect control of mixed autonomous vehicle platoons | |
| Jian et al. | Multi-model-based local path planning methodology for autonomous driving: An integrated framework | |
| CN112965476A (en) | High-speed unmanned vehicle trajectory planning system and method based on multi-window sampling | |
| US20200363816A1 (en) | System and method for controlling autonomous vehicles | |
| CN114407929A (en) | Method, device, electronic device and storage medium for handling obstacles around unmanned vehicles | |
| JP7019885B2 (en) | Vehicle control value determination device, vehicle control value determination method, and program | |
| CN113759892B (en) | Unmanned vehicle obstacle bypassing method, device, unmanned vehicle and storage medium | |
| WO2024249076A1 (en) | Interaction prediction based on tracking trajectory | |
| CN117198082B (en) | Vehicle ramp merging decision method and system based on double-layer optimization | |
| US20240116511A1 (en) | Multi-policy lane change assistance for vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |