CN117032201B - Mine automatic driving vehicle coordination planning method based on vehicle-road coordination - Google Patents

Mine automatic driving vehicle coordination planning method based on vehicle-road coordination Download PDF

Info

Publication number
CN117032201B
CN117032201B CN202310215591.3A CN202310215591A CN117032201B CN 117032201 B CN117032201 B CN 117032201B CN 202310215591 A CN202310215591 A CN 202310215591A CN 117032201 B CN117032201 B CN 117032201B
Authority
CN
China
Prior art keywords
vehicle
planning
automatic driving
coordination
planner
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
Application number
CN202310215591.3A
Other languages
Chinese (zh)
Other versions
CN117032201A (en
Inventor
吴宏涛
张林梁
牛秉青
李朝霞
孟颖
李臻
吕永萍
樊恩成
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanxi Intelligent Transportation Research Institute Co ltd
Original Assignee
Shanxi Intelligent Transportation Research Institute Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shanxi Intelligent Transportation Research Institute Co ltd filed Critical Shanxi Intelligent Transportation Research Institute Co ltd
Priority to CN202310215591.3A priority Critical patent/CN117032201B/en
Publication of CN117032201A publication Critical patent/CN117032201A/en
Application granted granted Critical
Publication of CN117032201B publication Critical patent/CN117032201B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a vehicle-road-cooperation-based mine automatic driving vehicle coordination planning method, which comprises the steps of acquiring a mine high-precision map provided by a cloud platform by an automatic driving vehicle road, planning a global smooth navigation path based on a road center line in the high-precision map, and realizing complete reference line smoothing; the automatic driving vehicle bottom layer planner carries out path and speed decision planning, and track information output by the bottom layer planner is used as an output to be sent to the control module; according to the planning track of the bottom layer of the vehicle, the vehicle automatically drives normally, and advanced planning is carried out according to specific conditions; planning respectively by adopting path and speed decoupling, and iteratively solving a feasible self-vehicle track; the vehicle track of each driving vehicle is used as input to the control module, and the transverse and longitudinal control of the driving vehicle is executed to pass through the mine crossing so as to complete the vehicle meeting action. The invention realizes the efficient planning of conflict-free trajectories of a plurality of automatic driving vehicles and improves the running efficiency of the automatic driving vehicles in the interactive scene.

Description

Mine automatic driving vehicle coordination planning method based on vehicle-road coordination
Technical Field
The invention belongs to the technical field of automatic driving, and particularly relates to a vehicle coordination planning method for underground automatic driving based on vehicle-road coordination.
Background
Along with the rapid development of the automatic driving technology, the special plan of 'mechanical person changing and automatic person reducing' of the coal mine is effectively pushed to be implemented based on the safety and the reliability of the automatic driving vehicle. The automatic driving technology is realized by means of the Internet of vehicles (vehicle to everything, V2X), and when the infrastructure of the Internet of vehicles is more and more perfect, the automatic driving technology will also develop towards a more mature trend. V2X provides vehicle information through sensors, vehicle-mounted terminals and electronic tags which are arranged on the vehicle, various communication technologies are adopted to realize interconnection and intercommunication of vehicles (vehicle to vehicle, V2V), vehicles and people (vehicle to pedestrian, V2P) and vehicles and roads (infrastructure) (vehicle to infrastructure, V2I), and the vehicle information is extracted, shared and the like on an information network platform so as to realize effective control and comprehensive service of the vehicle. Different from the automatic driving in the field of ground traffic, the tunnel road under the mine is narrow and the road width is not uniform, and the widest part can reach 7 meters under the general condition, and the narrow part is only about 4 meters. The conventional underground vehicle meeting is realized based on concave avoidance areas arranged by drivers and underground tunnels, the vehicle meeting action is completed under the conditions of low roadway illuminance and poor road conditions, and the efficient traffic passing strategy is not available when the vehicles meet at the tunnel intersection area. The situation of meeting vehicles in the scene among underground automatic driving vehicles is complex, and the probability of decision-making errors or planning failures is high.
Therefore, how to provide a vehicle-road-cooperation-based method for coordinated planning of an automatic driving vehicle under a mine becomes a problem to be solved by those skilled in the art.
Disclosure of Invention
Aiming at the problems of underground automatic driving meeting scenes, the invention provides a method for coordinated planning of underground automatic driving vehicles based on vehicle-road cooperation, and the technical problem to be solved by the invention is how to avoid automatic driving vehicle decision errors or planning failures under the conditions of underground narrow road meeting and intersection meeting, realize efficient planning of conflict-free trajectories of a plurality of automatic driving vehicles and improve the running efficiency under the interaction scenes of the automatic driving vehicles.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
a mine automatic driving vehicle coordination planning method based on vehicle-road coordination comprises the following steps:
the automatic driving road obtains a mine underground high-precision map provided by the cloud platform, and planning of a global smooth navigation path is carried out based on a road center line in the high-precision map, so that complete reference line smoothing is realized;
according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle carries out path and speed decision planning, and track information output by the bottom layer planner is used as an output to be sent to the control module;
According to the planning track of the bottom layer of the self-vehicle, the self-driving vehicle normally runs, when meeting a narrow tunnel meeting and an intersection meeting scene, a coordination node is triggered, and advanced planning is carried out according to specific conditions;
the planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, adopts path and speed decoupling to plan respectively, and solves the feasible self-vehicle track iteratively;
the vehicle track of each driving vehicle is used as input to the control module, and the control module executes the transverse and longitudinal control of the driving vehicle to pass through the mine crossing so as to complete the vehicle meeting action.
Further, the automatic driving vehicle road obtains a mine underground high-precision map provided by the cloud platform, and planning of a global smooth navigation path is performed based on a road center line in the high-precision map to realize complete reference line smoothing, and the method comprises the following steps:
the method comprises the steps that an automatic driving road obtains a mine under-mine high-precision map provided by a cloud platform, and planning of a global smooth navigation path is firstly carried out based on a road center line in the high-precision map; wherein the road center line is a discrete point set, which is used as a reference line after smoothing, three times of polynomials are adopted to connect the discrete point sets of the road center line and uniformly sample the encrypted center line discrete points, and the polynomials connect adjacent discrete points (x i ,y i ) And (x) i+1 ,y i+1 )：
y＝f(x)＝a 0 +a 1 x+a 2 x 2 +a 3 x 3
Wherein a0, a1, a2, a3 respectively represent the 0 th order coefficient, the 1 st order coefficient, the 2 nd order coefficient, the 3 rd order coefficient of the cubic polynomial;
carrying out smooth reference line planning, searching for a self-vehicle projection point in a discrete point set in a planning period, carrying out segmentation processing based on the projection point, and taking a segmented path as a path segment to be smoothed; converting the reference line smoothing problem into a quadratic programming problem based on a segmented path segment consisting of densified discrete points, and solving according to a cost function and constraint conditions of smoothing of a segmented center line point set to obtain a smoothed reference line point set; and finally, splicing the reference line segments in different periods, thereby realizing complete reference line smoothing.
Further, the cost function of centerline point set smoothing is:
wherein w is 1 、w 2 、w 3 Weights, x, for each term in the cost function i 、y i And x ref 、y ref The abscissa and ordinate of the reference line and the densified center line, respectively.
Further, the under-mine high-precision map stores road data and fixed opposite direction information of an under-mine tunnel as structured data, in the process of carrying out reference line smoothing on a road center line, projection points of a self-vehicle on the road center line in each automatic driving vehicle planning period are taken as starting points, point sets in a certain range before and after the starting points are smoothed, and the smoothed point sets are taken as reference lines.
Further, according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle performs path and speed decision planning, and sends the track information output by the bottom layer planner as an output to the control module, comprising the following steps:
according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle carries out path and speed decision planning based on a Frenet coordinate system taking the navigation path as a coordinate axis, and track information output by the bottom layer planner is used as an output to be sent to a control module; the bottom layer planner adopts an SLT dimension reduction method to make a decision and plan process as follows:
(1) The SLT dimension reduction method is adopted to divide the system into an SL layer and an ST layer for planning, and then a path and speed planning problem is constructed in an SL coordinate system and an ST coordinate system:
wherein l represents a lateral offset of the autonomous vehicle path relative to the centerline of the roadway and s represents a longitudinal offset of the autonomous vehicle path along the centerline of the roadway; t represents the moment of corresponding longitudinal offset in the speed planning;
(2) Based on static and low-speed obstacle projection, establishing an SL graph, discretizing a state space, and carrying out path decision planning by adopting a heuristic search method and a numerical optimization method;
(3) Based on dynamic obstacle track prediction, an ST diagram is established, state space discretization is carried out, and a heuristic search method and a numerical optimization method are adopted to carry out speed decision planning.
Further, the cartesian coordinate system is converted into the Frenet coordinate system in the planning process of the bottom layer planner, and the Frenet coordinate system is converted into the global cartesian coordinate system before the track information is sent to the control module.
Further, different non-uniform sampling scales are set according to tunnel scenes in path planning in the SL diagram and speed planning in the ST diagram, state space discretization is firstly carried out, cost values at discrete points are distributed according to a cost function, and initial solution is quickly obtained by adopting improved A-algorithm heuristic search; the initial solution is used as a decision scheme to open up a safety space, the original safety space problem is converted into a convex optimization problem, and an optimal track solution is obtained by adopting a convex optimization solving method under constraint conditions; in the numerical optimization process of path planning in the SL diagram, the cost function is as follows:
wherein w is 1 、w 2 、w 3 、w 4 、w 5 Weights, l, for each term in the cost function i 、l centre The longitudinal offsets of the paths and reference lines in the SL graph are shown, respectively;
in the numerical optimization process of speed planning in the ST diagram, the cost function is as follows:
Wherein w is 1 、w 2 、w 3 、w 4 For the weight of each term in the cost function, s i 、v ref The lateral displacement amount of the path sum in the ST diagram and the reference velocity are shown, respectively.
Further, when meeting a narrow tunnel meeting and an intersection meeting scene, triggering a coordination node and performing advanced planning according to specific conditions, wherein the method comprises the following steps:
according to the planned track of the bottom layer of the self-driven vehicle, the self-driven vehicle normally runs, when meeting a narrow tunnel meeting and an intersection meeting scene, a coordination node is triggered, whether the original track of the self-driven vehicle conflicts or not is firstly judged, and if no conflict exists, the self-driven vehicle runs according to the original track; if collision exists, a collision area is formed in the vehicle interaction area, and a buffer coordination area is formed in front of the collision area of the narrow tunnel meeting of the mine and the intersection meeting scene; one or more automatic driving vehicles in the mine tunnel drive into the buffer coordination area to slow down or stop, and wait for coordination in sequence;
the coordination node receives driving maneuver states of all automatic driving vehicles in the intersection buffer coordination area through V2I communication, wherein the advanced planner carries out coordination planning on the vehicles in all the buffer coordination areas to generate a coordination reference track of the passing conflict area, and the coordination reference track only considers the automatic driving vehicles in the buffer coordination area and does not consider other static or dynamic obstacles; the path generation of the advanced planner adopts a smooth optimization method based on straight lines and circular arcs, and firstly, the straight lines and the circles are sampled:
Knots：{(x k,m ,y k,m ,s k,m )；m＝0,1,…,n k }
Anchorpoints：{(x a,j ,y a,j ,s a,j )；j＝0,1,…,n a }
Wherein Knots and anchors represent nodes and anchor points dividing straight lines and circles, m and j represent numbers of corresponding nodes and points, (x) k,m ,y k,m ,s k,m ) Respectively representing the abscissa and ordinate of the node and the total length of the divided straight line, (x) a,j ,y a,j ,s a,j ) Respectively representing the abscissa and ordinate of the anchor point and the total length of the divided circle;
the reference paths between every two adjacent nodes are connected by using a polynomial of five times, and then smooth and feasible paths are searched nearby the straight line path and the round path by adopting an optimization method;
the advanced planner carries out speed planning on all automatic driving vehicles in a range in an ST graph based on the path planning result of each vehicle, firstly projects interaction between each vehicle and a conflict area into the ST graph, then carries out state space discretization and determines the passing sequence of the automatic driving vehicles, and sequentially carries out initial solution searching and optimizing of the speed; and the speed planning of the advanced planner meets the constraint condition that the conflict area is occupied by the same vehicle at the same time;
the automatic driving vehicle enters a buffer coordination area, the coordination node realizes V2I communication with the automatic driving vehicle in the area through a PC5 direct communication interface of C-V2X, road side perception and vehicle-mounted perception are connected together by utilizing a V2I communication technology, the requirements of low time delay and high reliability of data transmission are met, a reliable information transmission channel is established, and multidimensional and full-scale perception information sharing and cooperative scheduling control are realized;
When the coordination node judges that the track of the automatic driving vehicle at each intersection has a conflict relation with the tracks of other automatic driving vehicles, the advanced planner determines a planning starting point based on the current maneuvering state of the automatic driving vehicle, and then the coordination buffer coordination area is re-planned for all vehicles.
Further, the planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, adopts path and speed decoupling to plan respectively, and solves the feasible self-vehicle track iteratively, comprising the following steps:
the planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, the respective automatic driving vehicle establishes a Frenet coordinate system according to the coordination reference line, intersection obstacle information perceived by the vehicle-mounted sensor is projected into the SL diagram and the ST diagram, the self-vehicle bottom layer planner carries out re-planning, the self-vehicle bottom layer planner adopts path and speed decoupling to plan respectively, and feasible self-vehicle tracks are solved iteratively;
the time of entering and exiting conflict areas of the respective motor-driven vehicles output by the advanced planner is used as a limiting area of the ST diagram in the bottom planning process, so that no conflict exists between the output track of the self-vehicle re-planning and the output tracks of other motor-driven vehicles:
wherein t is sl 、t el Time domain boundary of traffic conflict area for own vehicle in speed planning result of advanced planner, t in 、t out The interaction time between the speed planning result of the own-vehicle bottom layer planner and the conflict area is represented;
the speed of the conflicting zone passes should meet the speed constraint:
wherein the method comprises the steps ofRepresenting the speed planning result of the bottom layer planner and being smaller than the conflict area speed limit v l ；
The high-level planner plans out the coordination reference track of each mobile driving vehicle in the corresponding scene, and the bottom planner of each mobile driving vehicle takes the coordination reference track as input to carry out own weight planning so as to avoid encountering various barriers when passing through the conflict area, and the high-level planner and the bottom planner jointly ensure that each vehicle passes through the conflict area in sequence in a safe and coordinated manner.
Further, the vehicle tracks of the respective driving vehicles are used as input to the control module, the control module executes transverse and longitudinal control of the driving vehicles to complete vehicle meeting actions through a mine intersection, the vehicle tracks of the driving vehicles are used as input to the control module after being converted into a coordinate system, wherein the transverse control adopts a model predictive control method, and the longitudinal control adopts a PID control method.
The invention has the beneficial effects that:
1. aiming at an automatic driving scene under a mine, an advanced planner based on a vehicle-road cooperative system is adopted to consider the characteristics of vehicle meeting at an intersection and a non-intersection under the mine, and a collision-free reference track is provided for all vehicles in a buffer coordination area;
2. Considering a coordination strategy of a collision area of a vehicle meeting under a mine and obstacle avoidance during vehicle passing, and rescheduling by combining a self-floor planner to obtain a safe and feasible collision-free track by an automatic driving vehicle based on a reference track of a high-level planner;
3. and the comprehensive multi-layer planner realizes cooperative dispatch control and improves the passing efficiency of automatic driving vehicles under the mine.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required to be used in the embodiments or the description of the prior art will be briefly described below, and it is obvious that the drawings in the following description are only embodiments of the present invention, and that other drawings can be obtained according to the provided drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a planning method according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a system interaction architecture of each module under a tunnel according to an embodiment of the present invention;
FIG. 3 is a schematic view of a narrow tunnel meeting scene according to an embodiment of the present invention;
FIG. 4 is a schematic view of a tunnel junction meeting scene according to an embodiment of the present invention;
FIG. 5 is a schematic diagram of a vehicle speed planning coordination result according to an embodiment of the present invention;
FIG. 6 is a block diagram of an autonomous vehicle planner in accordance with an embodiment of the invention.
Detailed Description
In the following, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the embodiments of the present invention, and it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention provides a decision-making planning method for a vehicle meeting scene of an automatic driving vehicle under a mine, which is based on the coordination planning of a vehicle-road cooperative system and an automatic vehicle bottom planning method to integrate two layers of planners so as to solve the problem of vehicle meeting traffic of the automatic driving vehicle under a narrow tunnel and an intersection scene, and a reasonable and efficient safe vehicle meeting scheme is planned in a coordinated manner according to the meeting characteristics and environmental constraints of a plurality of automatic driving vehicles under the mine so as to ensure safe and efficient vehicle meeting and improve the passing efficiency under the complex environment of the mine tunnel.
Referring to fig. 1, the invention provides a vehicle-road-cooperation-based method for planning the coordination of an automatic driving vehicle under a mine, which is characterized by comprising the following steps:
The automatic driving road obtains a mine underground high-precision map provided by the cloud platform, and planning of a global smooth navigation path is carried out based on a road center line in the high-precision map, so that complete reference line smoothing is realized;
according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle carries out path and speed decision planning, and track information output by the bottom layer planner is used as an output to be sent to the control module;
according to the planning track of the bottom layer of the self-vehicle, the self-driving vehicle normally runs, when meeting a narrow tunnel meeting and an intersection meeting scene, a coordination node is triggered, and advanced planning is carried out according to specific conditions;
the planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, adopts path and speed decoupling to plan respectively, and solves the feasible self-vehicle track iteratively;
the vehicle track of each driving vehicle is used as input to the control module, and the control module executes the transverse and longitudinal control of the driving vehicle to pass through the mine crossing so as to complete the vehicle meeting action.
In this embodiment, an automatic driving vehicle road obtains a mine under-mine high-precision map provided by a cloud platform, and performs planning of a global smooth navigation path based on a road center line in the high-precision map to realize complete reference line smoothing, including the following steps:
The method comprises the steps that an automatic driving road obtains a mine under-mine high-precision map provided by a cloud platform, and planning of a global smooth navigation path is firstly carried out based on a road center line in the high-precision map; wherein the road center line is a discrete point set, which is used as a reference line after smoothing, three times of polynomials are adopted to connect the discrete point sets of the road center line and uniformly sample the encrypted center line discrete points, and the polynomials connect adjacent discrete points (x i ,y i ) And (x) i+1 ,y i+1 )：
y＝f(x)＝a 0 +a 1 x+a 2 x 2 +a 3 x 3
Wherein a0, a1, a2, a3 respectively represent the 0 th order coefficient, the 1 st order coefficient, the 2 nd order coefficient, the 3 rd order coefficient of the cubic polynomial;
carrying out smooth reference line planning, searching for a self-vehicle projection point in a discrete point set in a planning period, carrying out segmentation processing based on the projection point, and taking a segmented path as a path segment to be smoothed; converting the reference line smoothing problem into a quadratic programming problem based on a segmented path segment consisting of densified discrete points, and solving according to a cost function and constraint conditions of smoothing of a segmented center line point set to obtain a smoothed reference line point set; and finally, splicing the reference line segments in different periods, thereby realizing complete reference line smoothing. The cost function of the centerline point set smoothing is:
Wherein w is 1 、w 2 、w 3 Weights, x, for each term in the cost function i 、y i And x ref 、y ref The abscissa and ordinate of the reference line and the densified center line, respectively.
The reference line is used as a navigation path of the automatic driving vehicle under the condition of meeting at a non-intersection, and the automatic driving vehicle normally runs along the navigation path.
The underground high-precision map stores road data and fixed opposite information of an underground tunnel as structured data, in the process of carrying out reference line smoothing on a road center line, projection points of a self-vehicle on the road center line in each automatic driving vehicle planning period are taken as starting points, point sets in a certain range before and after the starting points are smoothed, and the smoothed point sets are taken as reference lines.
In the planning process of the bottom layer planner, the Cartesian coordinate system is converted into the Frenet coordinate system, and the Frenet coordinate system is converted into the global Cartesian coordinate system before the track information is sent to the control module.
In this embodiment, according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle performs path and speed decision planning, and sends the track information output by the bottom layer planner as an output to the control module, which includes the following steps:
according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle carries out path and speed decision planning based on a Frenet coordinate system taking the navigation path as a coordinate axis, and track information output by the bottom layer planner is used as an output to be sent to a control module; the bottom layer planner adopts an SLT dimension reduction method to make a decision and plan process as follows:
(1) The SLT dimension reduction method is adopted to divide the system into an SL layer and an ST layer for planning, and then a path and speed planning problem is constructed in an SL coordinate system and an ST coordinate system:
wherein l represents a lateral offset of the autonomous vehicle path relative to the centerline of the roadway and s represents a longitudinal offset of the autonomous vehicle path along the centerline of the roadway; t represents the moment of corresponding longitudinal offset in the speed planning;
(2) Based on static and low-speed obstacle projection, establishing an SL graph, discretizing a state space, and carrying out path decision planning by adopting a heuristic search method and a numerical optimization method;
(3) Based on dynamic obstacle track prediction, an ST diagram is established, state space discretization is carried out, and a heuristic search method and a numerical optimization method are adopted to carry out speed decision planning.
The method comprises the steps of setting different non-uniform sampling scales according to tunnel scenes by path planning in an SL diagram and speed planning in an ST diagram, performing state space discretization firstly, distributing cost values at discrete points according to a cost function, and quickly obtaining initial solution by adopting improved A-algorithm heuristic search; the initial solution is used as a decision scheme to open up a safety space, the original safety space problem is converted into a convex optimization problem, and an optimal track solution is obtained by adopting a convex optimization solving method under constraint conditions; in the numerical optimization process of path planning in the SL diagram, the cost function is as follows:
Wherein w is 1 、w 2 、w 3 、w 4 、w 5 Weights, l, for each term in the cost function i 、l centre The longitudinal offsets of the paths and reference lines in the SL graph are shown, respectively;
in the numerical optimization process of speed planning in the ST diagram, the cost function is as follows:
wherein w is 1 、w 2 、w 3 、w 4 For the weight of each term in the cost function, s i 、v ref The lateral displacement amount of the path sum in the ST diagram and the reference velocity are shown, respectively.
In this embodiment, when meeting a narrow tunnel meeting and an intersection meeting scene, the coordination node is triggered, and advanced planning is performed according to specific conditions, including the following steps:
according to the planned track of the bottom layer of the self-driven vehicle, the self-driven vehicle normally runs, when meeting a narrow tunnel meeting and an intersection meeting scene, a coordination node is triggered, whether the original track of the self-driven vehicle conflicts or not is firstly judged, and if no conflict exists, the self-driven vehicle runs according to the original track; if collision exists, a collision area is formed in the vehicle interaction area, and a buffer coordination area is formed in front of the collision area of the narrow tunnel meeting of the mine and the intersection meeting scene; one or more automatic driving vehicles in the mine tunnel drive into the buffer coordination area to slow down or stop, and wait for coordination in sequence;
the coordination node receives driving maneuver states of all automatic driving vehicles in the intersection buffer coordination area through V2I communication, wherein the advanced planner carries out coordination planning on the vehicles in all the buffer coordination areas to generate a coordination reference track of the passing conflict area, and the coordination reference track only considers the automatic driving vehicles in the buffer coordination area and does not consider other static or dynamic obstacles; the path generation of the advanced planner adopts a smooth optimization method based on straight lines and circular arcs, and firstly, the straight lines and the circles are sampled:
Knots：{(x k,m ,y k,m ,s k,m )；m＝0,1,…,n k }
Anchorpoints：{(x a,j ,y a,j ,s a,j )；j＝0,1,…,n a }
Wherein Knots and anchors represent nodes and anchor points dividing straight lines and circles, m and j represent numbers of corresponding nodes and points, (x) k,m ,y k,m ,s k,m ) Respectively representing the abscissa and ordinate of the node and the total length of the divided straight line, (x) a,j ,y a,j ,s a,j ) Respectively representing the abscissa and ordinate of the anchor point and the total length of the divided circle;
the reference paths between every two adjacent nodes are connected by using a polynomial of five times, and then smooth and feasible paths are searched nearby the straight line path and the round path by adopting an optimization method;
the advanced planner carries out speed planning on all automatic driving vehicles in a range in an ST graph based on the path planning result of each vehicle, firstly projects interaction between each vehicle and a conflict area into the ST graph, then carries out state space discretization and determines the passing sequence of the automatic driving vehicles, and sequentially carries out initial solution searching and optimizing of the speed; and the speed planning of the advanced planner meets the constraint condition that the conflict area is occupied by the same vehicle at the same time;
and triggering a coordination node during the meeting scene, wherein the coordination node comprises two situations of a narrow tunnel meeting scene and a multi-intersection meeting scene.
Aiming at the situation of vehicle meeting in a narrow tunnel under a mine, the road is narrow and the road width is not uniform, and the collision-free condition of the vehicle meeting cannot be met, so that concave avoidance areas are arranged in the narrow tunnel of the mine at certain intervals so as to facilitate the vehicle meeting; the tunnel multi-intersection is a non-narrow tunnel road, and the road width is enough to pass two automatic driving vehicles simultaneously, so that the multi-vehicle meeting condition is met.
The vehicle-road cooperative system is mainly based on a road side system and a vehicle-mounted system, so that information in a mine tunnel area is acquired and collected, information is transmitted in time through a wireless communication technology, and data storage and intelligent decision making are realized based on a cloud control platform; the automatic driving vehicle-mounted system comprises a high-precision map module, an environment sensing module, a prediction module, a positioning module, a decision planning module and a vehicle control module.
The automatic driving vehicle enters a buffer coordination area, the coordination node realizes V2I communication with the automatic driving vehicle in the area through a PC5 direct communication interface of C-V2X, road side perception and vehicle-mounted perception are connected together by utilizing a V2I communication technology, the requirements of low time delay and high reliability of data transmission are met, a reliable information transmission channel is established, and multidimensional and full-scale perception information sharing and cooperative scheduling control are realized;
when the coordination node judges that the track of the automatic driving vehicle at each intersection has a conflict relation with the tracks of other automatic driving vehicles, the advanced planner determines a planning starting point based on the current maneuvering state of the automatic driving vehicle, and then the coordination buffer coordination area is re-planned for all vehicles.
In this embodiment, the planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, adopts path and speed decoupling to plan respectively, and solves the feasible vehicle track iteratively, including the following steps:
The planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, the respective automatic driving vehicle establishes a Frenet coordinate system according to the coordination reference line, intersection obstacle information perceived by the vehicle-mounted sensor is projected into the SL diagram and the ST diagram, the self-vehicle bottom layer planner carries out re-planning, the self-vehicle bottom layer planner adopts path and speed decoupling to plan respectively, and feasible self-vehicle tracks are solved iteratively;
the time of entering and exiting conflict areas of the respective motor-driven vehicles output by the advanced planner is used as a limiting area of the ST diagram in the bottom planning process, so that no conflict exists between the output track of the self-vehicle re-planning and the output tracks of other motor-driven vehicles:
wherein t is sl 、t el Representing advanced planner speed planning junctionsTime domain boundary, t, of traffic collision area for own vehicle in fruit in 、t out The interaction time between the speed planning result of the own-vehicle bottom layer planner and the conflict area is represented;
the speed of the conflicting zone passes should meet the speed constraint:
wherein the method comprises the steps ofRepresenting the speed planning result of the bottom layer planner and being smaller than the conflict area speed limit v l ；
The high-level planner plans out the coordination reference track of each mobile driving vehicle in the corresponding scene, and the bottom planner of each mobile driving vehicle takes the coordination reference track as input to carry out own weight planning so as to avoid encountering various barriers when passing through the conflict area, and the high-level planner and the bottom planner jointly ensure that each vehicle passes through the conflict area in sequence in a safe and coordinated manner.
In this embodiment, the vehicle tracks of the respective driving vehicles are input to the control module, the control module performs transverse and longitudinal control of the driving vehicles through the mine intersection to complete the vehicle meeting action, and the vehicle tracks of the driving vehicles are converted by the coordinate system and then input to the control module, wherein the transverse control adopts a model predictive control method (MPC, model Predictive Control), and the longitudinal control adopts a PID control method.
Examples
Referring to fig. 1, the method for coordinated planning of an underground automatic driving vehicle based on vehicle-road coordination provided by the embodiment of the invention is applied to a vehicle with an automatic driving function, and the vehicle can implement the method through software and/or hardware, and the method specifically comprises steps S01 to S04.
Step S01: firstly, carrying out smoothing treatment on a discrete point set of a road center line in a high-precision map under a mine, and then using the discrete point set as a reference line so as to facilitate coordinate conversion and projection, wherein the scheme for smoothing the road center line is as follows:
connecting discrete point sets of the road center line provided by the high-precision map by adopting a cubic polynomial, and uniformly sampling after connecting to obtain a denser center line discrete point set; in each planning period, the point, closest to the vehicle position, of the discrete point center is found by the automatic driving vehicle, and the point is approximately regarded as a projection point of the automatic driving vehicle on the road center line; then carrying out segmentation processing, wherein a central line discrete point set with a certain distance from the front and back of the projection point is used as a path segment to be smoothed; the path segment smoothing processing is carried out, the smoothing method converts a smoothing problem into a quadratic programming problem to be solved based on a densified discrete center line point set, and the smoothing problem is solved according to a cost function and constraint conditions of the segmented center line point set smoothing to obtain a smoothing reference line point set; splicing the smoothed reference line segments in different planning periods, so that complete reference line smoothing is realized;
The cubic polynomial connects adjacent discrete points (x i ,y i ) And (x) i+1 ,y i+1 ) The polynomial equation thereof can be expressed as:
y＝f(x)＝a 0 +a 1 x+a 2 x 2 +a 3 x 3
the above equation represents a polynomial expression between any two adjacent points in the densified n discrete point sets, where 0.ltoreq.i.ltoreq.n-1, provided (x) i ,y i ) And (x) i+1 ,y i+1 ) The derivatives at are k respectively i And k i+1 Then the cubic polynomial at the two points satisfies:
determining polynomial coefficients a from the above conditions 0 -a 3 And obtaining a segmented polynomial, and sampling at uniform intervals to obtain a densification point set.
The cost function of the segmented centerline point set smoothing mainly comprises three terms, namely a center offset term, a length uniform term and a smoothness term, and each cost function expression is as follows:
the center offset term Obj1, the length uniform term Obj2 and the smoothness term Obj3 are multiplied by the corresponding weight coefficients to form a total cost function of reference line smoothness, and meanwhile, in order to avoid overlarge offset, constraint is carried out on reference points, and the total cost function and constraint conditions are as follows:
Function cost ＝w 1 ×Obj1+w 2 ×Obj2+w 3 ×Obj3
d in limx 、d limy Representing the set of centerline points allows for maximum offset in the x-direction and y-direction, the optimization problem is defined as a quadratic programming optimization problem,
s.t.x ref -d limx ≤x i ≤x ref +d limx
y ref -d limy ≤y i ≤y ref +d limy
and calling a quadratic programming problem solving library, and rapidly solving the quadratic programming problem by using osqp as a solving tool, wherein the obtained solution set is used as a reference line point set.
Step S02: then, based on a smooth reference line, a Frenet coordinate system is established by taking a projection point of the vehicle on the reference line as a coordinate origin, so as to prepare for path decision planning and speed decision planning of an automatic driving vehicle bottom layer planner; firstly, finding projection information of a vehicle coordinate on a reference line, and obtaining coordinate, orientation and curvature information of the projection point under a Cartesian coordinate system based on a denser reference line point set, wherein the searched matching point is approximately the projection point; obtaining information of corresponding coordinate points under the Frenet coordinate system according to the conversion relation between the Cartesian coordinate system and the Frenet coordinate system; then, decoupling an SLT three-dimensional problem in a Frenet coordinate system into two-dimensional problems of an SL layer and an ST layer, and carrying out path decision and planning by combining a quadratic programming through a space discretization method based on projection information of an SL image and a static obstacle; the ST diagram is built again with the path result, dynamic obstacle projection is carried out, and similar space discretization method is adopted to combine with quadratic programming to carry out speed decision and planning; finally, combining the path planning result and the speed planning result to obtain track information;
in a Frenet coordinate system established by the projection of the vehicle on the reference line, the vehicle is taken as an origin, the vehicle is divided into an S direction and an L direction, a vertical axis S represents the driving distance of the vehicle in a road, a horizontal axis L represents the distance of the vehicle deviating from the reference line, and the horizontal axis and the vertical axis are mutually perpendicular;
The matching points are approximate to the projection points, the point set of the reference line is dense enough on the premise that the matching points are approximate to the projection points, the position errors of the matching points and the projection points are in a smaller range, and the projection points are calculated through the matching points when the position errors exceed the range;
the SLT three-dimensional problem is decoupled and considered from an SL layer and an ST layer respectively, wherein the SL layer problem is mainly solved based on an SL diagram and a space discretization method, the ST layer problem is mainly solved based on an ST diagram and the space discretization method, and the discretization scale can be dynamically adjusted according to the underground road width of an ore well and the speed limit of an automatic driving vehicle;
the obstacle information projection obtained by the sensing module is based on an SL diagram and an ST diagram, and the obstacles are divided into a static obstacle, a low-speed obstacle and a high-speed obstacle according to the speed of the obstacle, wherein the static obstacle and the low-speed obstacle are projected into the SL diagram and considered in path planning; high-speed obstacles are projected into the ST image, and are considered in speed planning;
the high-speed obstacle refers to a moving object with a speed greater than a certain speed under the speed limiting condition of an automatic driving vehicle under a mine, and mainly comprises ground pedestrians, other vehicles and the like; the high-speed barrier performs ST map projection and relies on a prediction track obtained by a prediction module, the prediction method adopts a Gaussian mixture model to model the motion mode of the moving object, and the probability distribution of different motion modes is counted, so that the track is divided into different Gaussian process components, and accurate and efficient position prediction is realized;
The method comprises the steps that a discrete grid point-formed path is obtained by adopting an A-algorithm searching method based on an SL graph, is used as a path decision to develop a safety space under the current road condition, and then a path optimization problem is constructed in an SL coordinate system:
l＝f(s)
the path optimization problem under the safety space is built into the quadratic programming problem again, and the cost function can be expressed as follows:
each item of the cost function respectively represents a first derivative cost, a second derivative cost, a third derivative cost, a transverse offset cost and a central line cost, and each item is preceded by a corresponding cost weight, and the constraint conditions are as follows:
A eq x＝b eq ,Ax≤b,lb≤x≤ub
x＝(l 1 ,l’ 1 ,l” 1 ,…l n ,l' n ,l” n ) T
wherein A is eq A represents a matrix of corresponding dimensions in the equality constraint and the inequality constraint, b eq B, lb, ub represent column vectors in the constraint; x represents the column vector of the optimization variables and their higher derivatives in the constructed optimization problem.
The corresponding constraint conditions are continuity equality constraint, collision inequality constraint and upper and lower bound constraint respectively, and the specific calculation is carried outThe resolver can quickly obtain l 1 ,l’ 1 ,l” 1 ,…l n ,l’ n ,l” n And s 1 ,s 2 ,s 3 ,…s n A combined quadratic programming path result; s is(s) 1 ,s 2 ,s 3 ,…s n Representing the corresponding longitudinal displacement in the n optimization problem solutions, respectively.
The speed planning step is similar to path planning, and is based on the predicted obstacle track and the space discretization of projection on an ST image, wherein the obstacle track prediction depends on the data execution of a perception module, a decision speed scheme is obtained according to a dynamic planning method, a safety space is opened up, the final speed optimization is constructed into a quadratic programming problem, a cost function respectively comprises a reference speed cost item and a one-to-three-order derivative cost item, and the total cost function can be expressed as:
A eq x＝b eq ,Ax≤b,lb≤x≤ub
The corresponding constraint conditions in the formula are continuity constraint, speed and acceleration inequality constraint and safety space upper and lower bound constraint respectively, and a speed planning result is obtained through a solver, so that the path planning result is combined to complete track planning;
step S03: according to the planned track of the bottom layer of the self-driven vehicle, the self-driven vehicle normally runs, when meeting a narrow tunnel meeting and an intersection meeting scene, a coordination node is triggered, whether the original track of the self-driven vehicle conflicts or not is firstly judged, and if no conflict exists, the self-driven vehicle runs according to the original track; if collision exists, a collision area is formed in the vehicle interaction area, and a buffer coordination area is formed in front of the collision area of the narrow tunnel meeting of the mine and the intersection meeting scene; one or more automatic driving vehicles in the mine tunnel drive into the buffer coordination area to slow down or stop, and wait for coordination in sequence; the coordination node receives driving maneuvering states of all automatic driving vehicles in the buffer coordination area through V2I communication based on the vehicle-road coordination system, and the information exchange between the vehicles and intelligent facilities in the tunnel enables the vehicles and road infrastructure to share the states, positions and intentions of the vehicles and the road infrastructure;
the advanced planner in the coordination node coordinates the path and the speed of the automatic driving vehicle under a narrow tunnel scene to determine concave avoidance areas in the two-direction buffer coordination areas and generate conflict areas; the coordination process of the advanced planner firstly generates a target point in a concave avoidance area, plans a track of a target position of one automatic driving vehicle in the concave avoidance area, enables the automatic driving vehicle to drive into the concave avoidance area of a narrow tunnel and avoid before the opposite vehicle enters the conflict area, plans a collision-free meeting track for the vehicles of the opposite meeting, and ensures that the respective automatic driving vehicles pass through the conflict area safely in sequence; when entering a crossing meeting scene under a mine, automatic driving vehicles at each crossing enter respective buffer coordination areas, coordination nodes coordinate all vehicles in the buffer coordination areas, and an advanced planner plans a path of a collision-free passing crossing, wherein speed planning ensures that only one vehicle can occupy the same time in the crossing collision area, namely, the time of any two automatic driving vehicles passing through the collision area between the crossings cannot be overlapped, and finally, coordination is completed, and the vehicles at each crossing pass through the crossing in sequence to complete meeting;
Referring to fig. 2-4, in the under-tunnel infrastructure system frame, each module coordinates and operates normally; the sensor layout scheme for comprehensively covering all intersections is realized in the detection area with coordinated buffering, and all conflict targets in the traveling area are detected without dead angles; the underground roadside end deployment radar, cameras and other roadside sensors assist the vehicle to sense the front vehicle information based on roadside equipment, vehicle-mounted units and V2I communication modes, and reliable basis is provided for the advanced planners and the bottom layer planners;
the path generation of the advanced planner adopts a smooth optimization method based on straight lines and circular arcs, and firstly, the straight lines and the circles are sampled:
Knots：{(x k,m ,y k,m ,s k,m )；m＝0,1,…,n k }
Anchorpoints：{(x a,j ,y a,j ,s a,j )；j＝0,1,…,n a }
wherein Knots and anchors represent nodes and anchor points dividing straight lines and circles, m and j represent numbers of corresponding nodes and points, (x) k,m ,y k,m ,s k,m ) Respectively representing the abscissa and ordinate of the node and the total length of the divided straight line, (x) a,j ,y a,j ,s a,j ) Respectively the abscissa of the anchor point and the total length of the divided circle.
The reference path between every two nodes uses a fifth order polynomial connection:
wherein x and y represent the abscissa, which is formed by a five-degree polynomial, connecting two nodes, and k m0 -k m5 And b m0 -b m5 Representing the respective fifth degree polynomial f m (s) and g m Coefficients of(s);
the smooth feasible paths are then searched for in the vicinity of the straight and circular paths using an optimized approach,
s.t.|f(s a,j )-x a,j |＜ε
|g(s a,j )-y a,j |＜ε
in which the optimization problem is constructedRepresenting the coefficients f(s) a,j )-x a,j And g(s) a,j )-y a,j Represents the offset between the optimization solution and the original fifth-degree polynomial, epsilon represents the maximum allowable offset of the limiting path point, ++>Andrepresenting a continuous constraint that guarantees a polynomial at spline nodes by matching the endpoints of the gamma derivative. m=0, 1, …, n k -2,j＝0,1,…,n a γ=0, 1,2, 3; the optimization function respectively ensures the curvature continuity and the path smoothness, the maximum allowable offset of the path points is limited by the first two constraint conditions, the continuity is ensured by the last two constraint conditions, and a path solution is obtained by a quadratic programming solving method;
the advanced planner carries out speed planning on all automatic driving vehicles in a range in an ST graph based on the path planning result of each vehicle, firstly projects interaction between each vehicle and a conflict area into the ST graph, then carries out state space discretization and determines the passing sequence of the automatic driving vehicles, and sequentially carries out initial solution searching and optimizing of the speed; and the speed planning of the advanced planner meets the constraint condition that the conflict area is occupied by the same vehicle at the same time; the initial solution searching and optimizing process of the speed planning is similar to the speed planning step of the bottom layer planner;
The collaborative planning process will be further described from narrow tunnel non-intersection and multi-intersection meeting scenarios:
as shown in fig. 3, in a narrow tunnel meeting scene, a collision zone is projected into an SL image of a vehicle for avoiding in a path planning process, so that an avoidance path is planned, and virtual obstacles in the collision zone in the SL image of the vehicle for avoiding are removed when the opposite vehicle finishes passing the collision zone; in the speed planning process, a parking action is planned for a vehicle entering a concave avoidance zone in an ST diagram according to a path generation result, and the passing sequence is shown in fig. 5 (a); in fig. 3, each vehicle is in a buffer coordination area at time t=t0, the advanced planner performs coordination planning, when t=t2, the autonomous vehicle 1 enters a concave avoidance area according to a track planning result to finish parking avoidance, and when the autonomous vehicle 2 is passing through a conflict area to perform vehicle meeting action, the time corresponds to between t=ta and t=tb in fig. 5 (a);
as shown in fig. 4, in a multi-intersection meeting scene, vehicles at all intersections generate all vehicle tracks passing through the conflict area in the buffer coordination area; completing path planning according to the running intention of each driving vehicle, carrying out speed planning in an ST graph based on a path result, wherein collision areas are occupied by the same vehicle at the same moment, as shown in fig. 5 (b), lines 1, 2, 3 and 4 in the ST graph are respectively speed planning passing sequences of four driving vehicles, a shaded part represents ST graph projection of crossing collision areas of each driving vehicle at a certain moment, the time dimension of the ST graph is the same when the speed of each meeting vehicle is planned by advanced planning, and the time domains of the passing collision areas in the ST graph of each vehicle in a coordination planning result are not intersected;
The relation between the high-level planner and the bottom-layer planner is shown in fig. 6, the high-level planner outputs reference track information to replace the original reference track of the automatic driving vehicle, the bottom-layer planner is iteratively executed to process other obstacle avoidance problems of the passing conflict area, a Frenet frame is established according to the given reference path, an automatic obstacle avoidance path is generated, and automatic re-planning is carried out according to the time of entering and exiting the conflict area of the respective driving vehicle output by the high-level planner as a limiting area in an SL graph to ensure the safety of meeting;
step S04: the complete planning control flow is subjected to reference line planning, interaction of a bottom layer planner and an advanced planner, the final output result of the planning module is not directly sent to the control module, and the final output result is firstly converted from a Frenet coordinate system to a Cartesian coordinate system, so that track information under the Cartesian coordinate system is obtained and sent to the control module for transverse and longitudinal control;
the transverse control adopts a model predictive control method, the longitudinal control adopts a PID control method, and the control model adopts a dynamics bicycle model in consideration of the fact that the running speed of an automatic driving vehicle under a mine is generally low; the decision planning module and the control module are combined to realize track tracking of the automatic driving vehicle under the mine tunnel.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims (7)

