CN116069031A - Underground unmanned mine car path optimization method and system based on car body sweep model - Google Patents

Underground unmanned mine car path optimization method and system based on car body sweep model Download PDF

Info

Publication number
CN116069031A
CN116069031A CN202310042576.3A CN202310042576A CN116069031A CN 116069031 A CN116069031 A CN 116069031A CN 202310042576 A CN202310042576 A CN 202310042576A CN 116069031 A CN116069031 A CN 116069031A
Authority
CN
China
Prior art keywords
vehicle
sweep
vehicle body
path
model
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.)
Granted
Application number
CN202310042576.3A
Other languages
Chinese (zh)
Other versions
CN116069031B (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.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
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 Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202310042576.3A priority Critical patent/CN116069031B/en
Publication of CN116069031A publication Critical patent/CN116069031A/en
Application granted granted Critical
Publication of CN116069031B publication Critical patent/CN116069031B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • 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

Abstract

The invention discloses an underground unmanned mine car path optimization method and system based on a car body sweep model, wherein the method comprises the following steps: constructing a travelable region of an underground mine; acquiring sampling points based on a drivable area, and performing curve fitting based on a Bezier curve and a polynomial curve to construct a vehicle body sweep model; and (3) based on the vehicle body sweep model, iteratively optimizing the path points in the vehicle sweep area with the targets that the distances between the left and right sweep boundaries and the obstacle are equal and the curvature meets the minimum turning radius constraint until the optimal running path is obtained. According to the method, the vehicle body sweep model is built in real time according to the change of the control points of the approximate vehicle body model, the vehicle sweep area is accurately planned based on the direct calling algorithm of the vehicle body sweep model and the addition of constraint conditions, and the traffic force and the safety of the vehicle in the narrow beam space are ensured.

Description

Underground unmanned mine car path optimization method and system based on car body sweep model
Technical Field
The invention relates to the field of planning and optimizing an automatic driving path of an underground mining area narrow-beam space unmanned mine car, in particular to a method for constructing a car body sweep model and optimally planning a sweep path based on an optimization algorithm, and particularly relates to an underground unmanned mine car path optimizing method and system based on the car body sweep model.
Background
The automatic driving technology of the unmanned mine car in the underground mining area can greatly improve the traffic capacity of the automatic mine car in the narrow-beam space and the driving safety. The unmanned technology of the underground mining area is currently in a starting stage, and the rule-based decision model algorithm is complex, has low adaptability and reliability and the like, is a key problem, and relates to the technologies of map making, optimal path decision planning and the like of the complex underground mining area.
Aiming at the construction of an approximate model of an automatic unmanned mine car body in a narrow beam space, the current solution is that an underground mine car can plan a target path by a manual teaching method, but the manual teaching method relies on accurate measurement of the driving mileage of the underground mine car, when the measurement error accumulation of an odometer or the skid of a tire is serious, the actual tracking track of the underground mine car has larger track deviation compared with the manual teaching target path, and the safe and reliable driving of the underground mine car is influenced.
Disclosure of Invention
In order to overcome the defects in the prior art, the invention provides an underground unmanned mine car path optimization method and system based on a car body sweep model, which are used for solving at least one technical problem.
According to an aspect of the present disclosure, there is provided a method for optimizing a path of an underground unmanned mine car based on a car body sweep model, including:
constructing a travelable region of an underground mine;
acquiring sampling points based on a drivable area, and performing curve fitting based on a Bezier curve and a polynomial curve to construct a vehicle body sweep model;
and (3) based on the vehicle body sweep model, iteratively optimizing the path points in the vehicle sweep area with the targets that the distances between the left and right sweep boundaries and the obstacle are equal and the curvature meets the minimum turning radius constraint until the optimal running path is obtained.
According to the technical scheme, the vehicle body sweep model is built in real time according to the change of the approximate vehicle body model control points, the vehicle sweep area is accurately planned based on the vehicle body sweep model direct calling algorithm and the added constraint condition, and the traffic force and the safety of the vehicle in the narrow beam space are ensured.
Optionally, the method further comprises:
based on a vehicle body sweep model, taking the distance between a left sweep boundary and a right sweep boundary and an obstacle as a target and the curvature meeting the minimum turning radius constraint, and taking the vehicle dynamics constraint of the functional relation among the position, the speed, the acceleration and the external force which are required to be met by the vehicle into consideration to obtain an optimized sampling point;
and performing curve fitting again on the optimized sampling points based on the Bezier curve and the polynomial curve to obtain a more reasonable vehicle rear axle midpoint path curve, constructing an optimized vehicle body sweep model based on a Frenet coordinate system, and performing iterative optimization on the path points under the condition of a constraint function until an optimal running path is obtained.
According to the technical scheme, on the basis of a vehicle body sweep model, sampling points are continuously updated by taking equidistant left and right sweep boundaries and obstacles and path curvature constraint as targets, the updated sampling points are subjected to curve fitting again, and iterative optimization is carried out according to the operation, so that an optimal running path is obtained.
As a further technical solution, the method further includes:
acquiring a priori map, and carrying out coordinate transformation on the priori map;
based on the prior map after coordinate transformation, the matrix expression of the drivable area is obtained by using a grid map distance transformation method.
The technical scheme is to build a map capable of enabling the mine car to normally pass according to the complex terrain environment of the underground mining area, and prepare for planning the next path of the mine car.
As a further technical solution, the method further includes:
discrete sampling points in a drivable area are obtained;
based on the discrete sampling points, curve fitting is carried out through a path planning algorithm to obtain a vehicle rear axle midpoint path track;
considering that the coordinates of the vehicle body can exceed the starting point and the ending point, and prolonging the starting point and the ending point of the fitted vehicle rear axle path;
considering the geometric dimension and the curve length of the vehicle, calculating the coordinates of left and right boundary points of the vehicle body based on a six-degree polynomial parameter equation;
performing Frenet coordinate conversion on the calculated coordinates of the left and right boundary points of the vehicle body, and projecting the coordinates of the left and right boundary points of the vehicle under a Cartesian coordinate system onto an S axis and a D axis under the Frenet coordinate system;
and determining a sweeping boundary under the Frenet coordinate system, and forming a closed area according to the sweeping boundary.
According to the technical scheme, discrete sampling points are subjected to curve fitting through a Bezier curve and a polynomial curve to obtain a vehicle rear axle midpoint path planning point, then coordinates of a vehicle left boundary point and a vehicle right boundary point are calculated based on a polynomial curve parameter equation, and the construction of a sweep model is completed based on a Frenet coordinate system.
As a further technical solution, the method further includes:
counting from a vehicle starting boundary point under a Frenet coordinate system, and defining every M coordinate points as a sweep group;
the point with the largest vertical distance from the D axis in the sweep group is defined as a sweep point, and N vehicle sweep points are finally obtained respectively;
connecting left and right sweep points of the vehicle to obtain a sweep boundary;
considering a starting point tail and a finishing point head, connecting the sweeping boundaries to form a closed area.
Alternatively, the position coordinates, speed and acceleration of the initial and final states of the vehicle are substituted into polynomial curve parameter equations with known coefficients
Figure SMS_1
And adds one degree of freedom o of the vehicle body boundary (o is the trolley along the rail when calculating the coordinates of the left and right sweep boundary pointsIndependent variables of vehicle boundaries during trace motion).
The coordinate formula of the left boundary point and the right boundary point of the vehicle is as follows:
Figure SMS_2
wherein the method comprises the steps of
Figure SMS_5
L is vehicle width, o is one of the independent variables,>
Figure SMS_7
、
Figure SMS_9
for sweeping left boundary coordinate point value, +.>
Figure SMS_3
，/>
Figure SMS_8
To sweep the right boundary coordinate point value. />
Figure SMS_10
，/>
Figure SMS_11
Is polynomial->
Figure SMS_4
，/>
Figure SMS_6
And (3) deriving the formula of p.
Optionally, the method further comprises: based on the obtained coordinates of the left and right boundary points of the vehicle body, a Frenet coordinate system is established by taking the path point of the rear axle of the vehicle as a reference line, the position of the distance point of the vehicle is projected to the reference line on the Frenet coordinate system, and the mapping relation is established as follows:
Figure SMS_12
wherein the method comprises the steps of
Figure SMS_13
，/>
Figure SMS_14
For coordinates of left and right boundary points of the car body, +.>
Figure SMS_15
，/>
Figure SMS_16
And->
Figure SMS_17
，/>
Figure SMS_18
The distance along the vehicle rear axle path in the frenet coordinate system as a reference line and the normal distance perpendicular to the reference line, respectively.
As a further technical solution, the method further includes:
based on the vehicle body sweep model and the vehicle body geometry, an objective function for the equal distance between the left and right sweep boundaries and the obstacle is constructed as follows:
Figure SMS_19
wherein->
Figure SMS_20
For the vehicle rear axle midpoint coordinates and heading angle,
Figure SMS_21
a grid map matrix, a distance transformation matrix and a nearest neighbor matrix, respectively, < >>
Figure SMS_22
The method is an objective function composed of a plurality of independent variables such as a point coordinate, a course angle, map information and the like in a rear axle of the vehicle;
based on the objective function, adding a constraint function that constrains the objective to the curvature to meet the minimum turning radius is as follows:
Figure SMS_23
wherein A, B, C are each a constant matrix or array of constraint functions;
Figure SMS_24
respectively representing the increment of the point coordinate and the course angle of the rear axle of the vehicle;
updating the path points in the vehicle sweep area based on the objective function and the constraint function established by the vehicle body sweep model.
As a further technical solution, the method further includes:
based on prior map information, a grid coordinate system and a positioning coordinate system are constructed, and the positioning coordinate system and the decision grid coordinate system are mutually converted by utilizing a least square trust domain method.
The mathematical formula of the coordinate transformation problem is described as follows:
Figure SMS_25
where k is the scale factor (i.e., the ratio of the lengths of the same line in the two spatial coordinate systems), S is the rotation coefficient,
Figure SMS_26
，/>
Figure SMS_27
coordinate translation amounts, +.>
Figure SMS_28
，/>
Figure SMS_29
The coordinates of the grid coordinate system and the positioning coordinate system, respectively.
As a further technical solution, the method further includes:
based on the processed grid map, calculating the distance between each drivable area in the grid map and the nearest obstacle and the linear matrix index of the nearest obstacle by adopting a binarization image distance conversion method. Therefore, the distance between each point on the map and the nearest obstacle can be clearly determined through the prior point cloud data, and meanwhile, the specific coordinates of the obstacle can be obtained.
As a further technical solution, the method further includes:
three linear matrixes obtained by adopting a binarization image distance conversion method are respectively a grid map matrix, a distance conversion matrix and a nearest neighbor matrix, wherein '0' in the grid map matrix represents a passable area, and '1' is an unviewable area or an area marked as an obstacle; the '0' in the distance transformation matrix is an obstacle area, and other numbers of the matrix are the distance between the current point and the obstacle; the numbers in the nearest neighbor matrix represent the linear index of the current point from the nearest obstacle, respectively.
Optionally, the method further comprises: when the area of the map is larger, the grid map occupies a large memory space, and the grid map matrix can be converted into a sparse matrix to reduce the space occupied by the grid map.
According to an aspect of the present disclosure, there is provided an underground unmanned mine car path optimization system based on a car body sweep model for implementing the method, the system comprising:
the travelable region construction module is used for constructing a travelable region of the underground mining area;
the vehicle body sweep model construction module is used for acquiring sampling points based on the drivable area and carrying out curve fitting based on a Bezier curve and a polynomial curve so as to construct a vehicle body sweep model;
and the path optimization module is used for iteratively optimizing the path points in the vehicle sweeping area by taking the distance between the left and right sweeping boundaries and the obstacle as the target and the curvature meeting the minimum turning radius constraint based on the vehicle body sweeping model until the optimal running path is obtained.
Firstly, constructing a drivable region of an underground mining area, performing curve fitting on discrete sampling points through a Bezier curve and a polynomial curve to obtain a vehicle rear axle midpoint path planning point, calculating coordinates of left and right boundary points of a vehicle based on a polynomial curve parameter equation, and completing construction of a vehicle body sweep model based on a Frenet coordinate system; and finally, continuously updating sampling points on the basis of a vehicle body sweep model by taking equidistant left and right sweep boundaries and obstacles and path curvature constraint as targets, performing curve fitting again on the updated sampling points, and performing iterative optimization according to the operation to obtain an optimal running path.
Compared with the prior art, the invention has the beneficial effects that:
the invention provides a method for constructing a vehicle body sweeping model, which is different from the method for judging the relation between a vehicle and an obstacle by rectangular coverage of the vehicle body, aims to construct the display expression of a point driving path in a rear axle of the vehicle and a vehicle body sweeping boundary, not only improves the collision detection speed, but also can directly call an optimization algorithm to accurately plan a vehicle sweeping area due to the fact that the vehicle body sweeping model has a definitely quantized geometric boundary, and ensures the traffic capacity and safety of the vehicle in a narrow beam space.
The invention provides a method and a system for optimizing a path based on a vehicle body sweep model, wherein an objective function (the distance between a left sweep boundary and a right sweep boundary is equal to that between an obstacle) and a constraint function (the curvature meets the minimum turning half-month constraint) are established to update sampling points, the updated sampling points are used as data points for curve fitting in the next vehicle body sweep model construction, and the sampling points are continuously updated by the path optimizing method until an optimal running path is obtained.
Drawings
Fig. 1 is a schematic diagram of a prior art vehicle body rectangular method for determining a relationship between a vehicle and an obstacle.
Fig. 2 is a schematic diagram of an underground unmanned mine car path optimization method based on a car body sweep model according to an embodiment of the invention.
Fig. 3 is a schematic diagram of a distance transformation matrix according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of a grid map matrix according to an embodiment of the present invention.
Fig. 5 is a schematic diagram of a nearest neighbor matrix according to an embodiment of the invention.
Fig. 6 is a schematic diagram of a vehicle body sweep model according to an embodiment of the present invention.
The following description of the embodiments of the present invention will be made in detail and with reference to the accompanying drawings, wherein it is apparent that the embodiments described are only some, but not all embodiments of the present invention. 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 fall within the scope of the invention.
The invention provides an underground unmanned mine car path optimization method based on a car body sweep model. The method is different from the method for judging the relation between the vehicle and the obstacle by using rectangular coverage of the vehicle body (as shown in fig. 1), the vehicle body sweep model aims to construct the display expression of the middle point driving path of the rear axle of the vehicle and the vehicle body sweep boundary, so that the collision detection speed is improved, meanwhile, the vehicle body sweep model has a definitely quantized geometric boundary, and an optimization algorithm can be directly called to accurately plan the sweep area of the vehicle, so that the traffic capacity and the safety of the vehicle in a narrow space are ensured.
As shown in fig. 2, the method of the present invention comprises; constructing a travelable area and acquiring sampling points; constructing a vehicle body sweep model on a drivable area through fitting a curve; and on the basis of the vehicle body sweep model, carrying out iterative updating on the path points based on the objective function and the constraint function until an optimal path is obtained.
As an embodiment, the travelable region may be constructed as follows:
coordinate conversion is carried out based on prior map information;
and acquiring a matrix expression of the drivable area by adopting a grid map distance conversion method.
Optionally, firstly, constructing a grid coordinate system and a positioning coordinate system based on prior map information, and mutually converting the positioning coordinate system and a decision grid coordinate system by using a least square trust domain method. The mathematical formula of the coordinate transformation problem is described as follows:
Figure SMS_30
where k is the scale factor (i.e., the ratio of the lengths of the same line in the two spatial coordinate systems), S is the rotation coefficient,
Figure SMS_31
，/>
Figure SMS_32
coordinate translation amounts, +.>
Figure SMS_33
，/>
Figure SMS_34
The coordinates of the grid coordinate system and the positioning coordinate system, respectively.
Further, for the processed grid map, a binarization image distance conversion method is adopted to calculate the distance between each drivable area in the grid map and the nearest obstacle and the linear matrix index of the nearest obstacle, so that the distance between each point on the map and the nearest obstacle can be clearly determined through prior point cloud data, and meanwhile, specific coordinates of the obstacle can be obtained.
Optionally, the method further comprises: three linear matrixes obtained by adopting the binarization image distance conversion method are a grid map matrix, a distance transformation matrix and a nearest neighbor matrix respectively. As shown in fig. 3, a "0" in the distance transformation matrix is an obstacle region, and the other numbers of the matrix are distances from the point to the obstacle. As shown in fig. 4, "0" in the matrix of the grid map represents a passable area, and "1" is a non-passable area or marked as an obstacle-present area. As shown in fig. 5, the numbers in the nearest neighbor matrix represent the linear index of the point from the nearest obstacle, respectively.
Optionally, the method further comprises: when the area of the map is large, the grid map occupies a large memory space, and the grid map matrix can be converted into a sparse matrix to reduce the space occupied by the grid map.
As one embodiment, a vehicle body sweep model is constructed for an unmanned mine car in an underground mine based on a bezier curve and a polynomial curve by performing curve fitting on the basis of an acquired travelable region.
The specific embodiment comprises the following steps:
based on discrete sampling points, curve fitting is carried out through a path planning algorithm to obtain a vehicle rear axle midpoint path track;
considering that the coordinates of the vehicle body can exceed the starting point and the ending point, the fitted starting point and ending point of the rear axle path of the vehicle are required to be prolonged;
considering the geometric dimension and the curve length of the vehicle, calculating the coordinates of left and right boundary points of the vehicle body based on a six-degree polynomial parameter equation;
performing Frenet coordinate conversion on the calculated coordinates of the left and right boundary points of the vehicle body, and projecting the coordinates of the left and right boundary points of the vehicle under a Cartesian coordinate system onto an S axis and a D axis under the Frenet coordinate system;
under the Frenet coordinate system, counting is started from a vehicle starting boundary point, every M coordinate points are defined as a sweep group, the point of the maximum value of the vertical distance from the D axis to the distance point in the sweep group is defined as a sweep point, N vehicle sweep points are finally obtained respectively, and then the left and right sweep points of the vehicle are connected to obtain a sweep boundary.
Considering the starting parking space and the finishing parking space, the sweeping boundaries are connected to form a closed area as shown in fig. 6.
In this embodiment, the method for calculating the left and right coordinates of the vehicle body based on the midpoint path of the rear axle of the vehicle obtained by the sixth order polynomial parameter equation is as follows:
(1) A sixth order polynomial is first built based on the degree of freedom p of the curve (p-value between 0 and 1):
Figure SMS_35
(2) For a pair of
Figure SMS_36
And p in the parameter equation is derived to obtain:
Figure SMS_37
/>
the total of 14 parameters is that 12 parameters are known parameters, 2 parameters
Figure SMS_38
、/>
Figure SMS_39
For the parameter to be optimized, an optimal sweep path boundary is determined by determining an optimal value of the parameter to be optimized.
(3) Position coordinates, speed and acceleration of the initial state and the end state of the vehicle are substituted into the parameter equation, and an independent variable of a degree of freedom o of the vehicle body boundary (o is a moving point of the vehicle boundary along the track) is added when calculating the coordinates of the left and right sweep boundary points.
(4) The coordinate formula of the left boundary point and the right boundary point of the vehicle is as follows:
Figure SMS_40
wherein the method comprises the steps of
Figure SMS_41
L is vehicle width, o is one of the independent variables,>
Figure SMS_45
、
Figure SMS_48
for sweeping left boundary coordinate point value, +.>
Figure SMS_42
，/>
Figure SMS_44
To sweep the right boundary coordinate point value. />
Figure SMS_47
，
Figure SMS_49
Is polynomial->
Figure SMS_43
，/>
Figure SMS_46
And (3) deriving the formula of p.
According to the formula, the substituted actual parameters are calculated as follows:
substituting the actual parameters into the sixth degree polynomial:
Figure SMS_50
deriving the polynomials:
Figure SMS_51
based on the formula
Figure SMS_52
Substituting the actual value and will +.>
Figure SMS_53
Substituting the left boundary coordinate point into a calculation formula to calculate a left boundary coordinate point of the vehicle: />
Figure SMS_54
Substituting the numerical value to calculate a right boundary coordinate point of the vehicle:
Figure SMS_55
in this example, frenet coordinate transformation is realized based on the path track of the midpoint of the rear axle of the vehicle as a reference line, and the distance points in the Cartesian coordinate system are setLabel (C)
Figure SMS_56
Transfer Frenet coordinate->
Figure SMS_57
A Frenet coordinate system is established by taking a planned path at the midpoint of the rear axle of the vehicle as a reference line, taking the tangential direction of each point on the path as a transverse axis S and taking the normal direction of each point on the path at the midpoint of the rear axle of the vehicle as a longitudinal axis D.
The method for acquiring the left and right sweep lines based on the Frenet coordinate system comprises the following steps: and acquiring coordinates of path points in a relevant vehicle sweeping area based on a Frenet coordinate system, starting from a vehicle head initial boundary path point, selecting a point which is farthest from a reference line vehicle rear axis midpoint path in 40 distance points by a vertical distance as a sweeping point to an end point, and sequentially connecting the sweeping points to obtain a left sweeping line and a right sweeping line.
The optimization of the swept area in this example is based on a vehicle body sweep model, and the optimization method includes:
(1) Based on the vehicle body sweep model and the vehicle body geometry, an objective function for the equal distance between the left and right sweep boundaries and the obstacle is constructed as follows:
Figure SMS_58
wherein->
Figure SMS_59
For the vehicle rear axle midpoint coordinates and heading angle,
Figure SMS_60
a grid map matrix, a distance transformation matrix and a nearest neighbor matrix, respectively, < >>
Figure SMS_61
The method is an objective function composed of a plurality of independent variables such as a point coordinate, a course angle, map information and the like in a rear axle of the vehicle;
(2) Based on the objective function, adding a constraint function that constrains the objective to the curvature to meet the minimum turning radius is as follows:
Figure SMS_62
wherein A, B, C are each a constant matrix or array of constraint functions; />
Figure SMS_63
Respectively representing the increment of the point coordinate and the course angle of the rear axle of the vehicle;
(3) And updating the path points in the vehicle sweeping area on the basis of the objective function and the constraint function established by the sweeping model.
The invention also provides an underground unmanned mine car path optimization system based on a car body sweep model, which is used for realizing the method, and comprises the following steps:
the travelable region construction module is used for constructing a travelable region of the underground mining area;
the vehicle body sweep model construction module is used for acquiring sampling points based on the drivable area and carrying out curve fitting based on a Bezier curve and a polynomial curve so as to construct a vehicle body sweep model;
and the path optimization module is used for iteratively optimizing the path points in the vehicle sweeping area by taking the distance between the left and right sweeping boundaries and the obstacle as the target and the curvature meeting the minimum turning radius constraint based on the vehicle body sweeping model until the optimal running path is obtained.
The travelable region construction module is further used for constructing a grid coordinate system and a positioning coordinate system based on prior map information, and interconverting the positioning coordinate system and the decision grid coordinate system by utilizing a least square confidence region method.
The drivable region construction module is further used for calculating the distance between each drivable region and the nearest obstacle in the grid map and the linear matrix index of the nearest obstacle based on the processed grid map by adopting a binarization image distance conversion method, so that the distance between each point on the map and the nearest obstacle can be clearly determined through prior point cloud data, and meanwhile, specific coordinates of the obstacle can be obtained.
The driving area construction module is further used for obtaining three linear matrixes, namely a grid map matrix, a distance transformation matrix and a nearest neighbor matrix (nearest pixel diagram) by adopting a binarization image distance transformation method, wherein a 0 table in the grid map matrix can pass an area, and a 1 is an unviewable area or an area marked as an obstacle; the '0' in the distance transformation matrix is an obstacle area, and the other numbers of the matrix are the distance between the point and the obstacle; the numbers in the nearest neighbor matrix represent the linear index of the point from the nearest obstacle, respectively.
The vehicle body sweep model construction module is further used for carrying out curve fitting through a path planning algorithm based on discrete sampling points to obtain a vehicle rear axle midpoint path track; considering that the coordinates of the vehicle body can exceed the starting point and the ending point, and prolonging the starting point and the ending point of the fitted vehicle rear axle path; considering the geometric dimension and the curve length of the vehicle, calculating the coordinates of the left and right points of the vehicle body through a polynomial curve parameter equation based on the path track and the course angle of the midpoint of the rear axle of the vehicle; performing Frenet coordinate conversion on the calculated coordinates of the left and right points of the vehicle body, and projecting the coordinates of the left and right boundary points of the vehicle under the Cartesian coordinate system onto an S axis and a D axis under the Frenet coordinate system; under the Frenet coordinate system, counting from a vehicle initial boundary point, defining every M coordinate points as a sweep group, defining the point of the maximum value of the vertical distance from the D axis to the distance point in the sweep group as a sweep point, finally respectively obtaining N vehicle sweep points, and connecting the left and right vehicle sweep points to obtain a sweep boundary; considering a starting parking place and a finishing parking place, connecting the sweeping boundaries to form a closed area.
The vehicle body sweep model construction module is also used for substituting the position coordinates, the speed and the acceleration of the initial state and the end state of the trolley into a parameter equation
Figure SMS_64
And adding a parameter to be optimized of a vehicle body boundary degree of freedom o (o is an independent variable of a vehicle boundary when the trolley moves along the track) when calculating coordinates of left and right sweep boundary points.
The vehicle body sweep model construction module is also used for constructing a Frenet coordinate system by taking the vehicle rear axle path point as a reference line based on the obtained coordinates of the left and right boundary points of the vehicle body, and projecting the position of the vehicle distance point to the reference line on the Frenet coordinate system to construct a mapping relation.
The path optimization module is further used for continuously updating sampling points on the basis of the vehicle body sweep model by taking the equidistant between the left and right sweep boundaries and the obstacle and the constraint of the path curvature as targets, performing curve fitting again on the updated sampling points, and performing iterative optimization according to the operations to obtain the optimal running path.
In the description of the present specification, reference to the terms "one embodiment," "certain embodiments," "illustrative embodiments," "examples," "specific examples," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiments or examples. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
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 or all of the technical features thereof can be replaced with equivalents; these modifications or substitutions do not depart from the essence of the corresponding technical solutions from the technical solutions of the embodiments of the present invention.

Claims (9)

1. The underground unmanned mine car path optimization method based on the car body sweep model is characterized by comprising the following steps of:
constructing a travelable region of an underground mine;
acquiring sampling points based on a drivable area, and performing curve fitting based on a Bezier curve and a polynomial curve to construct a vehicle body sweep model;
and (3) based on the vehicle body sweep model, iteratively optimizing the path points in the vehicle sweep area with the targets that the distances between the left and right sweep boundaries and the obstacle are equal and the curvature meets the minimum turning radius constraint until the optimal running path is obtained.
2. The method of optimizing a path of an underground unmanned aerial vehicle based on a vehicle body sweep model of claim 1, further comprising:
acquiring a priori map, and carrying out coordinate transformation on the priori map;
based on the prior map after coordinate transformation, the matrix expression of the drivable area is obtained by using a grid map distance transformation method.
3. The method of optimizing a path of an underground unmanned aerial vehicle based on a vehicle body sweep model of claim 1, further comprising:
discrete sampling points in a drivable area are obtained;
based on the discrete sampling points, curve fitting is carried out through a path planning algorithm to obtain a vehicle rear axle midpoint path track;
considering that the coordinates of the vehicle body can exceed the starting point and the ending point, and prolonging the starting point and the ending point of the fitted vehicle rear axle path;
considering the geometric dimension and the curve length of the vehicle, calculating the coordinates of left and right boundary points of the vehicle body based on a six-degree polynomial parameter equation;
performing Frenet coordinate conversion on the calculated coordinates of the left and right boundary points of the vehicle body, and projecting the coordinates of the left and right boundary points of the vehicle under a Cartesian coordinate system onto an S axis and a D axis under the Frenet coordinate system;
and determining a sweeping boundary under the Frenet coordinate system, and forming a closed area according to the sweeping boundary.
4. A method of optimizing a path of an underground unmanned aerial vehicle based on a vehicle body sweep model according to claim 3, wherein the method further comprises:
counting from a vehicle starting boundary point under a Frenet coordinate system, and defining every M coordinate points as a sweep group;
the point with the largest vertical distance from the D axis in the sweep group is defined as a sweep point, and N vehicle sweep points are finally obtained respectively;
connecting left and right sweep points of the vehicle to obtain a sweep boundary;
considering a starting point tail and a finishing point head, connecting the sweeping boundaries to form a closed area.
5. A method of optimizing a path of an underground unmanned aerial vehicle based on a vehicle body sweep model according to claim 3, wherein the method further comprises:
based on the vehicle body sweep model and the vehicle body geometry, an objective function for the equal distance between the left and right sweep boundaries and the obstacle is constructed as follows:
Figure QLYQS_1
wherein->
Figure QLYQS_2
For the coordinates of the midpoint and heading angle of the rear axle of the vehicle,/->
Figure QLYQS_3
Respectively a grid map matrix, a distance transformation matrix and a nearest neighbor matrix;
based on the objective function, adding a constraint function that constrains the objective to the curvature to meet the minimum turning radius is as follows:
Figure QLYQS_4
wherein A, B, C are each a constant matrix or array of constraint functions; />
Figure QLYQS_5
Respectively representing the increment of the point coordinate and the course angle of the rear axle of the vehicle;
updating the path points in the vehicle sweep area based on the objective function and the constraint function established by the vehicle body sweep model.
6. The method of optimizing a path of an underground unmanned aerial vehicle based on a vehicle body sweep model of claim 5, further comprising:
based on prior map information, a grid coordinate system and a positioning coordinate system are constructed, and the positioning coordinate system and the decision grid coordinate system are mutually converted by utilizing a least square trust domain method.
7. The method of optimizing a path of an underground unmanned aerial vehicle based on a vehicle body sweep model of claim 6, further comprising:
based on the processed grid map, calculating the distance between each drivable area in the grid map and the nearest obstacle and the linear matrix index of the nearest obstacle by adopting a binarization image distance conversion method.
8. The method of optimizing a path of an underground unmanned aerial vehicle based on a vehicle body sweep model of claim 7, further comprising:
three linear matrixes obtained by adopting a binarization image distance conversion method are respectively a grid map matrix, a distance conversion matrix and a nearest neighbor matrix, wherein '0' in the grid map matrix represents a passable area, and '1' is an unviewable area or an area marked as an obstacle; the '0' in the distance transformation matrix is an obstacle area, and other numbers of the matrix are the distance between the current point and the obstacle; the numbers in the nearest neighbor matrix represent the linear index of the current point from the nearest obstacle, respectively.
9. An underground unmanned mine car path optimization system based on a car body sweep model for implementing the method of any one of claims 1-8, the system comprising:
the travelable region construction module is used for constructing a travelable region of the underground mining area;
the vehicle body sweep model construction module is used for acquiring sampling points based on the drivable area and carrying out curve fitting based on a Bezier curve and a polynomial curve so as to construct a vehicle body sweep model;
and the path optimization module is used for iteratively optimizing the path points in the vehicle sweeping area by taking the distance between the left and right sweeping boundaries and the obstacle as the target and the curvature meeting the minimum turning radius constraint based on the vehicle body sweeping model until the optimal running path is obtained.
CN202310042576.3A 2023-01-28 2023-01-28 Underground unmanned mine car path optimization method and system based on car body sweep model Active CN116069031B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310042576.3A CN116069031B (en) 2023-01-28 2023-01-28 Underground unmanned mine car path optimization method and system based on car body sweep model

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310042576.3A CN116069031B (en) 2023-01-28 2023-01-28 Underground unmanned mine car path optimization method and system based on car body sweep model

Publications (2)

Publication Number Publication Date
CN116069031A true CN116069031A (en) 2023-05-05
CN116069031B CN116069031B (en) 2023-08-11

Family

ID=86179710

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310042576.3A Active CN116069031B (en) 2023-01-28 2023-01-28 Underground unmanned mine car path optimization method and system based on car body sweep model

Country Status (1)

Country Link
CN (1) CN116069031B (en)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109059942A (en) * 2018-08-22 2018-12-21 中国矿业大学 A kind of high-precision underground navigation map building system and construction method
US20190235516A1 (en) * 2018-01-26 2019-08-01 Baidu Usa Llc Path and speed optimization fallback mechanism for autonomous vehicles
CN110352393A (en) * 2017-02-13 2019-10-18 淡水河谷公司 More landform investigate robot device and its configuration and bootstrap technique
US20200182627A1 (en) * 2018-12-11 2020-06-11 Here Global B.V. Segmented path coordinate system
US20210086780A1 (en) * 2019-09-24 2021-03-25 Baidu Usa Llc Variable boundary estimation for path planning for autonomous driving vehicles
WO2021142793A1 (en) * 2020-01-17 2021-07-22 华为技术有限公司 Path planning method and path planning apparatus
CN114995398A (en) * 2022-05-16 2022-09-02 中国第一汽车股份有限公司 Path generation method, path generation device, storage medium, processor and electronic device
CN115079695A (en) * 2022-06-14 2022-09-20 合众新能源汽车有限公司 Path planning method and device and computer readable storage medium
WO2022193584A1 (en) * 2021-03-15 2022-09-22 西安交通大学 Multi-scenario-oriented autonomous driving planning method and system
CN115730756A (en) * 2022-11-30 2023-03-03 吉林大学 Staged automatic driving vehicle track planning method

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110352393A (en) * 2017-02-13 2019-10-18 淡水河谷公司 More landform investigate robot device and its configuration and bootstrap technique
US20190235516A1 (en) * 2018-01-26 2019-08-01 Baidu Usa Llc Path and speed optimization fallback mechanism for autonomous vehicles
CN109059942A (en) * 2018-08-22 2018-12-21 中国矿业大学 A kind of high-precision underground navigation map building system and construction method
US20200182627A1 (en) * 2018-12-11 2020-06-11 Here Global B.V. Segmented path coordinate system
US20210086780A1 (en) * 2019-09-24 2021-03-25 Baidu Usa Llc Variable boundary estimation for path planning for autonomous driving vehicles
WO2021142793A1 (en) * 2020-01-17 2021-07-22 华为技术有限公司 Path planning method and path planning apparatus
WO2022193584A1 (en) * 2021-03-15 2022-09-22 西安交通大学 Multi-scenario-oriented autonomous driving planning method and system
CN114995398A (en) * 2022-05-16 2022-09-02 中国第一汽车股份有限公司 Path generation method, path generation device, storage medium, processor and electronic device
CN115079695A (en) * 2022-06-14 2022-09-20 合众新能源汽车有限公司 Path planning method and device and computer readable storage medium
CN115730756A (en) * 2022-11-30 2023-03-03 吉林大学 Staged automatic driving vehicle track planning method

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
S. THRUN 等: "Autonomous exploration and mapping of abandoned mines", 《IEEE ROBOTICS & AUTOMATION MAGAZINE》, vol. 11, no. 4, pages 79 - 91, XP011123968, DOI: 10.1109/MRA.2004.1371606 *
刘厚强 等: "道路实体扫掠路径优化算法研究", 《高速铁路技术》, vol. 7, no. 5, pages 8 - 10 *

Also Published As

Publication number Publication date
CN116069031B (en) 2023-08-11

Similar Documents

Publication Publication Date Title
EP3805703B1 (en) Movement track reconstruction method and device, storage medium, and electronic device
CN112673234B (en) Path planning method and path planning device
CN112270306B (en) Unmanned vehicle track prediction and navigation method based on topological road network
CN109416256B (en) Travel lane estimation system
CN114005280B (en) Vehicle track prediction method based on uncertainty estimation
CN110243380B (en) Map matching method based on multi-sensor data and angle feature recognition
Zheng et al. RRT based path planning for autonomous parking of vehicle
KR101585504B1 (en) Method and apparatus for generating pathe of autonomous vehicle
CN111006667B (en) Automatic driving track generation system under high-speed scene
Song et al. Laser‐based SLAM automatic parallel parking path planning and tracking for passenger vehicle
CN111066064A (en) Grid occupancy mapping using error range distribution
CN111137298B (en) Vehicle automatic driving method, device, system and storage medium
CN116300973B (en) Autonomous obstacle avoidance method for unmanned mine car in complex weather
CN116337045A (en) High-speed map building navigation method based on karto and teb
CN113654556A (en) Local path planning method, medium and equipment based on improved EM algorithm
CN116576857A (en) Multi-obstacle prediction navigation obstacle avoidance method based on single-line laser radar
CN116069037A (en) Unmanned vehicle formation track planning control method, device, equipment and storage medium
CN114578834A (en) Target layered double-perception domain-based reinforcement learning unmanned vehicle path planning method
CN116069031B (en) Underground unmanned mine car path optimization method and system based on car body sweep model
CN113515111B (en) Vehicle obstacle avoidance path planning method and device
CN211427151U (en) Automatic guide system applied to unmanned freight vehicle in closed field
CN115877853A (en) Intelligent storage flow path planning system and method
CN115615449A (en) Path planning method, device, equipment and storage medium
CN115855095A (en) Autonomous navigation method and device and electronic equipment
CN114460933A (en) Mobile robot local path planning algorithm for dynamic environment

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