CN113031583B - Obstacle avoidance method for structured road - Google Patents
Obstacle avoidance method for structured road Download PDFInfo
- Publication number
- CN113031583B CN113031583B CN202010174378.9A CN202010174378A CN113031583B CN 113031583 B CN113031583 B CN 113031583B CN 202010174378 A CN202010174378 A CN 202010174378A CN 113031583 B CN113031583 B CN 113031583B
- Authority
- CN
- China
- Prior art keywords
- point
- current
- sampling
- motion
- obstacle avoidance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 151
- 238000005070 sampling Methods 0.000 claims abstract description 196
- 206010048669 Terminal state Diseases 0.000 claims abstract description 4
- 238000013507 mapping Methods 0.000 claims description 117
- 239000013598 vector Substances 0.000 claims description 75
- 238000001514 detection method Methods 0.000 claims description 43
- 230000014509 gene expression Effects 0.000 claims description 34
- 230000000007 visual effect Effects 0.000 claims description 27
- 238000004364 calculation method Methods 0.000 claims description 26
- 238000012952 Resampling Methods 0.000 claims description 13
- 230000004888 barrier function Effects 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000004422 calculation algorithm Methods 0.000 description 27
- 238000005065 mining Methods 0.000 description 22
- 238000010586 diagram Methods 0.000 description 15
- 238000004088 simulation Methods 0.000 description 7
- 238000010276 construction Methods 0.000 description 5
- 238000011156 evaluation Methods 0.000 description 4
- 238000007689 inspection Methods 0.000 description 4
- 238000005457 optimization Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 238000013473 artificial intelligence Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000005802 health problem Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000004630 mental health Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 238000011144 upstream manufacturing Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
-
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
-
- 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
- G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a structured road obstacle avoidance method, which comprises the following steps: acquiring the vehicle pose of a vehicle in real-time running, the state information of obstacles around a running environment and reference path information, and based on the state space information, performing state space sampling on visible points of the obstacles in front of the current vehicle to determine the pose of each sampling point; respectively connecting the pose of the current vehicle and the pose of each sampling point by taking the pose of each sampling point as a terminal state and the pose of the current vehicle as an initial state to obtain a motion element of the current vehicle relative to each sampling point; and constructing a structured road cost function comprising path smoothness, collision risk occurrence probability and reference line path following property, selecting an optimal motion element by using the cost function, and determining the optimal motion element as an obstacle avoidance path aiming at the current obstacle. The invention realizes autonomous obstacle avoidance of the unmanned mine card in the driving process of the structured road, improves the intelligent degree of the mine and realizes autonomous safe operation of the mine card.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to an obstacle avoidance method for an unmanned mine block in the driving of a structured road.
Background
Currently, mine cars are one of the most used construction machines in the mining of surface mines. In the material transportation process, the mine card is the main operation equipment. The mine working environment is very severe, so that the physical and mental health of mine workers is seriously harmed, and the labor intensity is high when the engineering vehicle is circularly driven by a driver. During the work of the driver, generally speaking, the driving route is also relatively fixed, and the driver always performs repeated labor. Therefore, with the increasing research heat of artificial intelligence and automatic driving, the call for realizing unmanned mines is also higher and higher.
In the unmanned surface mine, the unmanned mine card also becomes the key engineering mechanical equipment of the current research. However, in the existing obstacle avoidance methods for the unmanned mine card in the driving process of the structured road, most methods only give an approximate obstacle avoidance direction, do not give specific obstacle avoidance track points in a final obstacle avoidance path, do not perform collision inspection, and are not suitable for the structured road.
Since the structured road is narrow, in order to provide the obstacle avoidance method for the road, a track point sequence for obstacle avoidance should be provided, and whether the track meets kinematics, dynamics constraints and obstacle avoidance constraints needs to be calculated, which is obviously absent in the prior art.
Disclosure of Invention
In order to solve the technical problem, an embodiment of the present invention provides a structured road obstacle avoidance method, where the method includes: the method comprises the steps of sampling the pose, acquiring the vehicle pose of a vehicle in real-time running, obstacle state information around a running environment and reference path information, and based on the information, performing state space sampling on visible points of an obstacle in front of the current vehicle to determine the pose of each sampling point; generating a motion element, taking the pose of each sampling point as an end point state and the pose of the current vehicle as an initial state, and respectively connecting the pose of the current vehicle and the pose of each sampling point to obtain the motion element of the current vehicle relative to each sampling point, wherein the motion element comprises the pose of each track point in the motion process; the method comprises the steps of obstacle avoidance line generation, construction of a structured road cost function comprising path smoothness, collision risk occurrence probability and reference line path following performance, selection of an optimal motion element by utilizing the cost function, and determination of the optimal motion element as an obstacle avoidance path for a current obstacle.
Preferably, the method further comprises: the method comprises the following steps of a path detection step, curvature constraint detection is carried out on the obstacle avoidance path, collision constraint detection is carried out on each track point in the obstacle avoidance path, and if any one is unqualified, the pose sampling step is returned to for resampling; otherwise, outputting the current obstacle avoidance path.
Preferably, in the collision constraint detecting process, the method includes: establishing a two-dimensional vehicle body model aiming at a current vehicle, wherein the two-dimensional vehicle body model comprises a polygon approximate to the current vehicle, a circumcircle of the polygon and a plurality of enveloping circles which are overlapped and cover the polygon along the longitudinal axis direction of the current vehicle, and the radius of each enveloping circle is the same; utilize two-dimensional automobile body model goes current vehicle every track point in keeping away the barrier route simulates to confirm every track point department the circumscribed circle with the position of a plurality of envelope circles, based on this, to every track point detects in proper order in keeping away the barrier route two-dimensional automobile body model with whether the place ahead barrier of current vehicle bumps, if take place, then marks the track point that bumps.
Preferably, the pose sampling step includes: selecting the visual point with barrier-free relation between the current barrier and the current vehicle according to the barrier state information; selecting a mapping point having a minimum distance relationship with the visual point from among a plurality of path discrete points constituting the reference path information; along the direction of a mapping point normal vector, carrying out state space sampling on each mapping point according to a preset sampling step length; and calculating the course angle and the curvature of each sampling point according to the course angle and the curvature of each mapping point, and determining the pose information of each sampling point.
Preferably, when a first side sampling point and a second side sampling point corresponding to the mapping point and axially symmetric to the tangential vector direction of the mapping point are collected at the same time, the coordinates of the pair of sampling points are determined by using the following expression:
[x n ,y n ]·τ＝0
||X n -X p ||＝c*λ
in the formula, x n 、y n Respectively representing the coordinate values of the sampling points, tau representing the normal vector of the current mapping point, X p Representing the coordinates of the current mapping point, λ represents the sampling step length, c represents the sampling order, and further, calculating the curvatures of the first side sample point and the second side sample point using the following expressions:
wherein, κ p Representing the curvature of the current mapping point.
Preferably, when κ p Is positive, κ n1 Represents the curvature of the first side sample point, κ n2 Representing a curvature of the second side sample point; when k is p When it is negative, κ n1 Represents the curvature of the second side sampling point, k n2 Representing the curvature of the first side sample point.
Preferably, the distribution positions of the first side sampling point and the second side sampling point are determined according to the positive and negative states of the wheel rotation angle of the current vehicle at the mapping point, wherein when the wheel rotation angle is positive, the first side sampling point is positioned on the left side of the direction of the mapping point tangent vector, and the second side sampling point is positioned on the right side of the direction of the mapping point tangent vector; when the wheel rotation angle is negative, the first side sampling point is located on the right side of the reflection point tangent vector direction, and the second side sampling point is located on the left side of the reflection point tangent vector direction.
Preferably, the motion primitive generating step includes: establishing a motion element solving constraint equation aiming at each sampling point by utilizing a polynomial spiral curve, wherein the motion element solving equation comprises a curvature equation, a course angle equation and a motion position equation; determining an unknown solution vector of the equation according to the motion primitive solution constraint equation, constructing a motion state vector representing each track point in the motion process in the current motion primitive solving result, and further establishing a motion state equation related to the unknown solution vector by taking a current sampling point as an end point state; setting initial conditions and termination conditions of the motion state equation, and solving the motion state equation by adopting a Jacobian matrix recursion mode by using preset disturbance vectors to obtain the motion element formed by the solved unknown solution vector.
Preferably, in the obstacle avoidance line generating step, the cost function is expressed by the following expression:
f＝w smooth f smooth +w risk f risk +w ref f ref
where f denotes the generation for the current motion primitiveValue f smooth Representing a path smoothness cost value, f risk Representing the cost of collision risk, f ref Representing a reference line following cost value, w smooth 、w risk 、w ref And respectively representing the weight value of the path smoothness cost item, the weight value of the collision risk cost item and the weight value of the reference line following cost item.
Preferably, according to the curvature of each track point in the current motion primitive, calculating the curvature square integral of the current motion primitive, and taking the calculation result as the path smoothness cost value; determining the distance from each track point to the current obstacle and determining the minimum distance according to the coordinate of each track point in the current motion primitive, and then obtaining the collision risk cost value by utilizing the preset first disturbance quantity and the minimum distance; and determining a mapping point corresponding to each track point in the current reference route according to the coordinates of each track point in the current motion primitive, and calculating the mean value of the square deviation of the motion primitive and the current reference route based on the mapping points to obtain the reference route following cost value.
Compared with the prior art, one or more embodiments in the above scheme can have the following advantages or beneficial effects:
the invention provides a structured road obstacle avoidance method. The method realizes an important part of the unmanned mine, and can bring the following benefits by realizing autonomous obstacle avoidance of the unmanned mine in the driving process of the structured road: the workload of the driver can be reduced; the number of the drivers can be reduced, so that the labor cost of mine development is reduced; for drivers, the health problem caused by mine environment is reduced; the intelligent degree of the mine is improved, and the autonomous safe operation of the mine card is realized; the mining efficiency of the mine is improved to a certain extent, and the method has great practical application value.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. The objectives and other advantages of the invention will be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention and not to limit the invention. In the drawings:
fig. 1 is a schematic view of an unmanned mine card in a structured road obstacle avoidance method according to an embodiment of the present application.
Fig. 2 is a step diagram of a structured road obstacle avoidance method according to an embodiment of the present application.
Fig. 3 is a schematic flow chart of an obstacle avoidance line generation process in the structured road obstacle avoidance method according to the embodiment of the present application.
Fig. 4 is a schematic diagram illustrating a principle of a process of selecting an obstacle visible point in the structured road obstacle avoidance method according to the embodiment of the present application.
Fig. 5 is a schematic diagram illustrating a principle of a process of converting a visible point into a mapping point in the structured road obstacle avoidance method according to the embodiment of the present application.
Fig. 6 is a schematic diagram illustrating a plurality of path discrete points in reference path information in the structured road obstacle avoidance method according to the embodiment of the present application.
Fig. 7 is a schematic diagram illustrating a principle of a curvature calculation process of a sampling point in the structured road obstacle avoidance method according to the embodiment of the present application.
Fig. 8 is a schematic diagram illustrating a principle of a motion primitive generation step in the structured road obstacle avoidance method according to the embodiment of the present application.
Fig. 9 is a flowchart of a path detection step in the structured road obstacle avoidance method according to the embodiment of the present application.
Fig. 10 is a schematic illustration showing a two-dimensional vehicle body model in the structured road obstacle avoidance method according to the embodiment of the application.
Fig. 11 is a schematic principle diagram of a simulation step performed on each track point of a current vehicle traveling to an obstacle avoidance path by using a two-dimensional vehicle body model in the structured road obstacle avoidance method according to the embodiment of the present application.
Fig. 12 is a specific flowchart of a structured road obstacle avoidance method according to an embodiment of the present application.
Detailed Description
The following detailed description of the embodiments of the present invention will be provided with reference to the drawings and examples, so that how to apply the technical means to solve the technical problems and achieve the technical effects can be fully understood and implemented. It should be noted that, as long as there is no conflict, the embodiments and the features in the embodiments of the present invention may be combined with each other, and the technical solutions formed are within the scope of the present invention.
Currently, mine cars are one of the most used construction machines in the mining of surface mines. In the material transportation process, the mine card is the main operation equipment. The mine working environment is very severe, so that the physical and mental health of mine workers is seriously harmed, and the labor intensity is high when the engineering vehicle is circularly driven by a driver. In the working process of a driver, generally speaking, the driving route is also relatively fixed, and the driver always performs repeated labor. Therefore, with the increasing research heat of artificial intelligence and automatic driving, the call for realizing unmanned mines is also higher and higher.
In the unmanned surface mining, the unmanned mine card also becomes the key engineering mechanical equipment of the current research. However, in the existing obstacle avoidance methods for the unmanned mine card in the driving process of the structured road, most methods only give an approximate obstacle avoidance direction, do not give specific obstacle avoidance track points in a final obstacle avoidance path, do not perform collision inspection, and are not suitable for the structured road.
Since a structured road is narrow, in order to provide an obstacle avoidance method for such a road, a track point sequence for obstacle avoidance should be provided, and it is necessary to calculate whether a track meets kinematics, dynamics constraints, and obstacle avoidance constraints, which is obviously absent in the prior art.
In order to solve the technical problems, the invention analyzes a kinematics model, an obstacle model and a driving environment model of the unmanned mine card, and provides an autonomous obstacle avoidance method of the unmanned mine card driving in a structured road when the unmanned mine card faces surrounding obstacles, no matter static obstacles or dynamic obstacles, and meanwhile, an obstacle avoidance module algorithm sends a track path curve containing a pose sequence to a control module in an automatic driving system of the unmanned mine card. The curve is utilized to improve the smoothness and safety of an output track of an obstacle avoidance algorithm of the existing automatic driving system, improve the obstacle avoidance capability of the unmanned mine card on the narrow terrain on the structured road, optimize the existing unmanned mine card automatic driving system, improve the automatic driving grade of the unmanned mine card and expand the scene where the unmanned mine card can drive.
Fig. 1 is a schematic view of an unmanned mine card in the structured road obstacle avoidance method according to the embodiment of the application. The unmanned mine card firstly acquires an off-line high-precision map, and acquires the state information of the mine card and the environment state information by acquiring the sensor information on line. The hardware layer comprises an XDE110 type mine card, and an ash point industrial camera, a single-line laser radar, a long-distance millimeter wave radar, a differential GPS antenna, a 16-line laser radar, an exchanger, an industrial personal computer, inertial navigation, a CAN bus and the like are arranged in the mine card. In addition, the unmanned mine card also comprises a programmable controller which is used for receiving driving control signals of an accelerator, a brake, a steering and the like sent by an upper industrial personal computer and sending control instructions to a bottom steering controller and a driving system controller, so that the driving speed, the accelerator pedal stroke, the brake pedal stroke and the turning angle direction of the vehicle meet the requirements of the driving control signals of the upper layer, and the control instructions of the upper layer are realized. The inertial navigation system, the radar system, the camera system, the industrial personal computer and the like are connected to the switch and communicate through the Ethernet, and the programmable controller communicates with the industrial personal computer, the underlying controller and the like through a CAN bus.
The invention discloses an obstacle avoidance algorithm aiming at an unmanned mine card autonomous driving process in a structured road. The obstacle avoidance algorithm mainly comprises five sub-processes, which are respectively as follows: designing a sampling rule in a structured road, and providing a solution equation of a sampling pose; selecting a proper motion element, connecting sampling poses, and satisfying kinematic constraint; by utilizing a cost function suitable for the evaluation of the structured road line, the generated obstacle avoidance track meets kinematics and obstacle avoidance constraints, has better smoothness and more ideal followability to road reference, namely the generated track can be kept in the center of the road to run as far as possible under the condition of meeting various constraints; an algorithm for rapidly detecting the obstacle avoidance track is designed, the anti-collision performance and the curvature constraint performance of the track are rapidly checked while the obstacle avoidance track is generated, once the generated track is found not to meet the strong constraint performance of the obstacle avoidance track detection, a resampling stage is started until the sampling frequency reaches the maximum value, iteration is stopped, and a conclusion that no feasible solution exists is output.
The basic process for realizing the real-time obstacle avoidance function of the unmanned mine card is as follows: 1) When a vehicle is started, an off-line high-precision map is automatically loaded, and in the driving process, an obstacle avoidance module (a module applicable to the structured road obstacle avoidance method) can obtain global path information given by a global planning module in a mine card and serve as reference path information of the obstacle avoidance module; 2) The current pose of the unmanned mine card is sent to the industrial personal computer through the differential GPS antenna and inertial navigation, a positioning module of an operating system in the industrial personal computer receives coordinate information, and positioning data and corresponding pose information under a geographic coordinate system of the mine card are issued. The state information of the obstacle is obtained by the laser radar, and the state information of a short time domain in the future of the obstacle (namely the obstacle state information) is sent out by the prediction module through processing. The decision-making module obtains positioning information, high-precision map information and decision-making information, and after judgment, the module can give out an approximate vehicle behavior mode instruction, such as instructions for parking, slow driving, lane changing and the like. 3) The obstacle avoidance module acquires obstacle state information provided by the upstream prediction module, positioning information provided by the decision module and reference path information provided by the global planning module, calculates an obstacle avoidance track, sends the track to the path tracking module, and completes tracking and driving of the track by the path tracking module to complete the whole obstacle avoidance behavior.
Fig. 2 is a step diagram of a structured road obstacle avoidance method according to an embodiment of the present application. As shown in fig. 2, an obstacle avoidance algorithm (a structured road obstacle avoidance method) provided in the embodiment of the present invention at least includes the following steps. Firstly, step S210 obtains vehicle pose, obstacle state information around a driving environment, and reference path information of a vehicle during real-time driving on a structured road, and performs state space sampling on visible points of an obstacle in front of the current vehicle according to the real-time vehicle pose, the obstacle state information, and the reference path information, thereby determining the pose of each sampling point. Structured roads refer to roads that contain boundary constraints, such as: the driving road comprises strong constraints such as road edges, lane lines and walls. In contrast, a driving environment without such strong constraints is generally called a free space environment. The pose is a vector containing position coordinates, heading angle, and curvature. When the unmanned mine card runs on the structured road, the positioning module obtains accurate vehicle positioning data in real time, obtains course angle information, and calculates curvature data in real time based on the course angle information, so that corresponding position and orientation information is obtained. In addition, the obstacle state information obtained in the embodiment of the present invention is the state information of the obstacle in the current vehicle driving environment after a future preset time period, which is sent by the prediction module after the obstacle point cloud data from the laser radar is obtained and through prediction processing. When the obstacle avoidance module executes the obstacle avoidance algorithm once according to the preset obstacle avoidance algorithm time interval period, the obstacles around the current vehicle driving environment can be approximately regarded as model data which keeps a static state in an obstacle avoidance algorithm execution period, and therefore stable obstacle information can be conveniently obtained when an obstacle avoidance track aiming at the period is generated. In the embodiment of the invention, the execution frequency of the obstacle avoidance algorithm is preferably 5Hz, or 10Hz, or 20Hz, and the like. Therefore, the obstacle avoidance algorithm can generate an obstacle avoidance track suitable for the current driving environment in each period, and the obstacle avoidance algorithm cannot occupy too many system resources due to too high execution frequency, so that the automatic driving decision capability of the unmanned mine card is reduced.
In addition, the reference path information is basic reference path data planned by the global planning module in the mine card according to the obtained high-precision map about the driving environment, and the reference path information is road point sequence data formed by discrete reference line points (path discrete points) and is stored in a kd tree pre-constructed by the obstacle avoidance module so as to be inquired at any time.
In an actual application process, after the pose information of the visual points of the obstacle at the preset distance in front of the current vehicle is obtained, state space sampling processing is carried out on each visual point, and therefore the pose information of each sampling point is determined. The state space sampling process is a process of discretizing a state space (vehicle travel environment space) to sample the poses of travelable points near the obstacle visible points.
Then, in step S220, (motion element generating step) takes each sampling point as an end point state and the current vehicle pose as an initial state, and respectively connects the current vehicle pose with the pose of each sampling point to obtain a motion element of the current vehicle relative to each sampling point (travelable point). And the motion element comprises the pose of each track point in the motion process. The motion primitive is a track segment which meets the vehicle kinematics and dynamics constraints and reaches the travelable point from the current vehicle position, a preset model is formed by any one curve selected from a spiral curve, a Bezier curve and a polynomial curve, and then a track curve (each track point contains corresponding pose information) formed by a plurality of track points is formed by forward simulation of sampling of a control space (a vehicle driving environment space) based on the preset model, so that the motion primitive aiming at the current travelable point is generated.
Finally, step S230 constructs a structured road cost function including path smoothness, collision risk occurrence probability and reference line path following influence factors, and selects an optimal motion primitive from the plurality of motion primitives obtained in step S220 by using the cost function, and determines the optimal motion primitive as an obstacle avoidance path for the current obstacle.
Thus, the invention generates the obstacle avoidance track (obstacle avoidance line) containing track point sequence information by utilizing the steps S210 to S230, improves the smoothness and safety of the output track of the obstacle avoidance algorithm of the existing automatic driving system, improves the obstacle avoidance capability of the unmanned mine card in the narrow terrain on the structured road, optimizes the route planning capability of the existing unmanned mine card automatic driving system, improves the automatic driving grade of the unmanned mine card, and expands the scene where the unmanned mine card can drive.
Since the completeness of the obstacle avoidance algorithm can be improved by proper state space sampling, although a feasible solution can be effectively searched by improving the sampling resolution, the computational complexity is increased, and many sampling states are invalid; if the sampling resolution is too small, the sampling space will be much smaller than the true solution space, so the possibility of not searching for a feasible solution will likely arise. At present, a sampling space of a main stream includes a state space and a control space, and in order to fully utilize topographic features and increase sampling effectiveness, the embodiment of the present invention performs sampling in the state space.
Fig. 3 is a schematic flow chart of an obstacle avoidance line generation process in the structured road obstacle avoidance method according to the embodiment of the present application. As shown in fig. 3, first, in step S2101, an obstacle visible point having an obstacle-free relationship between an obstacle in front of the current vehicle and the current vehicle (position) is selected based on the currently acquired obstacle state information. Due to the adoption of the state space sampling strategy on the structured road, which is provided by the embodiment of the invention, in order to improve the sampling effectiveness, the road boundary is processed by using a visual map method for the obstacles on the structured road. Referring to fig. 4, if an obstacle in the shape of a vehicle is detected, the obstacle is regarded as a convex polygon, and visible vertices of the convex polygon need to be extracted. Specifically, in order to extract a visible vertex, the laser radar installation position on the main vehicle body of the current unmanned mine car needs to be linearly connected with the vertex of the convex polygon of the obstacle, if no other obstacle is included in the middle of the connected straight line, the vertex is extracted and used as the visible vertex, namely, a visible point which has an obstacle-free relationship with the current unmanned mine car on the current obstacle.
Fig. 4 is a schematic diagram illustrating a principle of a process of selecting an obstacle visible point in the structured road obstacle avoidance method according to the embodiment of the present application. As shown in fig. 4, three vertices are extracted according to the method for extracting an obstacle visual point described in step S2101 above. After extracting the visible point of the obstacle polygon, the process proceeds to step S2102, where the visible point needs to be mapped onto a reference line (reference path).
Step S2102 screens out a mapping point having the smallest distance relationship with the current viewable point from among the plurality of path discrete points constituting the reference path information. Since the reference path information obtained by the present invention is a discrete waypoint (path discrete point) sequence, in order to make these discrete waypoints have continuity, a continuous reference line is first expressed by using a line continuity equation. In the embodiment of the invention, a spline interpolation curve is used for connecting the road point sequence to form a reference line. Wherein the line continuity equation is represented by the following expression:
y i (x)＝a 0 +a 1 x+a 2 x 2 +a 3 x 3 (1)
in the formula (1), (x, y) i ) Representing the coordinates of each discrete point of the path in the sequence of points in the reference path information, a 0 、a 1 、a 2 And a 3 Representing the coefficients after equation fitting.
Fig. 5 is a schematic diagram illustrating a principle of a process of converting a visible point into a mapping point in the structured road obstacle avoidance method according to the embodiment of the present application. Fig. 6 is a schematic diagram illustrating a plurality of path discrete points in reference path information in the structured road obstacle avoidance method according to the embodiment of the present application. After the continuity processing is performed on the route point sequence (route discrete point sequence), mapping points corresponding to all the barrier visual points in the reference route information need to be searched. As shown in fig. 5, v1, v2, and v3 represent three visual points of the current obstacle, respectively, and p1, p2, and p3 represent corresponding mapping points of the three visual points in the reference line information, respectively. In order to determine the pose information of the three mapping points P1, P2, and P3 in fig. 5, the embodiment of the present invention adopts kd-tree algorithm to apply a discrete reference route point sequence (refer to fig. 6, which shows continuous route points … … P in the route point sequence) i-1 、P i 、P i+1 … …) are stored in the kd-tree in advance, and then the closest points of the visual points v1, v2 and v3 in the kd-tree are inquired to obtain approximate mapping points n1, n2 and n3, wherein the three approximate mapping points are very close to the real mapping points in physical space. Then, the reference line equation (line) where the approximate mapping points are located is found out respectivelyEquation of continuity) f 1 ,f 2 ,f 3 And searching for the mapping points by using a nonlinear optimization method according to a reference line equation, wherein the distance from the visual point to the corresponding mapping point is the distance from the visual point to the nearest waypoint in the reference line. Since the searching method for each visual point is the same, the following description will be given of the searching process of the mapping point using one visual point as an example.
With a visual point v k ＝[x k ,y k ]For example, an approximate mapping point n is obtained i ＝[x i ,y i ]The equation where the approximate mapping point is located is y i (x)＝a 0 +a 1 x+a 2 x 2 ++a 3 x 3 And y i-1 (x)＝a 0 +a 1 x+a 2 x 2 +a 3 x 3 The approximate mapping point is in two curves and may be different curve equations, i.e. the true mapping point may be in either of the two curves. Then, the minimum distance equation of the approximate mapping point and the visual point under different curve equations is expressed by the following expression:
in the formula (2), min d 1 Expressed in equation y i (x)＝a 0 +a 1 x+a 2 x 2 ++a 3 x 3 Minimum distance, min d, of lower approximate mapping point to visual point 2 Expressed in equation y i-1 (x)＝a 0 +a 1 x+a 2 x 2 +a 3 x 3 The lower approximate mapping point is the minimum distance from the viewable point. For the above two to-be-constrained nonlinear optimization problems, the initial value solutions are both n i ＝[x i ,y i ]In order to solve the unique mapping point, a sequential quadratic programming algorithm is used to calculate the two upper mapping points in parallelAn optimization problem, after the optimization reaches the termination condition, two optimal solutions X are given 1 ＝[d min1 ,x min1 ,y min1 ]And X 2 ＝[d min2 ,x min2 ,y min2 ]Wherein (x) min1 ,y min1 ) Represents solution X 1 (x) of (C) min2 ,y min2 ) Represents solution X 2 The coordinates of (a). Then, by comparing d min1 、d min2 The solution corresponding to the smaller minimum distance value is the mapping point having the minimum distance relationship with the current visual point. This mapping point, whose coordinates are expressed by the following expression, will result in the minimum distance from the current viewable point:
wherein, X opt Represents solution X 1 And solution of X 2 The optimal solution therebetween. After the mapping point of each visual point on the reference line is obtained through the equations (1) to (4), pose information of a plurality of mapping points (each visual point corresponds to a corresponding mapping point) is obtained, and the process proceeds to step S1103 to complete the sampling process.
Step S2103 performs state space sampling on each mapping point along the mapping point normal vector direction according to a preset sampling step length, and obtains a plurality of sampling points for each mapping point. Since the state space sampling method of each mapping point is the same, the embodiment of the present invention will be described by taking the state space sampling process of one mapping point as an example. For state space sampling of a current mapping point, it is necessary to extend the current mapping point in the normal vector direction of the current mapping point to the two end directions of the vector, and from the current mapping point, a pair of sampling points that are axisymmetric with respect to the current mapping point tangential vector direction is determined every sampling step length, and after sampling m times, m pairs of sampling points (i.e., 2m sampling points) for the current mapping point are obtained.
Fig. 7 is a schematic diagram illustrating a principle of a curvature calculation process of a sampling point in the structured road obstacle avoidance method according to the embodiment of the present application. As shown in FIG. 7Show, P m When sampling state space, the current mapping point is taken as an original point, sampling is respectively carried out along the directions of two ends of the normal vector of the current mapping point, a pair of sampling points is determined every sampling step length, and m pairs of sampling points are obtained after sampling is carried out for m times. As shown in FIG. 7, P n1 、P n2 Respectively obtaining a first pair of sampling points obtained by first sampling, wherein the distances between the two sampling points and the current mapping point are the length of a sampling step length; then, taking the first pair of sampling points as an origin, sampling along the directions of the two ends of the normal vector of the current mapping point, sampling the step length again at intervals, determining the second pair of sampling points, wherein the distances between the second pair of sampling points and the current mapping point are both 2 times of the sampling step length, and so on until m times of sampling is completed, and obtaining 2m sampling points (a group of sampling points, referring to the first group of sampling points and the second group of sampling points in fig. 8). It should be noted that, in the embodiment of the present invention, the sampling times of the state space of the mapping point are not specifically limited, and those skilled in the art may set the sampling times according to influence factors such as the requirement of sampling precision.
After the state space sampling for each mapping point is completed, the process proceeds to step S2104. Step S2104, according to the pose information (coordinates, course angles and curvatures) of each mapping point, the course angles and the curvatures of all sampling points corresponding to each mapping point are calculated, and the pose information of each sampling point is further determined. If 3 mapping points are obtained in step S2102, 4m sampling points are obtained for each mapping point in step S2103, and therefore 12m sampling points are obtained in total, and it is necessary to calculate the pose information of the 12m sampling points in step S2104. Since the determination process of the pose information of each sampling point is the same, the embodiment of the present invention describes step S2104 in detail by taking the pose information determination process of one sampling point as an example. The pose of the sampling point obtained by the embodiment of the invention is related to the pose of the mapping point to which the sampling point belongs, and the reference path information obtained by the embodiment of the invention comprises the pose information of each road point, so after the mapping point corresponding to each barrier visual point is obtained, the pose information of each mapping point can be known according to the reference path information.
In step S2104, first, a tangent vector τ and a normal vector n of the reference line at the mapping point are determined, and the direction of the tangent vector is the heading angle of the mapping point. The solution equation for the tangent vector direction is shown below:
y′(x)＝a 1 +2a 2 x+3a 3 x 2 (5)
then, the expression equation of the tangent vector is given:
τ＝[1,y′] T ＝[1,a 1 +2a 2 x+3a 3 x 2 ] T (6)
where T represents a transposed symbol. Because the course angle of the pose of the sampling point is equal to the course angle theta of the mapping point p In the embodiment of the invention, the course angle of the pose of the sampling point is set to be parallel to the reference line, so that the coordinate value of the sampling point is set to be X in order to calculate the coordinate value of the sampling point n ＝[x n ,y n ] T . Because the sampling point is located at the mapping point X p ＝[x p ,y p ] T Therefore, when a pair of sampling points (a first side sampling point and a second side sampling point) corresponding to the current mapping point and axisymmetric to the tangential vector of the mapping point is collected at the same time, the coordinates of the pair of sampling points are determined by using the following expression:
in the formula (7), x n 、y n Respectively representing coordinate values of sample points, X p Represents the current mapping point coordinate, lambda represents the sampling step length, and c represents the sampling order (1 ≦ c ≦ m, c is an integer). Further, the curvatures of the first side sample point and the second side sample point are calculated using the following expressions:
wherein, κ p Representing the curvature of the current mapped point. Further, the curvature κ of the current mapping point is expressed by the following expression p ：
y′(x)＝a 1 +2a 2 x+3a 3 x 2 (10.1)
y″(x)＝2a 2 +6a 3 x (10.2)
Further, when the sampling point coordinates are obtained by the above equation (7), two coordinate values of a pair of sampling points can be obtained for the same sampling (c value is the same), and the curvature values of the two sampling points can also be obtained by the above equations (8.1) and (8.2), but it is not clear which sampling point the two coordinate values and the two curvature values belong to respectively at the left and right ends of the tangential direction of the current mapping point. Therefore, in the state space sampling process of the embodiment of the present invention, when a pair of sampling points is obtained at one time of sampling and the coordinate values and curvature values of the pair of sampling points are calculated, the two coordinate values and curvature values need to be allocated.
More specifically, the two-coordinate value assignment process will be described first. When the coordinate value is carried out, a calculation method of vector outer product is required to be introduced, and the distribution positions of the first side sampling point and the second side sampling point are determined according to the calculation result, so that the curvature symbol is judged. That is, the distribution positions of the first side sampling point and the second side sampling point are determined according to the positive and negative states of the wheel rotation angle at which the current vehicle travels to the mapping point. When the wheel rotation angle is positive, the first side sampling point is positioned on the left side of the direction of the tangent vector of the current mapping point, and the second side sampling point is positioned on the right side of the direction of the tangent vector of the mapping point; when the wheel rotation angle is negative, the first side sampling point is positioned on the right side of the tangential vector direction of the mapping point, and the second side sampling point is positioned on the right side of the tangential vector direction of the mapping pointThe point is located to the left of the current mapping point tangent vector direction. Further, since the positive and negative states of the wheel angle can be represented by the calculation result of the vector outer product, the mapping point p is represented by the equation (7) m To obtain two solutions X n1 、X n2 (representing the coordinates of the first side sampling point and the second side sampling point respectively), calculating the outer product at the current mapping point by using the following outer product expression:
δ＝(X n1 -p m )×τ (11)
where δ represents the outer product at the current mapping point. When delta is greater than 0, then X is solved n1 (first side sample point) on the left side of the reference course curve (of the current mapping point tangent vector direction), refer to FIG. 6,p n1 ＝X n1 (ii) a Solution of X n2 (second side sample point) on the right side of the reference course curve (of the current mapped point tangent vector direction), reference plot 6,p n2 ＝X n2 . On the contrary, when delta is less than 0, X is solved n1 (first side sample point) on the right side of the reference course curve (of the current mapped point tangent vector direction), reference plot 6,p n2 ＝X n1 (ii) a Solution of X n2 (second side sample point) on the left side of the reference course curve (of the current mapped point tangent vector direction), reference diagram 6,p n1 ＝X n2 。
In addition, the embodiment of the present invention also needs to allocate the two curvature values. The positive and negative states of the curvature value of the mapping point are used for determining the curvature value of the corresponding sampling point calculated by the above equation (8.1) or equation (8.2). Specifically, the increase and decrease states of the curvature radius of a pair of sampling points distributed on two sides of the tangent vector direction of the current mapping point are judged according to the positive and negative states of the curvature of the current mapping point, so that a suitable sampling point curvature calculation formula is selected. If the curvature of the current mapping point is κ p If the curvature radius is positive, it means that the reference path curve is convex upward, and the radius of the curvature circle of the sampling point (first side sampling point) located at the left side of the curve (the tangential vector direction of the current mapping point) is increased, and at this time, the curvature of the sampling point is calculated by the above equation (8.1), i.e., k @ n1 Representing the curvature of the first side sample point; the right side sample (second side sample) is calculated using equation (8.2) aboveCurvature of the sample point, i.e. kappa n2 The curvature of the second side sample point is indicated. If the current mapping point curvature k p If the curvature of the sampling point is negative, the reference path curve is concave downward, and the radius of the curvature circle of the sampling point located at the left side of the curve (the tangential vector direction of the current mapping point) is decreased, and the curvature of the sampling point is calculated by the above equation (8.2), i.e., k n2 Representing the curvature of the first side sample point; the right side sample point (second side sample point) calculates the curvature of the sample point, i.e., κ, using equation (8.1) above n1 The curvature of the second-side sample point is indicated.
It should be noted that there is no sequential logical order relationship in the coordinate value allocation process and the curvature value allocation process, and the embodiment of the present invention does not specifically limit the sequential order of the coordinate value allocation process and the curvature value allocation process, and may perform coordinate value allocation first and then curvature value allocation, or vice versa.
In summary, when sampling a mapping point for the c-th time, a pair (two) of sampling points symmetric with respect to the tangential direction of the current mapping point is obtained, and the calculation equation of the pose of the pair of sampling points is expressed by the following expression:
referring to fig. 5, after all the visual points of the current obstacle have been determined, there are a plurality of mapping points on the reference line equal to the number of visual points, a corresponding number of normal vectors are generated, and then, the mapping points are uniformly sampled along the normal vector direction of each mapping point, as shown in fig. 8, thereby completing the whole sampling process.
Fig. 8 is a schematic diagram illustrating a principle of a motion primitive generation step in the structured road obstacle avoidance method according to the embodiment of the present application. Referring to fig. 3 and 8, the process proceeds to step S220, where corresponding motion primitives are generated for all sample points one by one. If one mapping point corresponds to 2m sampling points, if 2 mapping points are detected in the execution period of the obstacle avoidance algorithm, the number of all sampling points is 4m, that is, 4m motion elements need to be generated, and each sampling point corresponds to one motion element. Since the generation process of each motion primitive is the same, the embodiment of the present invention takes the generation process of one motion primitive as an example to describe step S120 in detail.
First, the process proceeds to step S2201, a polynomial spiral curve is used to establish a motion primitive solution constraint equation for each sample point, and then the process proceeds to step S2202. The motion element solving equation comprises a curvature equation, a course angle equation and a motion position equation.
In order to connect the position and pose of the sampling points and generate a track meeting non-integrity constraint and dynamic constraint, the embodiment of the invention firstly adopts a cubic polynomial spiral curve to connect the position and pose of the current unmanned mine position and each sampling point. The closed-form solution equation of the curvature, the course angle and the motion position of the cubic polynomial spiral curve is expressed by the following expression:
wherein s represents the accumulated travel of each track point in the motion primitive track curve (i.e. the accumulated travel from the track point to the current position of the unmanned mine position), κ(s) represents the curvature of each track point in the motion primitive track curve, θ(s) represents the course angle of each track point in the motion primitive track curve, [ x(s), y(s)]Representing the position coordinates, κ, of each trace point within the trace curve of the motion primitive 0 、κ 1 、κ 2 、κ 3 Respectively, are unknown solutions of closed form solution equations.
And then, generating a motion primitive solution constraint equation under the constraint conditions of satisfying the non-integrity constraint conditions (the non-integrity constraint here refers to curvature constraint) and the mine truck driving position. Wherein, the motion element solving constraint equation is expressed by the following expression:
wherein, κ 0 Denotes the curvature of the initial point in the motion element trajectory curve, κ init Denotes the curvature, κ, at the current unmanned mine card f Representing end-point curvature, s, in the trajectory curve of the moving element f Representing the accumulated travel between the end point and the initial point in the motion element trajectory curve, theta f Indicating course angle theta of end point track point in motion primitive track curve 0 Indicates the initial point course angle (i.e. the course angle at the current unmanned mine card), [ x ] 0 (s)，y 0 (s)]Represents the initial point coordinates (i.e., the coordinates at the current unmanned mine card), [ x ] f (s)，y f (s)]Coordinates, k, representing the end-point locus points in the locus curve of the motion primitive 0 ,κ 1 ,κ 2 ,κ 3 ,s f The unknown solutions of the constraint equations are solved for the motion primitives, respectively.
Step S2202 determines an unknown solution vector of the equation according to the motion primitive solution constraint equation, constructs a motion state vector representing each track point in the motion process in the current motion primitive solving result, and further establishes a motion state equation related to the unknown solution vector by taking the current sampling point as an end point state. To simplify the subsequent calculations, the unknown solution parameter vector of the kinematic primitive solution constraint equation is first written in four-dimensional form, q = [ κ ] 1 ,κ 2 ,κ 3 ,s f ] T Furthermore, in order to subsequently solve the four unknown parameters by using a target practice method and a Newton method, an initial value is set for the parameter vector; then, backward simulation is performed according to equation (14) to obtain a motion state vector x corresponding to each track segment curve g ＝[x g ,y g ,θ g ,κ g ] T I.e., forming motion state vectors for the trace points of each trace segment curve, and constructing a motion state equation for the unknown solution vector. Wherein, κ g The curvature of the end point track point of each track segment in the motion primitive track curve (the track segment is a curve segment formed between the current unmanned mine position as the initial point and different track points), theta g Representing each track piece in the track curve of the motion primitiveCourse angle of end point track point of segment, [ x ] g (s)，y g (s)]And the coordinates of the end point track point of each track segment in the track curve of the motion primitive are represented.
In particular, when performing backward simulation, the terminal state obtained by simulation for each track segment is an equation about an unknown solution vector q, which can be written as h (q) = x g (q) form. Thus, a motion state difference equation representing the motion state of each track point is obtained and is used for representing the subsequent simulation state of each track point. Wherein the differential equation of motion state is expressed by the following expression:
g(q)＝x f -h(q) (15)
wherein, g (q) represents the state variable quantity simulating the state of each track point moving from the current track point position to the motion primitive track curve end point. Specifically, equation (15) includes the following equation:
wherein h is x (q)＝x g (q)，h y (q)＝y g (q)，h θ (q)＝θ g (q)，h κ (q)＝κ g (q)，g 1 (q)、g 2 (q)、g 3 (q) and g 4 And (q) respectively representing the track point abscissa state variable quantity, the track point ordinate state variable quantity, the track point course angle state variable quantity and the track point curvature state variable quantity for simulating the motion of each track point from the current track point position to the motion primitive track curve end point state. In order to solve the coefficients using the newton method, the above motion state difference equation is rewritten into the form of newton's iterative equation and expressed by the following expression:
wherein the content of the first and second substances,
for the Jacobian matrix of the above-described differential equations of motion states, Δ q is the increment of the unknown solution vector, i.e., q k+1 -q k Therefore, the above formula (17) can be further rewritten into a jacobian matrix form and expressed by the following expression:
△q＝J(q) -1 g(q) (18)
further, the expression of the jacobian matrix J is as follows:
thus, after obtaining the above-described differential equation of the motion state, the differential equation is rewritten into the form of newton' S iteration equation and the form of jacobian matrix in order, the above-described construction of the motion state equation regarding the unknown solution vector is completed, and the expression (18) is obtained, so that the process proceeds to step S1203.
Step S2203 sets the initial conditions and the termination conditions of the above-mentioned equation of motion state, and solves the current equation of motion state by using a predetermined perturbation vector in a jacobian matrix recursion manner to obtain a motion primitive formed by the solved unknown solution vector, that is, a motion primitive formed by a plurality of solved trajectory points.
Specifically, in order to solve the above equation of motion state with a singular jacobian matrix, a perturbation vector needs to be set by a numerical method to estimate the jacobian matrix. Further, the above equation of state of motion needs to be rewritten into an estimable form and expressed by the following expression:
wherein i and j represent the row and column numbers of the vectors q and g (q), respectively, and e represents the perturbation vector e = [ e = 1 ,e 2 ,e 3 ,e 4 ] T ，e 1 、e 2 、e 3 、e 4 Respectively representing the disturbance quantity of the abscissa state and the disturbance of the ordinate stateThe vector is not particularly limited in the embodiment of the present invention, and may be set by a person skilled in the art according to the calculation accuracy of the actual motion primitive. In addition, the above initial conditions are expressed by the following expressions:
further, the above termination condition is expressed by the following expression:
wherein x is err 、y err 、θ err 、κ err And respectively representing the track point abscissa deviation amount, the track point ordinate deviation amount, the track point course angle deviation amount and the track point curvature deviation amount under the condition of reaching the iteration termination. In addition, the maximum iteration frequency is set to be 50 times in step S2203, and once the termination condition met by the algorithm in the iteration process or the maximum iteration frequency is reached, the iteration is exited to output the motion primitive result containing the pose information of the plurality of track points after the solution.
In addition, in the embodiment of the invention, in order to improve the solving efficiency of the motion state equation, a query table can be further constructed, the terminal states of a plurality of groups of sampling points are uniformly sampled in the first several obstacle avoidance algorithm execution periods, then, a motion element is generated by using a spiral curve, the generated query table is stored in an offline mode, and a better initial value of a parameter vector is searched for the spiral curve by searching the query table in the vehicle motion process.
Referring to fig. 8, by using the steps S2201 to S2203, a balance of the resolution of the sampling trajectory curve is achieved by setting a proper sampling step length, so that the algorithm can find a relatively ideal trajectory conforming to constraints of kinematics, dynamics, non-integrity and the like, and the time complexity of the algorithm is relatively small. In this way, the motion primitive trajectory curves for all the sampling points are generated through the above steps S2201 to S2203, and the process proceeds to step S230 to select the optimal motion primitive curve.
In order to select the optimal track in the sampling track set, the embodiment of the invention designs a cost function for comprehensively evaluating the motion element track, wherein the cost function is a linear combination of various cost items, including a smoothness cost item of a motion element track curve, a risk cost item of collision between a current unmanned mine card and a current obstacle, and an offset cost item representing the capability of the current unmanned mine card to follow a reference path. Firstly, each motion element trajectory curve obtained in the step S220 needs to be evaluated for the three cost items respectively, then a comprehensive cost value for the current motion element is obtained, and finally, the comprehensive cost values representing evaluation scores of the motion elements are compared, and the optimal motion element is screened out and output. Since the calculation process of the integrated cost value of each motion primitive is the same, the embodiment of the present invention takes the integrated cost value of one motion primitive as an example to explain the calculation process of the cost value.
First, the process proceeds to step S2301. Step S2301 is to calculate the curvature square integral of the current motion element by using the smoothness cost calculation formula according to the curvature of each track point in the current motion element, and the calculation result is used as the path smoothness cost value. Wherein, the smoothness cost calculation formula is expressed by the following expression:
wherein f is smooth Representing the path smoothness cost value, s, of the current motion primitive 0 Showing the accumulated travel of the initial point in the current motion primitive track curve. Thus, each motion primitive can calculate a corresponding path smoothness cost value using equation (23), and the process proceeds to step S2302.
Step S2302 determines the distance from each track point to the current obstacle according to the coordinates of each track point in the current motion primitive, determines the minimum distance among the distance values, and obtains the collision risk cost value for the current motion primitive by using a collision risk cost calculation formula according to the preset first disturbance quantity and the minimum distance between the current motion primitive and the current obstacle.
Firstly, the minimum distance between the current motion element and the current obstacle is determined by using a minimum distance calculation formula between the motion element and the obstacle. Specifically, the nearest track point from the current obstacle to the current motion primitive is determined, and the distance between the nearest track point and the current obstacle is determined. Wherein, the above-mentioned motion element and obstacle minimum distance formula is expressed by the following expression:
d min ＝min{||X 0 -X obs ||,||X 1 -X obs ||,...,||X i -X obs ||,...,||X n -X obs ||} (24)
wherein d is min The minimum distance between the current motion element and the current barrier is represented, i represents the sequence number of each track point in the current motion element, and X i Position coordinates, X, representing any one of the trace points in the current motion primitive 0 、X 1 、X n Respectively representing the coordinates of the initial track point, the second track point and the last track point in the current motion primitive, n representing the total number of the track points of the current motion primitive, X obs A position coordinate vector representing a visual point of the obstacle to which the current motion primitive belongs. Therefore, each motion element can calculate the minimum distance between the corresponding current motion element and the current obstacle by using the formula (24), and then, the corresponding collision risk cost value is calculated continuously.
Because the motion primitive is formed by connecting discretized n +1 track point sequences, a minimum distance is obtained after the distance calculation is carried out on the discretized track points, and the minimum distance d min May be 0, a small disturbance amount (first disturbance amount) needs to be added to the above calculation formula for collision risk cost. Wherein the collision risk cost calculation formula is expressed by the following expression:
wherein f is risk Represents the collision risk cost value of the current motion primitive, and σ represents the first disturbance amount. In the embodiment of the present invention, preferably, the first disturbance amount σ =0.0001. Thus, each motion primitive can calculate the corresponding collision risk cost value according to the corresponding minimum distance by using the above equation (25), and then the process proceeds to the above step S2303.
In practical application, the ideal state of the motion element in the lane is as close to the reference line of the lane as possible, that is, as close as possible to the center line of the lane, so the embodiment of the invention uses the mean value of the square deviation of the motion trajectory path and the lane reference line to represent the following performance of the road reference line. Step S2303, according to the coordinates of each track point in the current motion element, determining the corresponding mapping point of each track point in the current reference route, and based on the mapping point, calculating the mean value of the square deviation of the current motion element and the current reference route by using the following cost formula to obtain the following cost value of the reference route.
Specifically, in step S2303, each track point in the current motion primitive is taken as a visual point, and according to the method described in step S2102, the mapping point of the current track point in the current unmanned mine card reference route information is determined, and the mapping point corresponding to each track point in the current motion primitive is further determined. And then, calculating a deviation evaluation result of the current motion primitive and the current reference route by using a following cost formula according to the position coordinates of all mapping points in the current motion primitive, namely a reference route following cost value. Wherein the following cost formula is expressed by the following expression:
wherein f is ref A reference line-following cost value representing the current motion primitive,
and coordinates of a mapping point corresponding to any one track point in the current motion primitive in the current reference route. Thus, each motion primitive can calculate a corresponding reference line following cost value using the above equation (26), and the process proceeds to step S2304.
It should be noted that, since the above-mentioned calculation process of the path smoothness cost value, the calculation process of the collision risk cost value, and the calculation process of the reference line following cost value are in a parallel relationship, there is no precedence relationship among the three calculation processes, and no matter the three calculation processes may be executed in parallel, they may also be executed according to the corresponding order, which is not specifically limited in the present invention.
Step S2304 obtains a corresponding comprehensive cost value (i.e., the cost value of the current motion primitive) by using the cost function according to the path smoothness cost value, the collision risk cost value, and the reference line following cost value corresponding to each motion primitive trajectory curve. Wherein the cost function is represented by the following expression:
f＝w smooth f smooth +w risk f risk +w ref f ref (27)
where f denotes the cost value for the current motion primitive, w smooth 、w risk 、w ref And respectively representing the weight value of the path smoothness cost item, the weight value of the collision risk cost item and the weight value of the reference line following cost item. It should be noted that, in the embodiment of the present invention, the three weight values are not specifically limited, and those skilled in the art can set the three weight values according to actual application situations. Thus, each motion primitive obtains a corresponding overall cost value through the above steps S2301 to S2304, and the evaluation of the motion primitive is completed, so that the process proceeds to step S2305.
Step S2305, selecting a motion element corresponding to the minimum value from cost values corresponding to all motion elements, taking the motion element with the minimum value as an optimal motion element, and outputting the optimal motion element (obstacle avoidance path) containing track point pose sequence information to a path tracking module in the unmanned mine card, so that the unmanned mine card runs according to the current optimal obstacle avoidance line, current obstacles are avoided smoothly, and the safety of the unmanned mine card running in a structured road is guaranteed.
Further, in order to improve reliability and stability of the obstacle avoidance algorithm, after the obstacle avoidance path is generated, curvature constraint detection and collision constraint detection need to be performed on the path. After the optimal path in the obstacle avoidance behavior is planned, various hard constraint checks are required to be performed on the optimal path, and the hard constraints are constraints which the track must meet, including collision avoidance constraints and curvature constraints. Soft constraints are constraints that the trajectory is as good as possible, and certainly not, and include sufficient smoothness, i.e., spatial smoothness and temporal smoothness. The space smoothness comprises heading continuity and curvature continuity, and the track vehicle with discontinuous curvature can track the track vehicle, but the tracking effect is not good.
Referring to fig. 2, the obstacle avoidance algorithm of the present invention further includes: step S240 (path detection step) of curvature constraint detection on the currently generated obstacle avoidance path and collision constraint detection on each track point in the currently generated obstacle avoidance path, and if any one of the points is unqualified (curvature constraint detection is unqualified and/or collision constraint detection is unqualified), returning to the step S210 for resampling; otherwise, the current obstacle avoidance path is detected to be in a qualified state, and the current obstacle avoidance path is output.
In order to meet collision constraints, the embodiment of the invention needs to perform collision detection on the track of the vehicle body. Step S2401 establishes a two-dimensional vehicle body model for the current unmanned mining vehicle. The two-dimensional vehicle body model comprises a polygon approximate to the current vehicle, a circumscribed circle of the polygon and a plurality of enveloping circles which are sequentially overlapped and cover the polygon along the longitudinal axis direction of the current vehicle. Wherein the radius of each enveloping circle is the same.
In collision inspection, current unmanned mine cards and other obstacle objects and the like are modeled as objects that can translate and rotate in a two-dimensional plane space with three degrees of freedom. Fig. 10 is a schematic illustration showing a two-dimensional vehicle body model in the structured road obstacle avoidance method according to the embodiment of the application. As shown in fig. 10, the vehicle body of the current unmanned mine card is approximated by a rectangle and approximated by a group of overlapping circles, specifically, an circumscribed circle is used to cover the rectangle or any polygon, and three small envelope circles with the same radius are used to cover the rectangle or any polygon in an optimal manner, the centers of the three small envelope circles are sequentially arranged on the longitudinal axis of the center of the vehicle body, the first small envelope circle is circumscribed with the third small envelope circle, the tangent point is the center of the middle small envelope circle and is the center of the vehicle body (the center of the rectangle or any polygon), and the vertex of the rectangle or any polygon is on the corresponding first small envelope circle or the third small envelope circle.
Referring to fig. 10, l represents a longitudinal length of a current unmanned mining truck body, W represents a transverse width of the current unmanned mining truck body, R represents a circumscribed circle radius of the current unmanned mining truck body, R represents a small envelope circle radius of the current unmanned mining truck body, H represents a distance from a rear axle center point of the current unmanned mining truck to a rear boundary of the truck body, d O The distance between the centers of two adjacent small envelope circles is represented, the first small envelope circle is circumscribed with the third small envelope circle, the tangent point is the center of the middle small envelope circle and is the center of the rectangular body, and the vertex of the rectangle is on the corresponding first small envelope circle or the third small envelope circle. That is to say, the center of the first small enveloping circle is regarded as the center point position of the axle of the front wheel vehicle of the current unmanned mine card, and the center of the third small enveloping circle is regarded as the center point position of the axle of the rear wheel vehicle of the current unmanned mine card. The current unmanned vehicle body model meets the following mathematical relational expression:
because the GPS and the inertial navigation device of the unmanned mine card are arranged at the central point of the rear wheel axle of the mine card, when collision inspection is carried out, the position and attitude vector of the central point of the rear axle is known, the central position coordinate of the vehicle body and the position coordinate of the central point of the front wheel axle are required to be given, and the position and attitude vector is expressed by the following expression:
wherein (x) 2 ，y 2 ) Position coordinates (x) indicating the center of the vehicle body 3 ，y 3 ) Position coordinates [ x ] representing the center point of the front wheel axle 1 ,y 1 ,θ 1 ] T A pose vector representing the center point of the rear axis, (x) 1 ，y 1 ) Position coordinates, theta, representing the centre point of the axle of the rear wheel 1 Representing the heading angle of the current unmanned mining vehicle. This completes the construction process of the two-dimensional vehicle body model for the current unmanned mine card, and the process proceeds to step S2402 to perform collision detection.
Fig. 11 is a schematic principle diagram of a simulation step performed on each track point of a current vehicle traveling to an obstacle avoidance path by using a two-dimensional vehicle body model in the structured road obstacle avoidance method according to the embodiment of the present application. Referring to fig. 11, in step S2402, each track point of the current unmanned mine card traveling into the obstacle avoidance path is simulated by using the two-dimensional vehicle body model constructed in step S2401, and positions of an circumscribed circle and a plurality of enveloping circles at each track point are determined, based on which, whether the two-dimensional vehicle body model collides with an obstacle in front of the current vehicle is sequentially detected for each track point in the current obstacle avoidance path, and if so, the collided track point is marked.
Fig. 9 is a flowchart of a path detection step in the structured road obstacle avoidance method according to the embodiment of the present application. As shown in fig. 9, first, step S901 enters an obstacle avoidance line collision detection program, and step S902 writes position information of a first track point in a current obstacle avoidance route, and takes a position of the first track point as a position of a current unmanned mine card vehicle. Then, in step S903, according to the position of the current unmanned mine card vehicle, a center coordinate of the circumscribed circle (a center point coordinate of a vehicle body rectangle or a polygon) and an euclidean distance between the center coordinate of the circumscribed circle and a current obstacle coordinate (where the current obstacle coordinate refers to an obstacle boundary discrete point closest to the current circumscribed circle center coordinate, and the obstacle boundary discrete point is also a discrete point of the following obstacle contour information) are determined. Step S904 detects whether the Euclidean distance between the center coordinate of the circumscribed circle and the current obstacle coordinate is larger than the radius of the circumscribed circle, if so, the current vehicle body does not collide with the obstacle in front of the current unmanned mining vehicle, and step S905 is entered to continue to detect the three small enveloping circles. If the current vehicle collision constraint detection result is less than the threshold value, the current vehicle collides with an obstacle in front of the current unmanned mining vehicle, namely, the current collision constraint detection is unqualified, and the step S911 is entered to immediately exit the obstacle avoidance line collision detection program.
Step S905 determines the center coordinates of the three enveloping circles at the current position according to the position of the current unmanned mine-card vehicle, and determines the euclidean distance between the center coordinates of each enveloping circle and the current obstacle coordinates (where the current obstacle coordinates are the obstacle boundary discrete points closest to the center coordinates of each enveloping circle), so as to enter step S906. Step S906 detects whether the Euclidean distance between the center coordinate of each enveloping circle and the current obstacle coordinate is larger than the radius of the enveloping circle or not in sequence, if the Euclidean distance corresponding to each enveloping circle is larger than the radius of the enveloping circle, it is indicated that each enveloping circle does not collide with the obstacle in front of the current unmanned mining vehicle, and the step S907 is entered. If the Euclidean distance corresponding to one or more enveloping circles is smaller than or equal to the radius of the enveloping circle, the phenomenon that the one or more enveloping circles collide with the obstacle in front of the current unmanned mining vehicle is indicated, namely the current collision constraint detection is unqualified, and the step S911 is entered and the obstacle avoidance line collision detection program is immediately exited.
Step S907 judges whether the current unmanned mining vehicle collides with the road boundary, if so, the step S911 is immediately executed; if not, the process immediately proceeds to step S908. Specifically, when judging whether the current unmanned mining vehicle collides with the road boundary, it needs to detect whether the distance from the center of the circumscribed circle corresponding to the current mining vehicle to the perpendicular lines at the two ends of the structured road is greater than the radius of the circumscribed circle, and whether the distance from the center of each enveloping circle corresponding to the current mining vehicle to the perpendicular lines at the two ends of the structured road is greater than the radius of the enveloping circle, and if the two conditions are both satisfied, it indicates that the current unmanned mining vehicle does not collide with the road boundary. If one of the two conditions is not met, it indicates that the current unmanned mining vehicle collides with the road boundary.
Step S908 determines whether the current trace point is the end point of the obstacle avoidance line, and if not, the process proceeds to step S910. Step S910 adds 1 to the sequence number corresponding to the current trace point, and then returns to step S902 to perform collision detection on the next trace point. If the current track point is the end point of the current obstacle avoidance line, the process proceeds to step S909. Step S909 generates indication information indicating that the current collision constraint detection is qualified, immediately exits the current obstacle avoidance line collision detection program, and outputs the optimal obstacle avoidance line without the trajectory collision flag information.
In addition, when the current detection of the collision constraint is not qualified, the process proceeds to step S911, and step S911 further needs to mark the track points where the collision will occur, generate corresponding track collision flag information, and output the track collision flag information.
It should be noted that, in the embodiment of the present invention, the expression form of the obstacle information is mainly performed by discrete points, and during the traveling process of the unmanned mine card, the surrounding obstacle points are processed into a convex hull form; then, the boundary of the convex hull is sampled to obtain a series of boundary discrete points about a certain obstacle, and the series of boundary discrete points are stored in the obstacle state information as the contour information of the obstacle.
Thus, the collision constraint detection for the current obstacle avoidance trajectory is completed through the above steps S901 to S911.
Further, in the curvature constraint detection process, coordinates of each track point in the current obstacle avoidance route are sequentially substituted into the following curvature constraint detection formula according to the sequence of each track point in the current obstacle avoidance route to detect whether the curvature of each track point meets the curvature constraint detection formula condition, and if yes, the curvature constraint detection for the current obstacle avoidance route is qualified. Therefore, under the condition that the collision constraint detection and the curvature constraint detection are qualified, the current obstacle avoidance path is directly output.
In addition, if the curvature of one track point does not meet the curvature constraint detection formula condition, the curvature constraint detection of the current obstacle avoidance route is unqualified, the track point with the unqualified curvature detection is marked, and corresponding curvature detection mark information is generated and output. Wherein the curvature constraint detection formula is expressed by the following expression:
wherein, κ max The maximum steering angle which can be achieved by the current unmanned mine truck vehicle is shown, and y' are calculated by using the above equations (10.1) and (10.2), respectively.
Fig. 12 is a specific flowchart of a structured road obstacle avoidance method according to an embodiment of the present application. As shown in fig. 12, firstly, in step S1201, the vehicle pose of the vehicle in real-time running, the obstacle state information around the running environment, and the reference path information are acquired according to the method described in step S210, based on which, state space sampling is performed on the visible points of the obstacle in front of the current vehicle, the pose of each sampling point is determined, and state space sampling on the structured road is completed. Then, step S1202 connects the current vehicle position with the pose of each sampling point as an end point state and the current vehicle pose as an initial state in step S220, respectively, to obtain a motion element of the current vehicle relative to each sampling point, and generates a motion element for each sampling point. Then, step S1203 constructs a structured road cost function including path smoothness, collision risk occurrence probability and reference line path following property by using step S230, and selects an optimal motion primitive from all motion primitives by using the cost function, and determines the optimal motion primitive as an optimal obstacle avoidance path for the current obstacle, thereby completing selection of an optimal track. Next, step S1204 performs curvature constraint detection on the curvature at each trajectory point in the optimal obstacle avoidance path according to the method described above, and performs collision constraint detection on each trajectory point in the optimal obstacle avoidance path according to the methods described in steps S2401 to S2402. Step S1205 determines whether or not the hard constraint condition is satisfied (i.e., whether or not all of the curvature constraint detection and the collision constraint detection are acceptable), and if so, the routine proceeds to step S1206. Step S1206 outputs the current optimal obstacle avoidance route.
In step S1205, if the hard constraint condition is not satisfied (the curvature constraint detection fails and/or the collision constraint detection fails), the process proceeds to step S1207. Step S1207 reduces the sampling step size to perform state space sampling again, counts the number j of resampling times within the current obstacle avoidance algorithm execution cycle, and then proceeds to step S1208. Step S1208 determines whether the resampling time within the current obstacle avoidance algorithm execution period reaches a preset maximum resampling time, and if so, the process proceeds to step S1209. Step S1209, an obstacle avoidance module outputs a warning instruction which indicates that the unmanned mine card needs to stop immediately without track generation, so that the unmanned mine card can stop at a reduced speed immediately after receiving the warning instruction, and the safe driving of the unmanned mine card vehicle is guaranteed.
Further, in step S1208, if the resampling time in the current obstacle avoidance algorithm execution period does not reach the preset maximum resampling time, returning to step S1201, and resampling the state space of the structured road by using the reduced sampling step length. It should be noted that, in the embodiment of the present invention, the maximum resampling number is not specifically limited, and may be set by a person skilled in the art according to actual situations, and preferably the maximum resampling number is 3.
The embodiment of the invention provides an obstacle avoidance method for driving of an unmanned mine card on a structured road. The method designs a state space sampling rule suitable for the structured road, and provides a solving equation of the pose of a sampling point; then connecting the pose of the sampling points with the current vehicle, and generating a motion element meeting the kinematic constraint aiming at each sampling point; a cost function suitable for a structured road is established, so that the generated optimal track meets the kinematics and obstacle avoidance constraints, has better smoothness and more ideal following performance on road reference, namely the generated track can be kept to run in the center of the road as far as possible under the condition of meeting various constraints; finally, a set of rapid collision check algorithm is established, and curvature check is carried out on the optimal track; and once the generated optimal track is found not to meet the strong constraint condition, entering a resampling stage immediately until the sampling times reach the maximum value, exiting iteration, and outputting a conclusion that no feasible solution exists.
The invention is an important part for realizing unmanned mines, and can bring the following benefits by realizing autonomous obstacle avoidance of the unmanned mine in the driving process of the structured road: the workload of the driver can be reduced; the number of the drivers can be reduced, so that the labor cost of mine development is reduced; for drivers, the health problem caused by the mine environment is reduced; the intelligent degree of the mine is improved, and the autonomous safe operation of the mine card is realized; the mining efficiency of the mine is improved to a certain extent, and the method has great practical application value.
Although the embodiments of the present invention have been described above, the above descriptions are only for the convenience of understanding the present invention, and are not intended to limit the present invention. It will be understood by those skilled in the art that various changes in form and details may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.
Claims (9)
1. A structured road obstacle avoidance method, characterized in that the method comprises:
pose sampling: acquiring the vehicle pose of a vehicle in real-time running, the state information of obstacles around a running environment and reference path information, and based on the state space information, performing state space sampling on visible points of the obstacles in front of the current vehicle to determine the pose of each sampling point;
a motion primitive generation step: respectively connecting the pose of the current vehicle and the pose of each sampling point by taking the pose of each sampling point as a terminal state and the pose of the current vehicle as an initial state to obtain a motion element of the current vehicle relative to each sampling point, wherein the motion element comprises the pose of each track point in the motion process;
and an obstacle avoidance line generation step: constructing a structured road cost function comprising path smoothness, collision risk occurrence probability and reference line path following performance, selecting an optimal motion element by using the cost function, and determining the optimal motion element as an obstacle avoidance path aiming at the current obstacle, wherein the pose sampling step comprises the following steps of:
selecting the visual point with barrier-free relation between the current barrier and the current vehicle according to the barrier state information;
selecting a mapping point having a minimum distance relationship with the visual point from among a plurality of path discrete points constituting the reference path information;
along the direction of a mapping point normal vector, carrying out state space sampling on each mapping point according to a preset sampling step length;
and calculating the course angle and the curvature of each sampling point according to the course angle and the curvature of each mapping point, and determining the pose information of each sampling point.
2. The method of claim 1, further comprising:
the method comprises the following steps of a path detection step, curvature constraint detection is carried out on the obstacle avoidance path, collision constraint detection is carried out on each track point in the obstacle avoidance path, and if any one is unqualified, the pose sampling step is returned to for resampling; otherwise, outputting the current obstacle avoidance path.
3. The method according to claim 2, wherein in performing the collision-constraint detection, comprising:
establishing a two-dimensional vehicle body model aiming at a current vehicle, wherein the two-dimensional vehicle body model comprises a polygon approximate to the current vehicle, a circumcircle of the polygon and a plurality of enveloping circles which are overlapped and cover the polygon along the longitudinal axis direction of the current vehicle, and the radius of each enveloping circle is the same;
the method comprises the steps of utilizing the two-dimensional vehicle body model to simulate current vehicles to travel to each track point in the obstacle avoidance path, determining the position of each track point, namely the circumscribed circle and the positions of a plurality of enveloping circles, aiming at whether collision occurs to the front obstacle of the current vehicles or not by sequentially detecting each track point in the obstacle avoidance path, and marking the collided track points if the collision occurs.
4. The method according to claim 1, wherein when the first side sampling point and the second side sampling point corresponding to the mapping point and axisymmetric to the tangential vector direction of the mapping point are collected at the same time, the coordinates of the pair of sampling points are determined by using the following expression:
[x n ,y n ]·τ＝0
||X n -X p ||＝c*λ
in the formula, x n 、y n Respectively representing the coordinate values of the sampling points, tau representing the normal vector of the current mapping point, X p Representing the coordinates of the current mapping point, λ representing the sampling step length, c representing the sampling order, and further, calculating the curvatures of the first and second side sample points using the following expressions:
wherein, κ p Representing the curvature of the current mapped point.
5. The method of claim 4,
when k is p Is positive, κ n1 Represents the curvature of the first side sample point, κ n2 Representing a curvature of the second side sample point;
when k is p When it is negative, κ n1 Represents the curvature of the second side sampling point, k n2 Representing the curvature of the first side sample point.
6. The method according to claim 4, characterized in that the distribution positions of the first side sample point and the second side sample point are determined based on positive and negative states of a wheel rotation angle of the current vehicle at the map point, wherein,
when the wheel rotation angle is positive, the first side sampling point is positioned on the left side of the direction of the mapping point tangent vector, and the second side sampling point is positioned on the right side of the direction of the mapping point tangent vector;
when the wheel rotation angle is negative, the first side sampling point is located on the right side of the reflection point tangent vector direction, and the second side sampling point is located on the left side of the reflection point tangent vector direction.
7. The method according to any one of claims 1 to 6, wherein the motion primitive generation step comprises:
establishing a motion element solving constraint equation aiming at each sampling point by utilizing a polynomial spiral curve, wherein the motion element solving equation comprises a curvature equation, a course angle equation and a motion position equation;
determining an unknown solution vector of the equation according to the motion primitive solution constraint equation, constructing a motion state vector representing each track point in the motion process in the current motion primitive solving result, and further establishing a motion state equation related to the unknown solution vector by taking a current sampling point as an end point state;
setting initial conditions and termination conditions of the motion state equation, and solving the motion state equation by adopting a Jacobian matrix recursion mode by using preset disturbance vectors to obtain the motion element formed by the solved unknown solution vectors.
8. The method according to any one of claims 1 to 6, wherein in the obstacle avoidance line generating step, the cost function is expressed by an expression:
f＝w smooth f smooth +w risk f risk +w ref f ref
where f denotes the cost value for the current motion primitive, f smooth Representing a path smoothness cost value, f risk Representing a cost value of collision risk, f ref Represents a reference line following cost value, w smooth 、w risk 、w ref And respectively representing the weight value of the path smoothness cost item, the weight value of the collision risk cost item and the weight value of the reference line following cost item.
9. The method of claim 8,
calculating the curvature square integral of the current motion element according to the curvature of each track point in the current motion element, and taking the calculation result as the path smoothness cost value;
determining the distance from each track point to the current obstacle and determining the minimum distance according to the coordinate of each track point in the current motion primitive, and then obtaining the collision risk cost value by utilizing the preset first disturbance quantity and the minimum distance;
and determining a mapping point corresponding to each track point in the current reference route according to the coordinates of each track point in the current motion primitive, and calculating the mean value of the square deviation of the motion primitive and the current reference route based on the mapping point to obtain the reference line following cost value.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010174378.9A CN113031583B (en) | 2020-03-13 | 2020-03-13 | Obstacle avoidance method for structured road |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010174378.9A CN113031583B (en) | 2020-03-13 | 2020-03-13 | Obstacle avoidance method for structured road |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113031583A CN113031583A (en) | 2021-06-25 |
| CN113031583B true CN113031583B (en) | 2022-11-25 |
Family
ID=76458574
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010174378.9A Active CN113031583B (en) | 2020-03-13 | 2020-03-13 | Obstacle avoidance method for structured road |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113031583B (en) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113463718A (en) * | 2021-06-30 | 2021-10-01 | 广西柳工机械股份有限公司 | Anti-collision control system and control method for loader |
| CN113581181B (en) * | 2021-08-04 | 2022-09-27 | 武汉理工大学 | Intelligent vehicle overtaking track planning method |
| CN113799799A (en) * | 2021-09-30 | 2021-12-17 | 中国第一汽车股份有限公司 | Security compensation method and device, storage medium and electronic equipment |
| CN114234987A (en) * | 2021-11-05 | 2022-03-25 | 河北汉光重工有限责任公司 | Self-adaptive smooth adjustment method of off-line electronic map along with dynamic track of unmanned vehicle |
| CN113741179B (en) * | 2021-11-08 | 2022-03-25 | 北京理工大学 | Heterogeneous vehicle-oriented unified motion planning method and system |
| CN113963562B (en) * | 2021-12-21 | 2022-03-29 | 北京慧拓无限科技有限公司 | Avoidance method and device for multiple vehicles in working area |
| CN114357843B (en) * | 2022-01-11 | 2023-05-12 | 中国电建集团江西省水电工程局有限公司 | Method for carrying out numerical collision experiment simulation on transportation of wind power equipment |
| CN114509086B (en) * | 2022-02-15 | 2022-11-25 | 湖南大学无锡智能控制研究院 | Optimal trajectory planning method and system for intelligent vehicle in continuous curve scene |
| CN114923496A (en) * | 2022-03-29 | 2022-08-19 | 武汉路特斯汽车有限公司 | Path planning method and device, electronic equipment and storage medium |
| CN114676939B (en) * | 2022-05-26 | 2022-09-02 | 之江实验室 | Multi-vehicle-type parameter self-adaptive reference line smoothing method and system |
| CN115127576B (en) * | 2022-09-02 | 2022-12-13 | 青岛慧拓智能机器有限公司 | Path planning method, device, chip, terminal, electronic equipment and storage medium |
| CN115903853B (en) * | 2023-01-06 | 2023-05-30 | 北京理工大学 | Safe feasible domain generation method and system based on effective barrier |
| CN116907412B (en) * | 2023-09-12 | 2023-11-17 | 农业农村部南京农业机械化研究所 | Agricultural machinery row spacing deviation detection method, device and system |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109270933A (en) * | 2018-10-11 | 2019-01-25 | 中国科学院深圳先进技术研究院 | Unmanned barrier-avoiding method, device, equipment and medium based on conic section |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10019907B2 (en) * | 2015-09-11 | 2018-07-10 | Qualcomm Incorporated | Unmanned aerial vehicle obstacle detection and avoidance |
| CN107063280B (en) * | 2017-03-24 | 2019-12-31 | 重庆邮电大学 | Intelligent vehicle path planning system and method based on control sampling |
| CN106873600A (en) * | 2017-03-31 | 2017-06-20 | 深圳市靖洲科技有限公司 | It is a kind of towards the local obstacle-avoiding route planning method without person bicycle |
| CN109726480A (en) * | 2018-12-29 | 2019-05-07 | 青岛慧拓智能机器有限公司 | A kind of system for verifying unmanned mine card related algorithm |
| CN109960261B (en) * | 2019-03-22 | 2020-07-03 | 北京理工大学 | Dynamic obstacle avoiding method based on collision detection |
| CN110766220A (en) * | 2019-10-21 | 2020-02-07 | 湖南大学 | Local path planning method for structured road |
-
2020
- 2020-03-13 CN CN202010174378.9A patent/CN113031583B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109270933A (en) * | 2018-10-11 | 2019-01-25 | 中国科学院深圳先进技术研究院 | Unmanned barrier-avoiding method, device, equipment and medium based on conic section |
Non-Patent Citations (1)
| Title |
|---|
| 基于连续动态运动基元的移动机器人路径规划;梅壮;《信息与控制》;20191231;第48卷(第4期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113031583A (en) | 2021-06-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113031583B (en) | Obstacle avoidance method for structured road | |
| Lin et al. | Vehicle trajectory prediction using LSTMs with spatial–temporal attention mechanisms | |
| CN110297494B (en) | Decision-making method and system for lane change of automatic driving vehicle based on rolling game | |
| Chu et al. | Local path planning for off-road autonomous driving with avoidance of static obstacles | |
| US20200089246A1 (en) | Systems and methods for controlling the operation of a vehicle | |
| WO2020243162A1 (en) | Methods and systems for trajectory forecasting with recurrent neural networks using inertial behavioral rollout | |
| US11938926B2 (en) | Polyline contour representations for autonomous vehicles | |
| Artunedo et al. | Motion planning approach considering localization uncertainty | |
| Xiong et al. | Application improvement of A* algorithm in intelligent vehicle trajectory planning | |
| Mouhagir et al. | Integrating safety distances with trajectory planning by modifying the occupancy grid for autonomous vehicle navigation | |
| CN114846425A (en) | Prediction and planning of mobile robots | |
| US20220063651A1 (en) | Vehicle path planning | |
| EP4160146A1 (en) | Quadtree based data structure for storing information relating to an environment of an autonomous vehicle and methods of use thereof | |
| CN113255998B (en) | Expressway unmanned vehicle formation method based on multi-agent reinforcement learning | |
| Mouhagir et al. | A markov decision process-based approach for trajectory planning with clothoid tentacles | |
| US20230118472A1 (en) | Systems and methods for vehicle motion planning | |
| CN116758741A (en) | Multi-dimensional uncertainty perception intelligent automobile collision probability prediction method | |
| CN114237256A (en) | Three-dimensional path planning and navigation method suitable for under-actuated robot | |
| Park et al. | Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles | |
| Althoff et al. | Online verification of cognitive car decisions | |
| CN115246416B (en) | Track prediction method, track prediction device, track prediction equipment and computer readable storage medium | |
| CN115230688A (en) | Obstacle trajectory prediction method, system, and computer-readable storage medium | |
| CN112286211A (en) | Environment modeling and AGV path planning method for irregular layout workshop | |
| Zhan et al. | Risk-aware lane-change trajectory planning with rollover prevention for autonomous light trucks on curved roads | |
| Zhao et al. | On-road trajectory planning of connected and automated vehicles in complex traffic settings: a hierarchical framework of trajectory refinement |
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 |