1. The under-mine automatic driving vehicle coordination planning method based on vehicle-road coordination is characterized by comprising the following steps of:
the automatic driving road obtains a mine underground high-precision map provided by the cloud platform, and planning of a global smooth navigation path is carried out based on a road center line in the high-precision map, so that complete reference line smoothing is realized;
according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle carries out path and speed decision planning, and track information output by the bottom layer planner is used as an output to be sent to the control module;
according to the planning track of the bottom layer of the self-vehicle, the self-driving vehicle normally runs, when meeting a narrow tunnel meeting and an intersection meeting scene, a coordination node is triggered, and advanced planning is carried out according to specific conditions;
The planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, adopts path and speed decoupling to plan respectively, and solves the feasible self-vehicle track iteratively;
the vehicle track of each driving vehicle is used as input to the control module, and the control module executes the transverse and longitudinal control of the driving vehicle to pass through the mine intersection so as to complete the vehicle meeting action;
according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle carries out path and speed decision planning, and the track information output by the bottom layer planner is used as an output to be sent to the control module, and the method comprises the following steps:
according to the planned smooth reference line, the bottom layer planner of the automatic driving vehicle carries out path and speed decision planning based on a Frenet coordinate system taking the navigation path as a coordinate axis, and track information output by the bottom layer planner is used as an output to be sent to a control module; the bottom layer planner adopts an SLT dimension reduction method to make a decision and plan process as follows:
(1) The SLT dimension reduction method is adopted to divide the system into an SL layer and an ST layer for planning, and then a path and speed planning problem is constructed in an SL coordinate system and an ST coordinate system:
wherein l represents a lateral offset of the autonomous vehicle path relative to the centerline of the roadway and s represents a longitudinal offset of the autonomous vehicle path along the centerline of the roadway; t represents the moment of corresponding longitudinal offset in the speed planning;
(2) Based on static and low-speed obstacle projection, establishing an SL graph, discretizing a state space, and carrying out path decision planning by adopting a heuristic search method and a numerical optimization method;
(3) Based on dynamic obstacle track prediction, building an ST graph, discretizing a state space, and carrying out speed decision planning by adopting a heuristic search method and a numerical optimization method;
setting different non-uniform sampling scales according to tunnel scenes by path planning and speed planning in an SL diagram and an ST diagram, performing state space discretization firstly, distributing cost values of discrete points according to a cost function, and quickly obtaining an initial solution by adopting an improved A-type algorithm heuristic search; the initial solution is used as a decision scheme to open up a safety space, the original safety space problem is converted into a convex optimization problem, and an optimal track solution is obtained by adopting a convex optimization solving method under constraint conditions; in the numerical optimization process of path planning in the SL diagram, the cost function is as follows:
wherein w is 1 、w 2 、w 3 、w 4 、w 5 Weights, l, for each term in the cost function i 、l centre The longitudinal offsets of the paths and reference lines in the SL graph are shown, respectively;
in the numerical optimization process of speed planning in the ST diagram, the cost function is as follows:
wherein w is 1 、w 2 、w 3 、w 4 For the weight of each term in the cost function, s i 、v ref The lateral displacement amounts of the path sums in the ST map and the reference speeds, respectively;
triggering a coordination node when meeting a narrow tunnel meeting and an intersection meeting scene, and performing advanced planning according to specific conditions, wherein the method comprises the following steps of:
according to the planned track of the bottom layer of the self-driven vehicle, the self-driven vehicle normally runs, when meeting a narrow tunnel meeting and an intersection meeting scene, a coordination node is triggered, whether the original track of the self-driven vehicle conflicts or not is firstly judged, and if no conflict exists, the self-driven vehicle runs according to the original track; if collision exists, a collision area is formed in the vehicle interaction area, and a buffer coordination area is formed in front of the collision area of the narrow tunnel meeting of the mine and the intersection meeting scene; one or more automatic driving vehicles in the mine tunnel drive into the buffer coordination area to slow down or stop, and wait for coordination in sequence;
the coordination node receives driving maneuver states of all automatic driving vehicles in the intersection buffer coordination area through V2I communication, wherein the advanced planner carries out coordination planning on the vehicles in all the buffer coordination areas to generate a coordination reference track of the passing conflict area, and the coordination reference track only considers the automatic driving vehicles in the buffer coordination area and does not consider other static or dynamic obstacles; the path generation of the advanced planner adopts a smooth optimization method based on straight lines and circular arcs, and firstly, the straight lines and the circles are sampled:
Knots：{(x k,m ,y k,m ,s k,m )；m＝0,1,…,n k }
Anchorpoints：{(x a,j ,y a,j ,s a,j )；j＝0,1,…,n a }
Wherein Knots and anchors represent nodes and anchor points dividing straight lines and circles, m and j represent numbers of corresponding nodes and points, (x) k,m ,y k,m ,s k,m ) Respectively representing the abscissa and ordinate of the node and the total length of the divided straight line, (x) a,j ,y a,j ,s a,j ) Respectively representing the abscissa and ordinate of the anchor point and the total length of the divided circle;
the reference paths between every two adjacent nodes are connected by using a polynomial of five times, and then smooth and feasible paths are searched nearby the straight line path and the round path by adopting an optimization method;
the advanced planner carries out speed planning on all automatic driving vehicles in a range in an ST graph based on the path planning result of each vehicle, firstly projects interaction between each vehicle and a conflict area into the ST graph, then carries out state space discretization and determines the passing sequence of the automatic driving vehicles, and sequentially carries out initial solution searching and optimizing of the speed; and the speed planning of the advanced planner meets the constraint condition that the conflict area is occupied by the same vehicle at the same time;
the automatic driving vehicle enters a buffer coordination area, the coordination node realizes V2I communication with the automatic driving vehicle in the area through a PC5 direct communication interface of C-V2X, road side perception and vehicle-mounted perception are connected together by utilizing a V2I communication technology, the requirements of low time delay and high reliability of data transmission are met, a reliable information transmission channel is established, and multidimensional and full-scale perception information sharing and cooperative scheduling control are realized;
When the coordination node judges that the track of the automatic driving vehicle at each intersection has a conflict relation with the tracks of other automatic driving vehicles, the advanced planner determines a planning starting point based on the current maneuvering state of the automatic driving vehicle, and then the coordination buffer coordination area is re-planned for all vehicles.
2. The method for collaborative planning of an underground automatic driving vehicle based on vehicle-road cooperation according to claim 1, wherein the automatic driving vehicle-road obtains an underground high-precision map provided by a cloud platform, and planning a global smooth navigation path based on a road center line in the high-precision map to realize complete reference line smoothing, comprises the following steps:
the method comprises the steps that an automatic driving road obtains a mine under-mine high-precision map provided by a cloud platform, and planning of a global smooth navigation path is firstly carried out based on a road center line in the high-precision map; wherein the road center line is a discrete point set, which is used as a reference line after smoothing, three times of polynomials are adopted to connect the discrete point sets of the road center line and uniformly sample the encrypted center line discrete points, and the polynomials connect adjacent discrete points (x i ,y i ) And (x) i+1 ,y i+1 )：
y＝f(x)＝a 0 +a 1 x+a 2 x 2 +a 3 x 3
Wherein a0, a1, a2, a3 respectively represent the 0 th order coefficient, the 1 st order coefficient, the 2 nd order coefficient, the 3 rd order coefficient of the cubic polynomial; x is x i 、y i Is the abscissa of the reference line;
carrying out smooth reference line planning, searching for a self-vehicle projection point in a discrete point set in a planning period, carrying out segmentation processing based on the projection point, and taking a segmented path as a path segment to be smoothed; converting the reference line smoothing problem into a quadratic programming problem based on a segmented path segment consisting of densified discrete points, and solving according to a cost function and constraint conditions of smoothing of a segmented center line point set to obtain a smoothed reference line point set; and finally, splicing the reference line segments in different periods, thereby realizing complete reference line smoothing.
3. The method for coordinated planning of an automatic driving vehicle under a mine based on vehicle-road coordination according to claim 2, wherein the cost function of the centerline point set smoothing is:
wherein w is 1 、w 2 、w 3 Weights, x, for each term in the cost function ref 、y ref Is the abscissa of the densified centerline.
4. The method for coordinated planning of an underground automatic driving vehicle based on vehicle-road cooperation according to claim 3, wherein the underground high-precision map stores road data and fixed opposite direction information of an underground tunnel as structured data, and in the process of smoothing a road center line, projection points of an automatic driving vehicle on the road center line are used as starting points in each automatic driving vehicle planning period, point sets in a certain range before and after the starting points are smoothed, and the smoothed point sets are used as reference lines.
5. The method for coordinated planning of an underground automatic driving vehicle based on vehicle-road coordination according to claim 1, wherein a cartesian coordinate system is converted into a Frenet coordinate system in the planning process of an underlying planner, and the Frenet coordinate system is converted into a global cartesian coordinate system before track information is sent to the control module.
6. The method for coordinated planning of an underground automatic driving vehicle based on vehicle-road cooperation according to claim 1, wherein the planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, adopts path and speed decoupling to plan respectively, and solves the feasible self-vehicle track iteratively, and comprises the following steps:
the planning result of the advanced planner replaces the original reference line of the automatic driving vehicle, the respective automatic driving vehicle establishes a Frenet coordinate system according to the coordination reference line, intersection obstacle information perceived by the vehicle-mounted sensor is projected into the SL diagram and the ST diagram, the self-vehicle bottom layer planner carries out re-planning, the self-vehicle bottom layer planner adopts path and speed decoupling to plan respectively, and feasible self-vehicle tracks are solved iteratively;
the time of entering and exiting conflict areas of the respective motor-driven vehicles output by the advanced planner is used as a limiting area of the ST diagram in the bottom planning process, so that no conflict exists between the output track of the self-vehicle re-planning and the output tracks of other motor-driven vehicles:
Wherein t is sl 、t el Time domain boundary of traffic conflict area for own vehicle in speed planning result of advanced planner, t in 、t out The interaction time between the speed planning result of the own-vehicle bottom layer planner and the conflict area is represented;
the speed of the conflicting zone passes should meet the speed constraint:
wherein the method comprises the steps ofRepresenting the speed planning result of the bottom layer planner and being smaller than the conflict area speed limit v l ；
The high-level planner plans out the coordination reference track of each mobile driving vehicle in the corresponding scene, and the bottom planner of each mobile driving vehicle takes the coordination reference track as input to carry out own weight planning so as to avoid encountering various barriers when passing through the conflict area, and the high-level planner and the bottom planner jointly ensure that each vehicle passes through the conflict area in sequence in a safe and coordinated manner.
7. The method for coordinated planning of underground automatic driving vehicles based on vehicle-road coordination according to claim 1, wherein the own track of each automatic driving vehicle is used as input to the control module, the control module executes transverse and longitudinal control of the automatic driving vehicle to complete vehicle meeting action through a mine intersection, the own track of the automatic driving vehicle is used as input to the control module after being converted into a coordinate system, wherein the transverse control adopts a model predictive control method, and the longitudinal control adopts a PID control method.
CN202310215591.3A 2023-03-07 2023-03-07 Mine automatic driving vehicle coordination planning method based on vehicle-road coordination Active CN117032201B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310215591.3A CN117032201B (en) 2023-03-07 2023-03-07 Mine automatic driving vehicle coordination planning method based on vehicle-road coordination

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310215591.3A CN117032201B (en) 2023-03-07 2023-03-07 Mine automatic driving vehicle coordination planning method based on vehicle-road coordination

Publications (2)

Publication Number Publication Date
CN117032201A CN117032201A (en) 2023-11-10
CN117032201B true CN117032201B (en) 2024-04-12

Family

ID=88626815

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310215591.3A Active CN117032201B (en) 2023-03-07 2023-03-07 Mine automatic driving vehicle coordination planning method based on vehicle-road coordination

Country Status (1)

Country Link
CN (1) CN117032201B (en)

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110456745A (en) * 2019-07-29 2019-11-15 湖南大学 A kind of Full-automatic underground mining haul system
CN111845774A (en) * 2020-07-20 2020-10-30 上海大学 Automatic driving automobile dynamic trajectory planning and tracking method based on transverse and longitudinal coordination
CN112068545A (en) * 2020-07-23 2020-12-11 哈尔滨工业大学（深圳） Method and system for planning driving track of unmanned vehicle at crossroad and storage medium
CN112272805A (en) * 2019-03-29 2021-01-26 百度时代网络技术（北京）有限公司 Multipoint enhancement-based splicing method for connecting two smooth reference lines
CN112634632A (en) * 2020-12-15 2021-04-09 北京百度网讯科技有限公司 Vehicle scheduling method and device, electronic equipment and storage medium
CN113359752A (en) * 2021-06-24 2021-09-07 中煤科工开采研究院有限公司 Automatic driving method for underground coal mine skip car
CN113432618A (en) * 2021-06-16 2021-09-24 深圳市道通智能汽车有限公司 Trajectory generation method and apparatus, computing device and computer-readable storage medium
CN113963562A (en) * 2021-12-21 2022-01-21 北京慧拓无限科技有限公司 Avoidance method and device for multiple vehicles in working area
CN114296439A (en) * 2021-06-23 2022-04-08 中煤科工开采研究院有限公司 Automatic driver vehicle for underground coal mine
CN114355891A (en) * 2021-12-09 2022-04-15 青岛慧拓智能机器有限公司 Intelligent unmanned transportation system for underground mine
CN114611289A (en) * 2022-03-11 2022-06-10 苏州挚途科技有限公司 Method and device for determining reference line in automatic driving and electronic equipment
CN114676939A (en) * 2022-05-26 2022-06-28 之江实验室 Multi-vehicle-type parameter self-adaptive reference line smoothing method and system

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3861291B1 (en) * 2019-12-20 2023-01-25 Baidu.com Times Technology (Beijing) Co., Ltd. Spline curve and spiral curve based reference line smoothing method

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112272805A (en) * 2019-03-29 2021-01-26 百度时代网络技术（北京）有限公司 Multipoint enhancement-based splicing method for connecting two smooth reference lines
CN110456745A (en) * 2019-07-29 2019-11-15 湖南大学 A kind of Full-automatic underground mining haul system
CN111845774A (en) * 2020-07-20 2020-10-30 上海大学 Automatic driving automobile dynamic trajectory planning and tracking method based on transverse and longitudinal coordination
CN112068545A (en) * 2020-07-23 2020-12-11 哈尔滨工业大学（深圳） Method and system for planning driving track of unmanned vehicle at crossroad and storage medium
CN112634632A (en) * 2020-12-15 2021-04-09 北京百度网讯科技有限公司 Vehicle scheduling method and device, electronic equipment and storage medium
CN113432618A (en) * 2021-06-16 2021-09-24 深圳市道通智能汽车有限公司 Trajectory generation method and apparatus, computing device and computer-readable storage medium
CN114296439A (en) * 2021-06-23 2022-04-08 中煤科工开采研究院有限公司 Automatic driver vehicle for underground coal mine
CN113359752A (en) * 2021-06-24 2021-09-07 中煤科工开采研究院有限公司 Automatic driving method for underground coal mine skip car
CN114355891A (en) * 2021-12-09 2022-04-15 青岛慧拓智能机器有限公司 Intelligent unmanned transportation system for underground mine
CN113963562A (en) * 2021-12-21 2022-01-21 北京慧拓无限科技有限公司 Avoidance method and device for multiple vehicles in working area
CN114611289A (en) * 2022-03-11 2022-06-10 苏州挚途科技有限公司 Method and device for determining reference line in automatic driving and electronic equipment
CN114676939A (en) * 2022-05-26 2022-06-28 之江实验室 Multi-vehicle-type parameter self-adaptive reference line smoothing method and system

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
多场景下无人驾驶汽车轨迹规划与控制方法研究;侯泽州;《中国知网网络出版》;参见第11, 13-14, 26-28, 37-45页 *

Also Published As

Publication number Publication date
CN117032201A (en) 2023-11-10

Similar Documents

Publication Publication Date Title
US11726477B2 (en) Methods and systems for trajectory forecasting with recurrent neural networks using inertial behavioral rollout
US20230347877A1 (en) Navigation Based on Detected Size of Occlusion Zones
US11554785B2 (en) Driving scenario machine learning network and driving environment simulation
US11897518B2 (en) Systems and methods for navigating with sensing uncertainty
US10976741B2 (en) Safety and comfort constraints for navigation
US11561551B2 (en) Prioritized constraints for a navigational system
JP6911214B1 (en) Orbit planning
US20220043456A1 (en) Trajectory selection for an autonomous vehicle
JP2021519720A (en) Time expansion and contraction method for autonomous driving simulation
US20200284591A1 (en) Aligning road information for navigation
US20220340177A1 (en) Systems and methods for cooperative driving of connected autonomous vehicles in smart cities using responsibility-sensitive safety rules
Qian Model predictive control for autonomous and cooperative driving
CN117032201B (en) Mine automatic driving vehicle coordination planning method based on vehicle-road coordination
US20240012106A1 (en) Multiple sensor calibration in autonomous vehicles performed in an undefined environment
US20240010223A1 (en) Multiple sensor calibration in autonomous vehicles performed in an undefined environment
CN115432008A (en) Optimization-multitask-based automatic driving decision planning method, electronic equipment and computer storage medium
Ochs et al. One Stack to Rule them All: To Drive Automated Vehicles, and Reach for the 4th level
US20230202529A1 (en) System and method of using a machine learning model to plan autonomous vehicles routes
US20230202521A1 (en) System and method of using an autolabeler to generate yield/assert labels based on on-road autonomous vehicle use
US20230202522A1 (en) System and method of spoofing a planning stack of an autonomous vehicle
CN114620058A (en) Trajectory planning method, trajectory planning device, computing apparatus, moving object, and storage medium

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