CN115127576B - Path planning method, device, chip, terminal, electronic equipment and storage medium - Google Patents
Path planning method, device, chip, terminal, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN115127576B CN115127576B CN202211068224.7A CN202211068224A CN115127576B CN 115127576 B CN115127576 B CN 115127576B CN 202211068224 A CN202211068224 A CN 202211068224A CN 115127576 B CN115127576 B CN 115127576B
- Authority
- CN
- China
- Prior art keywords
- value
- curvature
- point
- unmanned vehicle
- reference line
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
Abstract
The invention discloses a path planning method, a path planning device, a chip, a terminal, electronic equipment and a storage medium, relates to the technical field of automation, and mainly aims to realize automatic starting planning so that an unmanned vehicle automatically returns to a preset reference line to run along a track without manual intervention. The method comprises the following steps: when detecting that the comprehensive error between the unmanned vehicle and a preset reference line exceeds the limit, determining a target sampling point on the preset reference line; generating a planned driving track of the unmanned vehicle by using a sampling track planner and controlling the unmanned vehicle to move according to the planned driving track by using the current position of the unmanned vehicle as a starting point and a target sampling point as a terminal point; and when the calculated real-time comprehensive error is smaller than a preset error threshold value, controlling the unmanned vehicle to run according to the preset reference line.
Description
Technical Field
The present invention relates to the field of automation technologies, and in particular, to a method, an apparatus, a chip, a terminal, an electronic device, and a storage medium for path planning.
Background
Compared with a common road, most of the roads in the mining area are non-public roads and have no information such as road boundaries, lane lines, traffic indicator lights and the like, so that a global path is planned in the routing module of the unmanned mine card, and the unmanned mine card runs along the global path planned by the routing module under normal conditions.
However, sometimes the unmanned mine card may deviate too far from the global path when starting, so that the comprehensive error exceeds the limit, the unmanned mine card decides to stop, and cannot return to the global path to continue tracking, and the starting planning needs to be performed manually. In order to implement manual starting planning, a safe driver needs to be configured for each unmanned vehicle, which increases labor cost, and thus manual starting planning is not suitable for unmanned application scenes in mining areas. Therefore, a driving path planning method for mine vehicles is needed to ensure that the unmanned mine card can be started normally when the vehicle stops with the comprehensive error exceeding the limit, and returns to the global path planned by the routing module to run normally.
Disclosure of Invention
The invention provides a path planning method, a path planning device, a chip, a terminal, an electronic device and a storage medium, and mainly aims to realize automatic starting planning with comprehensive error exceeding limit, so that an unmanned vehicle automatically returns to a preset reference line to track and run without manual intervention, and the intelligence is better.
According to a first aspect of the present invention, there is provided a path planning method, comprising:
when detecting that the comprehensive error between the unmanned vehicle and a preset reference line exceeds the limit, determining a target sampling point on the preset reference line, wherein the preset reference line is a preset vehicle running track in the unmanned vehicle;
generating a planned driving track of the unmanned vehicle by using a sampling track planner by taking the current position of the unmanned vehicle as a starting point and the target sampling point as a terminal point, and controlling the unmanned vehicle to advance according to the planned driving track;
and calculating a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line in real time in the process that the unmanned vehicle travels according to the planned travel track, and controlling the unmanned vehicle to travel according to the preset reference line when the calculated real-time comprehensive error is smaller than a preset error threshold value.
Optionally, before determining the target sampling point on the preset reference line when detecting that the combined error between the unmanned vehicle and the preset reference line exceeds the limit, the method further includes:
acquiring position and posture information of the current position of the unmanned vehicle every other preset detection period, and determining the preset reference line preset in the unmanned vehicle;
reading a plurality of path sequence points included by the preset reference line, respectively calculating the distance between each path sequence point in the plurality of path sequence points and the current position of the unmanned vehicle, and determining a reference sequence point in the plurality of path sequence points, wherein the distance between the reference sequence point and the current position of the unmanned vehicle is smaller than other path sequence points except the reference sequence point in the plurality of path sequence points;
acquiring the sequence point attitude coordinate of the reference sequence point, and calculating the comprehensive error between the sequence point attitude coordinate and the position attitude information;
and when the comprehensive error is larger than the preset error threshold value, determining that the comprehensive error between the unmanned vehicle and a preset reference line is detected to be out of limit.
Optionally, the calculating a composite error between the sequence point pose coordinates and the position pose information includes:
reading the abscissa and the ordinate of the attitude coordinate of the sequence point, and reading the abscissa and the ordinate of the position attitude information;
calculating an abscissa difference value between the abscissa of the sequence point attitude coordinate and the abscissa of the position attitude information, and calculating an ordinate difference value between the ordinate of the sequence point attitude coordinate and the ordinate of the position attitude information;
reading a sequence point course angle of the sequence point attitude coordinate, reading a vehicle course angle of the position attitude information, and calculating a course angle difference value between the sequence point course angle and the vehicle course angle;
calculating a first square value of the horizontal coordinate difference value and a second square value of the vertical coordinate difference value, calculating a sum of the first square value and the second square value, and performing square root calculation on the sum to obtain a root value;
calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value;
and calculating the sum of the root value and the absolute value as the comprehensive error between the sequence point attitude coordinate and the position attitude information.
Optionally, after obtaining the sequence point pose coordinates of the reference sequence point and calculating a composite error between the sequence point pose coordinates and the position pose information, the method further includes:
when the comprehensive error is smaller than or equal to the preset error threshold, controlling the unmanned vehicle to run according to the preset reference line tracking, continuously acquiring position posture information of the current position of the unmanned vehicle every other preset detection period, determining the preset reference line, determining a new datum sequence point in a plurality of path sequence points included in the preset reference line again, acquiring sequence point posture coordinates of the new datum sequence point, calculating the comprehensive error between the sequence point posture coordinates of the new datum sequence point and the position posture information acquired again, and judging whether the comprehensive error is larger than the preset error threshold.
Optionally, the determining a target sampling point on the preset reference line comprises:
acquiring a preset sampling distance;
and sampling a point, with the current position of the unmanned vehicle as a reference, on the preset reference line, wherein the distance between the point and the current position meets the preset sampling distance, and taking the point as the target sampling point.
Optionally, the generating a planned driving trajectory of the unmanned vehicle by using a sampling trajectory planner includes:
according to the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point, solving a curvature coefficient corresponding to a curvature function about the curve arc length in the sampling track planner by using a Newton root-finding method;
and determining a driving curve between the current position of the unmanned vehicle and the target sampling point, which meets vehicle kinematic constraints, based on the curvature coefficient, and taking the driving curve as a planned driving track of the unmanned vehicle.
Optionally, the solving, according to the abscissa, the ordinate, the heading angle, and the curvature value corresponding to the target sampling point, a curvature coefficient corresponding to a curvature function about a curve arc length in the sampling trajectory planner by using a newton root-finding method includes:
determining a curvature coefficient function with respect to a start curvature value, a first curvature, a second curvature, an end curvature value, and a curve arc length based on a start curvature value of the unmanned vehicle at the current location, an end curvature value at the target sampling point, and the curvature function with respect to a curve arc length, wherein the first curvature and the second curvature are curvatures at 1/3 curve arc length and 2/3 curve arc length, respectively;
determining an abscissa function, an ordinate function, and a course angle function with respect to a start curvature value, a first curvature, a second curvature, a curve arc length, and an end curvature value, respectively, based on the curvature coefficient function and the sampling trajectory planner;
acquiring a first initial value, a second initial value and a third initial value corresponding to a first curvature, a second curvature and a curve arc length respectively, and substituting the first initial value, the second initial value, the third initial value, the starting point curvature value and the ending point curvature value into the abscissa function, the ordinate function, the course angle function and the curvature coefficient function respectively to obtain an abscissa, an ordinate, a course angle and a curvature value corresponding to a target insertion point between the current position of the obstacle vehicle and the target sampling point;
correspondingly subtracting the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point from the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point to obtain an abscissa difference value, an ordinate difference value, a course angle difference value and a curvature difference value between the target sampling point and the target insertion point, and calculating the difference sum of the abscissa difference value, the ordinate difference value, the course angle difference value and the curvature difference value;
continuously updating and iterating the first initial value, the second initial value and the third initial value based on the sum of the difference values until the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point are respectively the same as the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point, and outputting a first curvature value, a second curvature value and a curve arc length after final iteration updating;
and determining the curvature coefficient according to the first curvature value, the second curvature value, the curve arc length and the curvature coefficient function after the final iteration update.
Optionally, the continuously updating and iterating the first initial value, the second initial value, and the third initial value based on the sum of the difference values until an abscissa, an ordinate, a heading angle, and a curvature value corresponding to a target insertion point are respectively the same as an abscissa, an ordinate, a heading angle, and a curvature value corresponding to the target sampling point, and outputting a finally iteratively updated first curvature value, second curvature value, and curve arc length includes:
calculating Jacobian matrixes respectively corresponding to the first curvature, the second curvature and the curve arc length based on the abscissa function, the ordinate function and the course angle function;
calculating inverse matrixes of Jacobian matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively, and multiplying the inverse matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively with the sum of the difference values to obtain a first curvature variation, a second curvature variation and a curve arc length variation of the iteration;
updating the first initial value, the second initial value and the third initial value respectively based on the first curvature variation, the second curvature variation and the curve arc length variation;
and repeating the iterative updating process of the first initial value, the second initial value and the third initial value until the sum of the difference values is 0, and outputting a first curvature value, a second curvature value and a curve arc length after final iterative updating.
Optionally, the calculating a real-time composite error between the real-time position of the unmanned vehicle and the preset reference line in real time includes:
sampling the preset reference line in real time to obtain a plurality of sampling points, and respectively calculating the Euclidean distance between each sampling point in the plurality of sampling points and the real-time position;
extracting sampling points with Euclidean distances smaller than a preset distance threshold value from the plurality of sampling points as evaluation points, and acquiring real-time position attitude information of real-time positions and evaluation point attitude coordinates of the evaluation points in real time;
and calculating a comprehensive error between the attitude coordinate of the evaluation point and the attitude information of the real-time position as a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line.
Optionally, the calculating the euclidean distance between each of the plurality of sampling points and the real-time location separately includes:
for each sampling point in the plurality of sampling points, acquiring the abscissa and the ordinate of the sampling point;
reading the horizontal coordinate and the vertical coordinate of the real-time position;
calculating a horizontal coordinate difference value of the horizontal coordinate of the sampling point and the horizontal coordinate of the real-time position, and calculating a third square value of the horizontal coordinate difference value;
calculating a vertical coordinate difference value of a vertical coordinate of the sampling point and a vertical coordinate of the real-time position, and calculating a fourth square value of the vertical coordinate difference value;
and calculating the sum of the third square value and the fourth square value, carrying out square root calculation on the sum, and taking the obtained root value as the Euclidean distance between the currently calculated sampling point and the real-time position.
Optionally, the calculating a composite error between the evaluation point attitude coordinate and the real-time position attitude information as a real-time composite error between the real-time position of the unmanned vehicle and the preset reference line includes:
reading the abscissa and the ordinate of the attitude coordinate of the evaluation point, and reading the abscissa and the ordinate of the attitude information of the real-time position;
calculating an abscissa difference value between the abscissa of the attitude coordinate of the evaluation point and the abscissa of the attitude information of the real-time position, and calculating an ordinate difference value between the ordinate of the attitude coordinate of the evaluation point and the ordinate of the attitude information of the real-time position;
reading an evaluation point course angle of the evaluation point attitude coordinate, reading a real-time course angle of the real-time position attitude information, and calculating a course angle difference value between the evaluation point course angle and the real-time course angle;
calculating a fifth square value of the horizontal coordinate difference value and a sixth square value of the vertical coordinate difference value, calculating a sum of the fifth square value and the sixth square value, and performing square root calculation on the sum to obtain a root value;
calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value;
and calculating the sum of the root value and the absolute value as the comprehensive error, and taking the comprehensive error as the real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line.
According to a second aspect of the present invention, there is provided a path planning apparatus comprising:
the device comprises a determining unit, a judging unit and a judging unit, wherein the determining unit is used for determining a target sampling point on a preset reference line when detecting that the comprehensive error between an unmanned vehicle and the preset reference line exceeds the limit, and the preset reference line is a preset vehicle running track in the unmanned vehicle;
the generating unit is used for generating a planned running track of the unmanned vehicle by adopting a sampling track planner by taking the current position of the unmanned vehicle as a starting point and the target sampling point as an end point, and controlling the unmanned vehicle to move according to the planned running track;
and the calculating unit is used for calculating a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line in real time in the process that the unmanned vehicle advances according to the planned driving track, and controlling the unmanned vehicle to drive according to the preset reference line tracking when the calculated real-time comprehensive error is smaller than a preset error threshold value.
Optionally, the calculation unit is further configured to acquire position and posture information of a current position of the unmanned vehicle every preset detection period, and determine the preset reference line preset in the unmanned vehicle; reading a plurality of path sequence points included by the preset reference line, respectively calculating the distance between each path sequence point in the plurality of path sequence points and the current position of the unmanned vehicle, and determining a reference sequence point in the plurality of path sequence points, wherein the distance between the reference sequence point and the current position of the unmanned vehicle is smaller than other path sequence points except the reference sequence point in the plurality of path sequence points; acquiring the sequence point attitude coordinate of the reference sequence point, and calculating the comprehensive error between the sequence point attitude coordinate and the position attitude information; and when the comprehensive error is larger than the preset error threshold value, determining that the comprehensive error between the unmanned vehicle and a preset reference line is detected to be out of limit.
Optionally, the computing unit is configured to read an abscissa and an ordinate of the sequence point posture coordinate, and read an abscissa and an ordinate of the position posture information; calculating an abscissa difference value between the abscissa of the sequence point attitude coordinate and the abscissa of the position attitude information, and calculating an ordinate difference value between the ordinate of the sequence point attitude coordinate and the ordinate of the position attitude information; reading a sequence point course angle of the sequence point attitude coordinate, reading a vehicle course angle of the position attitude information, and calculating a course angle difference between the sequence point course angle and the vehicle course angle; calculating a first square value of the horizontal coordinate difference value and a second square value of the vertical coordinate difference value, calculating a sum of the first square value and the second square value, and performing square root calculation on the sum to obtain a root value; calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value; and calculating the sum of the root value and the absolute value as the comprehensive error between the sequence point attitude coordinate and the position attitude information.
Optionally, the calculating unit is further configured to, when the composite error is less than or equal to the preset error threshold, control the unmanned vehicle to travel along the track of the preset reference line, and continue to acquire the position and posture information of the current position of the unmanned vehicle every other preset detection period, determine the preset reference line, determine a new reference sequence point again in the multiple path sequence points included in the preset reference line, acquire the sequence point posture coordinates of the new reference sequence point, calculate a composite error between the sequence point posture coordinates of the new reference sequence point and the position and posture information acquired again, and determine whether the composite error is greater than the preset error threshold.
Optionally, the determining unit is configured to obtain a preset sampling distance; and sampling a point, with the current position of the unmanned vehicle as a reference, on the preset reference line, wherein the distance between the point and the current position meets the preset sampling distance, and taking the point as the target sampling point.
Optionally, the generating unit is configured to solve, according to an abscissa, an ordinate, a course angle, and a curvature value corresponding to the target sampling point, a curvature coefficient corresponding to a curvature function about a curve arc length in the sampling trajectory planner by using a newton root method; and determining a driving curve meeting vehicle kinematic constraints between the current position of the unmanned vehicle and the target sampling points on the basis of the curvature coefficient, and taking the driving curve as a planned driving track of the unmanned vehicle.
Optionally, the generating unit is configured to determine a curvature coefficient function with respect to a first curvature, a second curvature and a curve arc length according to a start point curvature value of the unmanned vehicle at the current position, an end point curvature value at the target sampling point, and the curvature function with respect to the curve arc length, wherein the first curvature and the second curvature are curvatures at 1/3 curve arc length and 2/3 curve arc length, respectively; determining an abscissa function, an ordinate function, and a course angle function with respect to a start curvature value, a first curvature, a second curvature, a curve arc length, and an end curvature value, respectively, based on the curvature coefficient function and the sampling trajectory planner; acquiring a first initial value, a second initial value and a third initial value corresponding to a first curvature, a second curvature and a curve arc length respectively, and substituting the first initial value, the second initial value, the third initial value, the starting point curvature value and the finishing point curvature value into the abscissa function, the ordinate function, the course angle function and the curvature coefficient function respectively to obtain an abscissa, an ordinate, a course angle and a curvature value corresponding to a target insertion point between the current position of the obstacle vehicle and the target sampling point; correspondingly subtracting the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point from the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point to obtain an abscissa difference value, an ordinate difference value, a course angle difference value and a curvature difference value between the target sampling point and the target insertion point, and calculating the difference sum of the abscissa difference value, the ordinate difference value, the course angle difference value and the curvature difference value; continuously updating and iterating the first initial value, the second initial value and the third initial value based on the difference sum until the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point are respectively the same as the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point, and outputting a first curvature value, a second curvature value and a curve arc length after final iteration updating; and determining the curvature coefficient according to the first curvature value, the second curvature value, the curve arc length and the curvature coefficient function after the final iteration updating.
Optionally, the generating unit is configured to calculate, based on the abscissa function, the ordinate function, and the heading angle function, jacobian matrices corresponding to the first curvature, the second curvature, and the curve arc length respectively; calculating inverse matrixes of Jacobian matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively, and multiplying the inverse matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively by the difference sum to obtain a first curvature variation, a second curvature variation and a curve arc length variation of the iteration; updating the first initial value, the second initial value and the third initial value respectively based on the first curvature variation, the second curvature variation and the curve arc length variation; and repeating the iterative updating process of the first initial value, the second initial value and the third initial value until the sum of the difference values is 0, and outputting the finally iteratively updated first curvature value, second curvature value and curve arc length.
Optionally, the calculating unit is configured to sample the preset reference line in real time to obtain a plurality of sampling points, and calculate an euclidean distance between each sampling point of the plurality of sampling points and the real-time position; extracting sampling points with Euclidean distances smaller than a preset distance threshold value from the plurality of sampling points as evaluation points, and acquiring real-time position attitude information of real-time positions and evaluation point attitude coordinates of the evaluation points in real time; and calculating a comprehensive error between the attitude coordinate of the evaluation point and the attitude information of the real-time position as a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line.
Optionally, the calculating unit is configured to obtain, for each of the plurality of sampling points, an abscissa and an ordinate of the sampling point; reading the horizontal coordinate and the vertical coordinate of the real-time position; calculating a horizontal coordinate difference value of the horizontal coordinate of the sampling point and the horizontal coordinate of the real-time position, and calculating a third square value of the horizontal coordinate difference value; calculating a vertical coordinate difference value of a vertical coordinate of the sampling point and a vertical coordinate of the real-time position, and calculating a fourth square value of the vertical coordinate difference value; and calculating the sum of the third square value and the fourth square value, carrying out square root calculation on the sum, and taking the obtained root value as the Euclidean distance between the currently calculated sampling point and the real-time position.
Optionally, the computing unit is configured to read an abscissa and an ordinate of the attitude coordinate of the evaluation point, and read an abscissa and an ordinate of the real-time position attitude information; calculating an abscissa difference value between the abscissa of the attitude coordinate of the evaluation point and the abscissa of the attitude information of the real-time position, and calculating an ordinate difference value between the ordinate of the attitude coordinate of the evaluation point and the ordinate of the attitude information of the real-time position; reading an evaluation point course angle of the evaluation point attitude coordinate, reading a real-time course angle of the real-time position attitude information, and calculating a course angle difference value between the evaluation point course angle and the real-time course angle; calculating a fifth square value of the horizontal coordinate difference value and a sixth square value of the vertical coordinate difference value, calculating a sum of the fifth square value and the sixth square value, and performing square root calculation on the sum to obtain a root value; calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value; and calculating a sum of the root value and the absolute value as the composite error, and taking the composite error as a real-time composite error between the real-time position of the unmanned vehicle and the preset reference line.
According to a third aspect of the present invention, there is provided a chip comprising at least one chip processor and a communication interface, the communication interface being coupled to the at least one chip processor, the at least one chip processor being configured to execute a computer program or instructions to implement the path planning method according to any of the first aspects described above.
According to a fourth aspect of the present invention, there is provided a terminal comprising the path planning apparatus according to the second aspect.
According to a fifth aspect of the present invention, there is provided an electronic device comprising a device memory, a device processor and a computer program stored on the device memory and executable on the device processor, the computer program, when executed by the device processor, implementing the steps of the method of any one of the first aspects.
According to a sixth aspect of the present invention, there is provided a storage medium having stored thereon a computer program which, when executed by a media processor, carries out the steps of the method of any one of the first aspects.
Compared with the current manual starting planning mode, the path planning method, the device, the chip, the terminal, the electronic device and the storage medium provided by the invention have the advantages that whether the comprehensive error between the vehicle and the preset reference line exceeds the limit or not is periodically judged, if the comprehensive error exceeds the limit, the target sampling point is determined on the preset reference line to serve as the terminal point, starting path planning is started, the planned driving track is generated for the unmanned vehicle, the unmanned vehicle is controlled to advance according to the planned driving track, the comprehensive error between the unmanned vehicle and the preset reference line is calculated in real time in the advancing process, once the real-time comprehensive error is smaller than the preset error threshold value, the unmanned vehicle is determined to return to the preset reference line, the unmanned vehicle is controlled to travel according to the preset reference line, automatic starting planning with the comprehensive error exceeding the limit is realized, the unmanned vehicle automatically returns to the preset reference line to travel along the track, manual intervention is not needed, and the intelligence is better.
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 invention and not to limit the invention. In the drawings:
fig. 1 shows a flow chart of a path planning method according to an embodiment of the present invention;
fig. 2 shows a flowchart of another path planning method provided in the embodiment of the present invention;
fig. 3 is an interaction diagram illustrating a further path planning method according to an embodiment of the present invention;
fig. 4 is a schematic structural diagram illustrating a path planning according to an embodiment of the present invention;
FIG. 5 illustrates a storage medium provided by an embodiment of the present invention;
fig. 6 is a schematic physical structure diagram of an electronic device according to an embodiment of the present invention;
FIG. 7 is a schematic diagram illustrating a chip according to an embodiment of the present invention;
fig. 8 shows a schematic structural diagram of a terminal according to an embodiment of the present invention.
Detailed Description
The invention will be described in detail hereinafter with reference to the accompanying drawings in conjunction with embodiments. It should be noted that the embodiments and features of the embodiments of the present invention may be combined with each other without conflict.
Example one
At present, the unmanned mine card possibly deviates from the global path too far during starting, so that the comprehensive error exceeds the limit, the unmanned mine card decides to stop, the unmanned mine card cannot return to the global path to continue tracking, and starting planning needs to be carried out manually.
In order to solve the above problem, an embodiment of the present invention provides a path planning method, as shown in fig. 1, the method includes:
101. and when detecting that the comprehensive error between the unmanned vehicle and the preset reference line exceeds the limit, determining a target sampling point on the preset reference line.
Wherein, unmanned vehicle includes mining haulage vehicle, and this mining haulage vehicle specifically includes: mine trucks, wide body cars, articulated mine cars, and the like. The preset reference line is a preset vehicle running track in the unmanned vehicle, the preset reference line comprises a plurality of discrete path sequence points, and the discrete path sequence points can facilitate subsequent calculation of the distance between the unmanned vehicle and the preset reference line, so that starting planning is carried out on the unmanned vehicle.
The comprehensive error between the unmanned vehicle and the preset reference line exceeds the limit actually, namely the vehicle deviates from the preset reference line more and is too far away from the preset reference line, so that the vehicle cannot normally run according to the pre-planned route, and the unmanned vehicle stops. Therefore, in the embodiment of the invention, when the comprehensive error between the unmanned vehicle and the preset reference line is detected to exceed the limit, the target sampling point is determined on the preset reference line, so that the target sampling point is taken as an end point to plan a path for the unmanned vehicle, the unmanned vehicle is enabled to run to the target sampling point, and the tracking running is returned to the preset reference line. The target sampling point may be a path sequence point sampled on a preset reference line according to a preset sampling distance, so that the unmanned vehicle can quickly return to the preset reference line to travel by using the target sampling point as a path planned by the unmanned vehicle in the following.
The embodiment of the invention is mainly applied to the scene of the driving path planning of the mine vehicles. The execution subject of the embodiment of the invention is a planning module equipped in the unmanned vehicle, and the planning module can receive the pose information of the unmanned vehicle from a positioning module of the unmanned vehicle and receive the information of a preset reference line from a routing module of the unmanned vehicle; then periodically judging whether the comprehensive error between the vehicle and a preset reference line exceeds the limit, if not, controlling the vehicle to run along the tracking of the preset reference line, and simultaneously sending the information of the tracking path sequence points to a control module by a planning module; and finally, the planning module is used for solving the real-time comprehensive error between the vehicle and the preset reference line, finishing the starting planning state and returning to the tracking state once the comprehensive error is smaller than a preset error threshold value, and controlling the unmanned vehicle to run along the track according to the preset reference line.
102. And generating a planned running track of the unmanned vehicle by using the current position of the unmanned vehicle as a starting point and the target sampling point as an end point by using a sampling track planner, and controlling the unmanned vehicle to run according to the planned running track.
In the embodiment of the invention, after the target sampling point is determined on the preset reference line, the planning module takes the current position of the unmanned vehicle as a starting point and the target sampling point as an end point, adopts the sampling track planner to generate the planned driving track of the unmanned vehicle, and controls the unmanned vehicle to move according to the planned driving track so as to enable the unmanned vehicle to return to the preset reference line for driving.
The current position can be obtained by positioning the vehicle by a positioning module in the unmanned vehicle; specifically, the current position may be described by using position and attitude information of (x, y, z, θ, k, v), where (x, y, z) represents a current position coordinate of the unmanned vehicle, θ is a heading angle, k is a curvature value, and v is a speed, so that the position and attitude information can reflect the position and attitude and the speed of the host vehicle at the same time.
103. And when the calculated real-time comprehensive error is smaller than a preset error threshold value, controlling the unmanned vehicle to run according to the preset reference line.
In the embodiment of the invention, in the process that the unmanned vehicle travels according to the planned travel track, the planning module can continuously acquire the real-time position of the vehicle and calculate the real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line in real time so as to judge whether the vehicle returns to the preset reference line to travel. The real-time composite error is similar to the composite error described in step 101, and is also used for describing the deviation degree between the current vehicle and the preset reference line, so that when the calculated real-time composite error is smaller than the preset error threshold, it indicates that the unmanned vehicle is actually very close to the preset reference line, and it can be basically determined that the unmanned vehicle has returned to the preset reference line, and the unmanned vehicle starts to be controlled to travel along the track of the preset reference line.
The preset error threshold may be dynamically set according to the requirements of a scene, precision, or size of the unmanned vehicle, for example, may be 5 meters, 3 meters, 1 meter, and the like. Taking the preset error threshold value as 5 meters as an example, when the calculated real-time comprehensive error between the unmanned vehicle and the preset reference line is 3 meters, it is determined that the real-time comprehensive error is smaller than the preset error threshold value, and it can be determined that the unmanned vehicle has returned to the preset reference line, and the current process is ended, otherwise, it is required to continue calculating the real-time comprehensive error value between the unmanned vehicle and the preset reference line, and the current process is ended until the calculated real-time comprehensive error value is smaller than 5 meters. It should be noted that, for some vehicles with a larger size, the floor space is larger, so the preset error threshold may be appropriately increased, and conversely, for some vehicles with a smaller size, the preset error threshold may be appropriately decreased, and the present invention does not limit the specific value of the preset error threshold.
Compared with the current manual starting planning mode, the path planning method provided by the embodiment of the invention periodically judges whether the comprehensive error between the vehicle and the preset reference line exceeds the limit, if the comprehensive error exceeds the limit, the target sampling point is determined on the preset reference line as the terminal point, starting path planning is carried out, a planned driving track is generated for the unmanned vehicle, the unmanned vehicle is controlled to travel according to the planned driving track, the comprehensive error between the unmanned vehicle and the preset reference line is calculated in real time in the traveling process, once the real-time comprehensive error is smaller than the preset error threshold value, the unmanned vehicle is determined to return to the preset reference line and is controlled to travel according to the preset reference line, the automatic starting planning with the comprehensive error exceeding the limit is realized, the unmanned vehicle automatically travels back to the preset reference line along the track without manual intervention, and the intelligence is better.
Example two
Further, in order to better explain the above process of planning the driving path of the mine vehicle, as a refinement and an extension of the above embodiment, an embodiment of the present invention provides another path planning method, as shown in fig. 2, the method includes:
201. and acquiring position and posture information of the current position of the unmanned vehicle every other preset detection period, and determining a preset reference line preset in the unmanned vehicle.
In the embodiment of the invention, in order to start the starting planning in time when the comprehensive error of the unmanned vehicle exceeds the limit, the planning module of the unmanned vehicle periodically and circularly counts the comprehensive error, and the comprehensive error actually indicates the deviation degree between the current position of the unmanned vehicle and the preset reference line, so that the planning module acquires the position and posture information of the current position of the unmanned vehicle every preset detection period and determines the preset reference line preset in the unmanned vehicle, so as to count the comprehensive error of the vehicle based on the position and posture information and the preset reference line. The preset detection period can be set according to the actual application scene, for example, many unmanned vehicles exist in the application scene, accidents easily occur after the vehicles deviate, and the deviated vehicles need to be adjusted in time, so that the preset detection period can be set to be 1 minute, 2 minutes and the like, the duration of the preset detection period is shortened, the vehicle can be timely found to be out of limit, and the vehicle can be guided to a preset reference line as soon as possible; for example, only one unmanned vehicle exists in an application scene, and other unmanned vehicles do not exist in the scene, the unmanned vehicle is only used for executing peripheral patrol tasks, the preset detection period can be set to 10 minutes, 15 minutes and the like, although the unmanned vehicle deviates from a preset reference line, the condition of the patrol tasks is still met, the duration of the preset detection period can be properly increased, and the problem that a large amount of calculation power is wasted on calculating whether the vehicle exceeds the limit is avoided.
The unmanned vehicle is provided with a positioning module, and the positioning module can provide position and posture information of the vehicle for the planning module; in addition, a routing module is further configured in the unmanned vehicle, the routing module is responsible for interacting with a base station for controlling the unmanned vehicle, the base station sends a preset reference line to the routing module through a wired or wireless network, the routing module provides the preset reference line to the planning module, so that the planning module controls the self vehicle to run along a track according to the preset reference line and start and plan, and the specific contents of the position and posture information and the preset reference line are as described in the above steps 101 and 102, and are not described again here.
202. Reading a plurality of path sequence points included in a preset reference line, respectively calculating the distance between each path sequence point in the plurality of path sequence points and the current position of the unmanned vehicle, and determining a reference sequence point in the plurality of path sequence points.
In the embodiment of the present invention, when the composite error between the host vehicle and the preset reference line is counted, a path sequence point closest to the host vehicle needs to be found from the plurality of path sequence points included in the preset reference line, and an offset between the host vehicle and the path sequence point is calculated as the composite error.
The distance between the reference sequence point and the current position of the unmanned vehicle is smaller than the distance between other path sequence points except the reference sequence point in the plurality of path sequence points. In an optional embodiment, the planning module may read a plurality of path sequence points included in the preset reference line, respectively calculate a distance between each of the plurality of path sequence points and the current position of the unmanned vehicle, and determine a reference sequence point among the plurality of path sequence points. In the practical application process, the position and posture information of the current position of the unmanned vehicle, which is acquired by the planning module, includes horizontal and vertical coordinates, so that when the distance between each path sequence point and the current position of the unmanned vehicle is calculated, the distance between the horizontal and vertical coordinates of each path sequence point and the horizontal and vertical coordinates of the vehicle can be calculated in a two-dimensional coordinate system, and a path sequence point with the minimum distance is selected as the reference sequence point.
203. Acquiring the sequence point attitude coordinate of a reference sequence point, calculating the comprehensive error between the sequence point attitude coordinate and the position attitude information, and executing the following step 204 when the comprehensive error is less than or equal to a preset error threshold; when the composite error is greater than the preset error threshold, the following steps 205 to 208 are performed.
In the embodiment of the invention, after the reference sequence point is determined, the comprehensive error between the current position of the vehicle and the reference sequence point is calculated, and the calculated comprehensive error represents the comprehensive error between the unmanned vehicle and the preset reference line. In an alternative embodiment, the process of calculating the composite error is as follows:
firstly, reading the abscissa and the ordinate of the attitude coordinate of the sequence point, and reading the abscissa and the ordinate of the position attitude information. Then, an abscissa difference value between the abscissa of the sequence point attitude coordinate and the abscissa of the position attitude information is calculated, and an ordinate difference value between the ordinate of the sequence point attitude coordinate and the ordinate of the position attitude information is calculated. And then, reading the sequence point course angle of the sequence point attitude coordinate, reading the vehicle course angle of the position attitude information, and calculating the course angle difference between the sequence point course angle and the vehicle course angle. And finally, calculating a first square value of the transverse coordinate difference value and a second square value of the longitudinal coordinate difference value, calculating a sum of the first square value and the second square value, carrying out square root calculation on the sum to obtain a root value, calculating a sine value of the course angle difference value, acquiring an absolute value of the sine value, and calculating a sum of the root value and the absolute value to be used as a comprehensive error between the sequence point attitude coordinate and the position attitude information. The process of calculating the composite error can be realized by the following formula 1:
equation 1:
wherein E represents the composite error, x n Is the abscissa of the sequence point attitude coordinate, x is the abscissa of the position attitude information, y n Is the ordinate of the sequence point attitude coordinate, y is the ordinate of the position point attitude information, theta n For sequence point heading angle, θ is vehicle heading angle, sin () represents sine function, fabs () represents absolute value function.
After the comprehensive error is obtained through calculation, a preset error threshold value is set in the planning module in advance, and the planning module can compare the comprehensive error obtained through calculation with the preset error threshold value, so that whether the comprehensive error of the unmanned vehicle exceeds the limit or not is judged. When the composite error is less than or equal to the preset error threshold, it indicates that the composite error of the unmanned vehicle does not exceed the limit, the unmanned vehicle can normally advance according to the preset reference line, and the unmanned vehicle can normally advance, so the following step 204 is executed; and when the comprehensive error is greater than the preset error threshold, it indicates that the deviation between the unmanned vehicle and the preset reference line is large, the vehicle is far away from the preset reference line, the comprehensive error exceeds the limit, and the starting planning needs to be performed for the unmanned vehicle, so the following steps 205 to 208 are performed.
204. And when the comprehensive error is less than or equal to the preset error threshold value, controlling the unmanned vehicle to run according to the preset reference line tracking.
In the embodiment of the invention, when the comprehensive error is less than or equal to the preset error threshold value, the unmanned vehicle is indicated to not generate comprehensive error overrun, the unmanned vehicle can normally advance according to the preset reference line tracking, and the unmanned vehicle only needs to normally advance. And because the comprehensive error overrun detection of the planning module is carried out in real time or in a circulating manner, the planning module can continuously obtain the position attitude information of the current position of the unmanned vehicle again every other preset detection period, determine a preset reference line, determine a new datum sequence point again in a plurality of path sequence points included by the preset reference line, obtain the sequence point attitude coordinate of the new datum sequence point, calculate the comprehensive error between the sequence point attitude coordinate of the new datum sequence point and the newly obtained position attitude information, and judge whether the comprehensive error is greater than a preset error threshold value, so that the comprehensive error overrun parking of the unmanned vehicle can be sensed in time and a response can be made in time.
205. And when the comprehensive error is larger than a preset error threshold value, determining that the comprehensive error between the unmanned vehicle and a preset reference line is detected to be out of limit, and determining a target sampling point on the preset reference line.
In the embodiment of the invention, when the comprehensive error is greater than the preset error threshold, the deviation between the unmanned vehicle and the preset reference line is large, the vehicle is far away from the preset reference line, the comprehensive error exceeds the limit, and starting planning needs to be carried out on the unmanned vehicle.
And in order to determine the target sampling point by sampling, a preset sampling distance is set in the planning module. In an optional embodiment, the planning module obtains a preset sampling distance, and samples a point, with the current position of the unmanned vehicle as a reference, on a preset reference line, where the distance between the preset sampling distance and the current position meets the preset sampling distance as a target sampling point.
206. And generating a planned driving track of the unmanned vehicle by using the sampling track planner and controlling the unmanned vehicle to move according to the planned driving track by using the current position of the unmanned vehicle as a starting point and the target sampling point as an end point.
In the embodiment of the invention, after the target sampling point is determined, the planning module generates the planned driving track of the unmanned vehicle by using the current position of the unmanned vehicle as a starting point and the target sampling point as an end point and adopting the sampling track planner, and controls the unmanned vehicle to move according to the planned driving track. In an alternative embodiment, the process of generating the planned driving trajectory is as follows:
firstly, according to an abscissa, an ordinate, a course angle and curvature values corresponding to a target sampling point, solving a curvature coefficient corresponding to a curvature function about a curve arc length in a sampling track planner by using a Newton root method; then, a driving curve satisfying vehicle kinematic constraints between the current position of the unmanned vehicle and the target sampling points is determined based on the curvature coefficient, and the driving curve is used as a planned driving track of the unmanned vehicle. The sampling track planner adopts a cubic polynomial spiral line to sample and generate a starting planning path, namely the planning running track is substantially the cubic polynomial spiral line, and a curvature function of the curve arc length of the planning running track is as follows formula 2:
equation 2:
wherein, the k(s) curvature function is a cubic polynomial function about the curve arc length, s is the curve arc length, and a, b, c and d are unknown curvature coefficients needing to be solved. Therefore, as long as the curvature coefficients a, b, c and d are determined through the abscissa, the ordinate, the course angle and the curvature value of the target sampling point, a driving curve can be established as a planned driving track by taking the current position of the unmanned vehicle as a starting point and the target sampling point as an end point.
Further, the sampling trajectory planner includes an abscissa function, an ordinate function, a heading angle function, and a curvature function with respect to the arc length of the curve, which are specifically shown in the following formula 3:
equation 3:
where x(s) is an abscissa function with respect to the arc length of the curve, y(s) is an ordinate function with respect to the arc length of the curve, and θ(s) is a course angle function with respect to the arc length of the curve. Since the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point and the abscissa, the ordinate, the course angle and the curvature value corresponding to the current position of the unmanned vehicle are known and are substituted into the formula 3 to solve the equation, the curvature coefficients a, b, c and d can be determined, but the curvature coefficients a, b, c and d are difficult to directly solve due to the fact that the expression comprises Fresnel integral, the curvature coefficients a, b, c and d are difficult to directly solve, and therefore the curvature coefficients a, b, c and d of the cubic polynomial are solved by the Newton root method in the embodiment of the invention.
Further, for a specific process of solving curvature coefficients by using a newton root method, as an optional embodiment, the method includes: determining a curvature coefficient function with respect to a starting point curvature value, a first curvature, a second curvature, an end point curvature value and a curve arc length according to a starting point curvature value of the obstacle vehicle at the current position, an end point curvature value at the target sampling point and a curvature function with respect to the curve arc length, wherein the first curvature and the second curvature are respectively a curvature at a 1/3 curve arc length and a curvature at a 2/3 curve arc length; determining an abscissa function, an ordinate function, and a course angle function with respect to a start curvature value, a first curvature, a second curvature, a curve arc length, and an end curvature value, respectively, based on the curvature coefficient function and the sampling trajectory planner; acquiring a first initial value, a second initial value and a third initial value corresponding to a first curvature, a second curvature and a curve arc length respectively, and substituting the first initial value, the second initial value, the third initial value, the starting point curvature value and the ending point curvature value into the abscissa function, the ordinate function, the course angle function and the curvature coefficient function respectively to obtain an abscissa, an ordinate, a course angle and a curvature value corresponding to a target insertion point between the current position of the obstacle vehicle and the target sampling point; correspondingly subtracting the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point from the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point to obtain an abscissa difference value, an ordinate difference value, a course angle difference value and a curvature difference value between the target sampling point and the target insertion point, and calculating a difference sum of the abscissa difference value, the ordinate difference value, the course angle difference value and the curvature difference value; continuously updating and iterating the first initial value, the second initial value and the third initial value based on the sum of the difference values until the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point are respectively the same as the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point, and outputting a first curvature value, a second curvature value and a curve arc length after final iteration updating; and determining the curvature coefficient according to the first curvature value, the second curvature value, the curve arc length and the curvature coefficient function after the final iteration updating.
Further, the continuously updating and iterating the first initial value, the second initial value, and the third initial value based on the sum of the difference values until the abscissa, the ordinate, the course angle, and the curvature value corresponding to the target insertion point are respectively the same as the abscissa, the ordinate, the course angle, and the curvature value corresponding to the target sampling point, and outputting the finally iteratively updated first curvature value, second curvature value, and curve arc length includes: calculating Jacobian matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively based on the abscissa function, the ordinate function and the course angle function; calculating inverse matrixes of the Jacobian matrix corresponding to the first curvature, the second curvature and the curve arc length respectively, and multiplying the inverse matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively by the difference sum to obtain a first curvature variation, a second curvature variation and a curve arc length variation of the iteration; updating the first initial value, the second initial value and the third initial value respectively based on the first curvature variation, the second curvature variation and the curve arc length variation; and repeating the iterative updating process of the first initial value, the second initial value and the third initial value until the sum of the difference values is 0, and outputting a first curvature value, a second curvature value and a curve arc length after final iterative updating.
Specifically, assume a total curve arc length ofS f The curvature at the starting position 0 of the arc length of the curve isP 0 The curvature at the arc of the third curve isP 1 The curvature at the arc of the two-thirds curve isP 2 The curvature at the end of the arc length of the curve is P 3 From this, an expression as shown in equation 4 can be obtained:
equation 4:
further, the following formula 5 can be obtained by combining formula 2 and formula 4:
equation 5:
wherein, P 0 Is the curvature at the current position of the unmanned vehicle, i.e. the curvature at the curve arc length starting position 0; p 3 Is the curvature at the target sampling point, i.e. the curvature at the end point of the arc length of the curve; p 0 And P 3 Are all known, S f 、P 1 And P 2 Is unknown, as can be seen from equation 5, as long as S is solved f 、P 1 And P 2 The curvature coefficients a, b, c, d can be determined.
Based on this, the embodiment of the present invention will solve S f 、P 1 And P 2 Is described as a root problem as described in equation 6:
equation 6:
wherein x is des Representing the abscissa, ordinate, course angle and curvature value, x, corresponding to the target sampling point p (s f ) Abscissa, ordinate, course angle and curvature values corresponding to target insertion points between the current position of the unmanned vehicle and the target sampling points, the abscissa corresponding to the target insertion points in the iterative processThe ordinate, the course angle and the curvature value can be changed constantly, when the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point are equal to the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point, the target insertion point is coincided with the target sampling point, and the iteration is stopped, specifically the following formula 7:
equation 7:
wherein x is pi (s f ) Representing the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point in the ith round of iteration process, deltax representing the difference sum of the abscissa difference, the ordinate difference, the course angle difference and the curvature difference between the target sampling point and the target insertion point, and P i Represents S in the ith iteration process f 、P 1 And P 2 Value of (A), P i+1 Represents S in the (i + 1) th iteration process f 、P 1 And P 2 Δ P represents a first curvature variation (P) 1 Amount of change), second amount of change of curvature (P) 2 Variation) and curve arc length variation (S) f Amount of change), J pi (x pi (s f ) ) represents S f 、P 1 And P 2 Respectively corresponding Jacobian matrix, J pi (x pi (s f )) -1 To represent the inverse of the Jacobian matrix, dx pi (s f ) Represents with respect to S f 、P 1 And P 2 The abscissa function, the ordinate function and the course angle function of the target vector are respectively corresponding to S f 、P 1 And P 2 The partial derivatives can be determined by simultaneous equations 5 and 3 with respect to S f 、P 1 And P 2 Abscissa function, ordinate function and course angle function of d pi As a parameter, 0.001 was set.
Particularly, the formula is utilized to solve S f 、P 1 And P 2 When the method is used, S is respectively given according to actual experience f 、P 1 And P 2 First, second and third initial values of (a), ofAnd then substituting the initial curvature coefficient into a value formula 5 to determine an initial curvature coefficient, setting S =0.1, substituting the initial curvature coefficient into a value formula 3 to obtain an abscissa, an ordinate, a navigation angle and a curvature value corresponding to the target insertion point, wherein the curve arc length S corresponding to each iteration of the target insertion point is increased by 0.1. Further, the abscissa, the ordinate, the navigation angle, and the curvature value corresponding to the target insertion point, and the abscissa, the ordinate, the navigation angle, and the curvature value corresponding to the target sampling point are substituted into formula 7 to calculate the difference sum Δ x.
Further, with respect to P, based on the abscissa function, the ordinate function and the heading angle function 1 And the first initial value, the second initial value and the third initial value, calculating P 1 The inverse of the corresponding Jacobian matrix, and apply P to the inverse of the Jacobian matrix 1 The inverse matrix of the corresponding Jacobian matrix is multiplied by the difference sum Delta x to obtain the P of the iteration of the current round 1 The variation can obtain P of the iteration in the same way 2 Amount of change and S f The amount of change. Then respectively pairing S based on the variables f 、P 1 And P 2 Is updated, thereby completing the cost wheel pair S f 、P 1 And P 2 The update iteration process of (1). Repeating the above pair S f 、P 1 And P 2 Until Deltax is 0, namely the abscissa, ordinate, course angle and curvature value corresponding to the target insertion point are the same as those corresponding to the target sampling point, stopping updating the iterative process, and obtaining the final output S f 、P 1 And P 2 And S to be finally output f 、P 1 And P 2 The value of (a) is substituted into the formula 5, so that a driving curve which meets the vehicle kinematic constraint between the current position of the unmanned vehicle and the target sampling point can be determined as a planning driving track.
According to the embodiment of the invention, the generated driving track meeting the vehicle kinematic constraint is used as the planning driving track, so that the generation precision of the planning driving track can be ensured, and powerful data support can be provided for the subsequent unmanned vehicle to return to the preset reference line.
207. And calculating a real-time comprehensive error between the real-time position of the unmanned vehicle and a preset reference line in real time in the process that the unmanned vehicle travels according to the planned travel track.
In the embodiment of the invention, after the planned driving track is planned for the unmanned vehicle, the planning module sequentially sends the path sequence points in the planned driving track to the control module, so that the control module controls the unmanned vehicle to move according to the planned driving track. Because the unmanned vehicle can be closer to the preset reference line when the unmanned vehicle travels according to the planned travel track, in order to judge whether the unmanned vehicle is on the preset reference line, the planning module can calculate a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line in real time, and judge whether the unmanned vehicle reaches the preset reference line based on the real-time comprehensive error.
When calculating the real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line, firstly, selecting a sampling point on the preset reference line as an evaluation point, calculating the comprehensive error between the unmanned vehicle and the evaluation point, and taking the comprehensive error as the real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line. In an optional embodiment, the preset reference line may be sampled in real time to obtain a plurality of sampling points, the euclidean distance between each sampling point and the real-time position in the plurality of sampling points is respectively calculated, and which sampling point is determined as the evaluation point, and a process of specifically calculating the euclidean distance between the real-time position and the sampling point is as follows: for each sampling point in the plurality of sampling points, acquiring the abscissa and the ordinate of the sampling point, and reading the abscissa and the ordinate of a real-time position; calculating a horizontal coordinate difference value of the horizontal coordinate of the sampling point and the horizontal coordinate of the real-time position, and calculating a third square value of the horizontal coordinate difference value; calculating a vertical coordinate difference value of a vertical coordinate of the sampling point and a vertical coordinate of the real-time position, and calculating a fourth square value of the vertical coordinate difference value; and calculating the sum of the third square value and the fourth square value, carrying out square root calculation on the sum, and taking the obtained root value as the Euclidean distance between the currently calculated sampling point and the real-time position. In practical application, the procedure of calculating the euclidean distance can be implemented by the following formula 8:
equation 8:
wherein D represents the calculated Euclidean distance, sqrt () represents the square root calculation function, and x 1 Abscissa, x, representing the sample point 2 Abscissa, y, representing the real-time position 1 Indicating the ordinate, y, of the sample point 2 An ordinate representing the real-time position.
After the Euclidean distance between each sampling point and the real-time position is obtained through calculation, the planning module extracts sampling points of which the Euclidean distance is smaller than a preset distance threshold value from the plurality of sampling points to serve as evaluation points, real-time position posture information of the real-time position and attitude coordinates of the evaluation points are obtained in real time, and a comprehensive error between the evaluation point posture coordinates and the real-time position posture information is calculated to serve as a real-time comprehensive error between the real-time position of the unmanned vehicle and a preset reference line. In an alternative embodiment, the process of calculating the real-time composite error is as follows: reading the abscissa and the ordinate of the attitude coordinate of the evaluation point, and reading the abscissa and the ordinate of the attitude information of the real-time position; calculating an abscissa difference value of an abscissa of the attitude coordinate of the evaluation point and an abscissa of the attitude information of the real-time position, and calculating an ordinate difference value of an ordinate of the attitude coordinate of the evaluation point and an ordinate of the attitude information of the real-time position; reading an evaluation point course angle of an evaluation point attitude coordinate, reading a real-time course angle of real-time position attitude information, and calculating a course angle difference between the evaluation point course angle and the real-time course angle; calculating a fifth square value of the horizontal coordinate difference value and a sixth square value of the vertical coordinate difference value, calculating a sum of the fifth square value and the sixth square value, and performing square root calculation on the sum to obtain a root value; calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value; and calculating the sum of the root value and the absolute value as a comprehensive error, and taking the comprehensive error as a real-time comprehensive error between the real-time position of the unmanned vehicle and a preset reference line. The process of calculating the real-time composite error is the same as the process of calculating the composite error described in step 203, and may also be calculated based on the above formula 1, which is not described herein again.
208. And when the calculated real-time comprehensive error is smaller than a preset error threshold value, controlling the unmanned vehicle to run according to the preset reference line tracking.
In the embodiment of the invention, after the real-time comprehensive error is calculated, the planning module compares the real-time comprehensive error with a preset error threshold value, and judges whether the unmanned vehicle returns to the preset reference line to run or not. If the calculated real-time comprehensive error is still larger than the preset error threshold value, it indicates that the unmanned vehicle does not return to the preset reference line to run, the unmanned vehicle continues to run according to the planned running track, and the planning module continues to calculate the real-time comprehensive error in real time until the calculated real-time comprehensive error is smaller than the preset error threshold value; and if the calculated real-time comprehensive error is smaller than the preset error threshold value, the fact that the unmanned vehicle returns to the preset reference line is indicated, the unmanned vehicle can normally run according to the preset reference line, and therefore the unmanned vehicle is controlled to run according to the preset reference line. The routing module sends the received preset reference line to the planning module, so that the planning module can send path sequence points on the preset reference line to the control module in sequence, and the control module controls the unmanned vehicle to run according to the preset reference line according to the path sequence points.
Compared with the current manual starting planning mode, the other path planning method provided by the embodiment of the invention periodically judges whether the comprehensive error between the vehicle and the preset reference line exceeds the limit, if the comprehensive error exceeds the limit, the target sampling point is determined on the preset reference line as the terminal point, starting path planning is carried out, a planned driving track is generated for the unmanned vehicle, the unmanned vehicle is controlled to travel according to the planned driving track, the comprehensive error between the unmanned vehicle and the preset reference line is calculated in real time in the traveling process, once the real-time comprehensive error is smaller than a preset error threshold value, the unmanned vehicle is determined to return to the preset reference line and is controlled to travel according to the preset reference line, the automatic starting planning with the comprehensive error exceeding the limit is realized, the unmanned vehicle automatically travels back to the preset reference line without manual intervention, and the intelligence is better.
EXAMPLE III
In order to better explain the process of the path planning, as a refinement and an extension of the above embodiment, an embodiment of the present invention provides a path planning method, as shown in fig. 3, since a positioning module, a planning module, a routing module, and a control module are disposed in an unmanned vehicle, the method involves interaction among the above mentioned modules, and the method includes:
301. the positioning module acquires position and posture information of the current position of the unmanned vehicle and sends the position and posture information to the planning module.
The specific content of the position and orientation information is as described in steps 101 and 201 above, and is not described herein again.
302. The routing module acquires a preset reference line and sends the preset reference line to the planning module.
The specific content of the preset reference line is as described in steps 102 and 201 above, and is not described herein again.
303. The planning module calculates a comprehensive error based on the received position and attitude information and a preset reference line, judges whether the comprehensive error exceeds the limit, and executes the following step 304 if the comprehensive error does not exceed the limit; if the combined error is out of limit, the following steps 305 to 306 are performed.
The process of calculating the composite error is described in step 202 to step 203, and is not described herein again.
304. If the comprehensive error is not out of limit, the planning module tracks along a preset reference line and sequentially sends the tracked path sequence points to the control module so that the control module controls the unmanned vehicle to move forward.
The situation where the combined error is not out of limit is as described in step 204 above, and is not described herein again.
305. If the comprehensive error exceeds the limit, the planning module starts to plan starting of the unmanned vehicle, generates a planned driving track, tracks along the planned driving track, and sequentially sends the tracked path sequence points to the control module, so that the control module controls the unmanned vehicle to move according to the planned driving track.
The process of generating the planned driving trajectory is described in steps 205 to 206 above, and is not described herein again.
306. The planning module repeatedly executes the processes in the steps 301 to 303, continuously calculates the real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line in real time, determines that the unmanned vehicle returns to the preset reference line when the calculated real-time comprehensive error is smaller than a preset error threshold value, tracks along the preset reference line, and sequentially sends the tracked path sequence points to the control module so that the control module controls the unmanned vehicle to move forward.
The process of continuously calculating the real-time composite error and tracking between the real-time position of the unmanned vehicle and the preset reference line in real time is the process from step 207 to step 208, and is not repeated here.
Compared with the current manual starting planning mode, the path planning method provided by the embodiment of the invention realizes the automatic starting planning with the comprehensive error exceeding limit, enables the unmanned vehicle to automatically return to the preset reference line for tracking driving, does not need manual intervention, and has better intelligence.
Example four
Further, as a specific implementation of fig. 1, an embodiment of the present invention provides a path planning apparatus, as shown in fig. 4, the apparatus includes: a determination unit 401, a generation unit 402 and a calculation unit 403.
The determining unit 401 is configured to determine a target sampling point on a preset reference line when it is detected that a comprehensive error between the unmanned vehicle and the preset reference line exceeds a limit, where the preset reference line is a preset vehicle running track in the unmanned vehicle;
the generating unit 402 is configured to generate a planned driving track of the unmanned vehicle by using a sampling track planner and controlling the unmanned vehicle to travel according to the planned driving track, where a current position of the unmanned vehicle is a starting point and the target sampling point is an end point;
the calculating unit 403 is configured to calculate a real-time composite error between the real-time position of the unmanned vehicle and the preset reference line in real time during the process that the unmanned vehicle travels according to the planned travel track, and control the unmanned vehicle to travel along the preset reference line when the calculated real-time composite error is smaller than a preset error threshold.
In a specific application scenario, the calculating unit 403 is further configured to obtain position and posture information of the current position of the unmanned vehicle every preset detection period, and determine the preset reference line preset in the unmanned vehicle; reading a plurality of path sequence points included by the preset reference line, respectively calculating the distance between each path sequence point in the plurality of path sequence points and the current position of the unmanned vehicle, and determining a reference sequence point in the plurality of path sequence points, wherein the distance between the reference sequence point and the current position of the unmanned vehicle is smaller than other path sequence points except the reference sequence point in the plurality of path sequence points; acquiring the sequence point attitude coordinate of the reference sequence point, and calculating the comprehensive error between the sequence point attitude coordinate and the position attitude information; and when the comprehensive error is larger than the preset error threshold value, determining that the comprehensive error between the unmanned vehicle and a preset reference line is detected to be out of limit.
In a specific application scenario, the calculating unit 403 is configured to read an abscissa and an ordinate of the sequence point posture coordinate, and read an abscissa and an ordinate of the position posture information; calculating an abscissa difference value between the abscissa of the sequence point attitude coordinate and the abscissa of the position attitude information, and calculating an ordinate difference value between the ordinate of the sequence point attitude coordinate and the ordinate of the position attitude information; reading a sequence point course angle of the sequence point attitude coordinate, reading a vehicle course angle of the position attitude information, and calculating a course angle difference value between the sequence point course angle and the vehicle course angle; calculating a first square value of the horizontal coordinate difference value and a second square value of the vertical coordinate difference value, calculating a sum of the first square value and the second square value, and performing square root calculation on the sum to obtain a root value; calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value; and calculating the sum of the root value and the absolute value as the comprehensive error between the sequence point attitude coordinate and the position attitude information.
In a specific application scenario, the calculating unit 403 is further configured to, when the composite error is less than or equal to the preset error threshold, control the unmanned vehicle to travel along the track of the preset reference line, continuously obtain the position and posture information of the current position of the unmanned vehicle again every other preset detection period, determine the preset reference line, determine a new reference sequence point again in the multiple path sequence points included in the preset reference line, obtain the sequence point posture coordinate of the new reference sequence point, calculate a composite error between the sequence point posture coordinate of the new reference sequence point and the newly obtained position and posture information, and determine whether the composite error is greater than the preset error threshold.
In a specific application scenario, the determining unit 401 is configured to obtain a preset sampling distance; and sampling a point, with the current position of the unmanned vehicle as a reference, on the preset reference line, wherein the distance between the point and the current position meets the preset sampling distance, and taking the point as the target sampling point.
In a specific application scenario, the generating unit 402 is configured to solve a curvature coefficient corresponding to a curvature function about a curve arc length in the sampling trajectory planner by using a newton root method according to an abscissa, an ordinate, a course angle, and a curvature value corresponding to the target sampling point; and determining a driving curve between the current position of the unmanned vehicle and the target sampling point, which meets vehicle kinematic constraints, based on the curvature coefficient, and taking the driving curve as a planned driving track of the unmanned vehicle.
In a specific application scenario, the generating unit 402 is configured to determine a curvature coefficient function regarding a start point curvature value, a first curvature, a second curvature, an end point curvature value, and a curve arc length according to a start point curvature value of the unmanned vehicle at the current location, an end point curvature value at the target sampling point, and the curvature function regarding a curve arc length, wherein the first curvature and the second curvature are curvatures at 1/3 curve arc length and 2/3 curve arc length, respectively; determining an abscissa function, an ordinate function, and a course angle function with respect to a start curvature value, a first curvature, a second curvature, a curve arc length, and an end curvature value, respectively, based on the curvature coefficient function and the sampling trajectory planner; acquiring a first initial value, a second initial value and a third initial value corresponding to a first curvature, a second curvature and a curve arc length respectively, and substituting the first initial value, the second initial value, the third initial value, the starting point curvature value and the finishing point curvature value into the abscissa function, the ordinate function, the course angle function and the curvature coefficient function respectively to obtain an abscissa, an ordinate, a course angle and a curvature value corresponding to a target insertion point between the current position of the obstacle vehicle and the target sampling point; correspondingly subtracting the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point from the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point to obtain an abscissa difference value, an ordinate difference value, a course angle difference value and a curvature difference value between the target sampling point and the target insertion point, and calculating a difference sum of the abscissa difference value, the ordinate difference value, the course angle difference value and the curvature difference value; continuously updating and iterating the first initial value, the second initial value and the third initial value based on the sum of the difference values until the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point are respectively the same as the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point, and outputting a first curvature value, a second curvature value and a curve arc length after final iteration updating; and determining the curvature coefficient according to the first curvature value, the second curvature value, the curve arc length and the curvature coefficient function after the final iteration updating.
In a specific application scenario, the generating unit 402 is configured to calculate, based on the abscissa function, the ordinate function, and the course angle function, jacobian matrices corresponding to the first curvature, the second curvature, and the curve arc length respectively; calculating inverse matrixes of Jacobian matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively, and multiplying the inverse matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively by the difference sum to obtain a first curvature variation, a second curvature variation and a curve arc length variation of the iteration; updating the first initial value, the second initial value and the third initial value respectively based on the first curvature variation, the second curvature variation and the curve arc length variation; and repeating the iterative updating process of the first initial value, the second initial value and the third initial value until the sum of the difference values is 0, and outputting the finally iteratively updated first curvature value, second curvature value and curve arc length.
In a specific application scenario, the calculating unit 403 is configured to sample the preset reference line in real time to obtain a plurality of sampling points, and calculate an euclidean distance between each sampling point of the plurality of sampling points and the real-time position; extracting sampling points with Euclidean distances smaller than a preset distance threshold value from the plurality of sampling points as evaluation points, and acquiring real-time position attitude information of real-time positions and evaluation point attitude coordinates of the evaluation points in real time; and calculating a comprehensive error between the attitude coordinate of the evaluation point and the attitude information of the real-time position as a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line.
In a specific application scenario, the calculating unit 403 is configured to, for each of the multiple sampling points, obtain an abscissa and an ordinate of the sampling point; reading the horizontal coordinate and the vertical coordinate of the real-time position; calculating a horizontal coordinate difference value of the horizontal coordinate of the sampling point and the horizontal coordinate of the real-time position, and calculating a third square value of the horizontal coordinate difference value; calculating a vertical coordinate difference value of a vertical coordinate of the sampling point and a vertical coordinate of the real-time position, and calculating a fourth square value of the vertical coordinate difference value; and calculating the sum of the third square value and the fourth square value, carrying out square root calculation on the sum, and taking the obtained root value as the Euclidean distance between the currently calculated sampling point and the real-time position.
In a specific application scenario, the calculating unit 403 is configured to read an abscissa and an ordinate of the attitude coordinate of the evaluation point, and read an abscissa and an ordinate of the real-time position attitude information; calculating an abscissa difference value between the abscissa of the attitude coordinate of the evaluation point and the abscissa of the attitude information of the real-time position, and calculating an ordinate difference value between the ordinate of the attitude coordinate of the evaluation point and the ordinate of the attitude information of the real-time position; reading an evaluation point course angle of the evaluation point attitude coordinate, reading a real-time course angle of the real-time position attitude information, and calculating a course angle difference value between the evaluation point course angle and the real-time course angle; calculating a fifth square value of the horizontal coordinate difference value and a sixth square value of the vertical coordinate difference value, calculating a sum of the fifth square value and the sixth square value, and performing square root calculation on the sum to obtain a root value; calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value; and calculating the sum of the root value and the absolute value as the comprehensive error, and taking the comprehensive error as the real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line.
It should be noted that other corresponding descriptions of the functional modules related to the path planning apparatus provided in the embodiment of the present invention may refer to corresponding descriptions of the methods shown in the first to third embodiments, and are not described herein again.
Based on the method shown in fig. 1, correspondingly, the embodiment of the present invention further provides a storage medium, as shown in fig. 5, a computer program is stored on the medium memory 720, the computer program is located in the program code space 730, and when executed by the medium processor 710, the program 731 implements the method steps shown in any one of the first embodiment, the second embodiment and the third embodiment.
EXAMPLE five
An embodiment of the present invention further provides an entity structure diagram of an electronic device, as shown in fig. 6, where the electronic device includes: a device processor 41, a device memory 42, and a computer program stored on the device memory 42 and operable on the device processor, wherein the device memory 42 and the device processor 41 are both arranged on a bus 43, and the steps shown in any one of the first to third embodiments are implemented when the program is executed by the device processor 41.
The method periodically judges whether the comprehensive error between the vehicle and a preset reference line exceeds the limit, if the comprehensive error exceeds the limit, a target sampling point is determined on the preset reference line to be used as a terminal point, starting path planning is started, a planned driving track is generated for the unmanned vehicle, the unmanned vehicle is controlled to move according to the planned driving track, the comprehensive error between the unmanned vehicle and the preset reference line is calculated in real time in the moving process, once the real-time comprehensive error is smaller than a preset error threshold value, the unmanned vehicle is determined to return to the preset reference line, the unmanned vehicle is controlled to move according to the preset reference line, automatic starting planning with the comprehensive error exceeding the limit is achieved, the unmanned vehicle automatically returns to the preset reference line to move along the track, manual intervention is not needed, and the intelligence is good.
EXAMPLE six
Fig. 7 is a schematic structural diagram of a chip according to an embodiment of the present invention, and as shown in fig. 7, the chip 500 includes one or more than two (including two) chip processors 510 and a communication interface 530. The communication interface 530 is coupled to the at least one chip processor 510, and the at least one chip processor 510 is configured to run a computer program or instructions to implement the path planning method according to any one of the first embodiment, the second embodiment, and the third embodiment.
Preferably, chip memory 540 stores the following elements: an executable module or a data structure, or a subset thereof, or an expanded set thereof.
In embodiments of the present invention, chip memory 540 may include both read-only memory and random access memory and provides instructions and data to chip processor 510. A portion of the chip memory 540 may also include non-volatile random access memory (NVRAM).
In an embodiment of the present invention, chip memory 540, communication interface 530, and chip processor 510 are coupled together by a bus system 520. The bus system 520 may include a power bus, a control bus, a status signal bus, and the like, in addition to the data bus. For ease of description, the various buses are labeled as bus system 520 in FIG. 7.
The method described in the above embodiments of the present invention may be applied to the chip processor 510, or implemented by the chip processor 510. The chip processor 510 may be an integrated circuit chip having signal processing capabilities. In implementation, the steps of the above method may be performed by hardware integrated logic circuits or instructions in software in the chip processor 510. The chip processor 510 may be a general-purpose processor (e.g., a microprocessor or a conventional processor), a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf programmable gate array (FPGA) or other programmable logic device, discrete gate, transistor logic device or discrete hardware component, and the chip processor 510 may implement or perform the methods, steps and logic blocks disclosed in the embodiments of the present invention.
EXAMPLE seven
Fig. 8 is a schematic structural diagram of a terminal according to an embodiment of the present invention, and as shown in fig. 8, the terminal 600 includes the path planning apparatus 100.
The terminal 600 may execute the method described in any of the above embodiments through the path planning apparatus 100. It can be understood that the implementation manner of the terminal 600 controlling the path planning apparatus 100 may be set according to an actual application scenario, and the embodiment of the present invention is not limited in particular.
The terminal 600 includes but is not limited to: the vehicle can implement the method provided by the invention through the vehicle-mounted terminal, the vehicle-mounted controller, the vehicle-mounted module, the vehicle-mounted component, the vehicle-mounted chip, the vehicle-mounted unit, the vehicle-mounted radar or the camera.
The terminal in the embodiment of the invention is used as a control or adjustment system for executing the non-electric variable to plan the path, so that the automatic starting planning of the comprehensive error overrun is realized, the unmanned vehicle automatically returns to the preset reference line to track and run, the manual intervention is not needed, and the intelligence is better.
It will be apparent to those skilled in the art that the modules or steps of the present invention described above may be implemented by a general purpose computing device, they may be centralized on a single computing device or distributed across a network of multiple computing devices, and alternatively, they may be implemented by program code executable by a computing device, such that they may be stored in a storage device and executed by a computing device, and in some cases, the steps shown or described may be performed in an order different than that described herein, or they may be separately fabricated into individual integrated circuit modules, or multiple ones of them may be fabricated into a single integrated circuit module. Thus, the present invention is not limited to any specific combination of hardware and software.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made without departing from the spirit and principle of the present invention shall fall within the protection scope of the present invention.
Claims (16)
1. A method of path planning, comprising:
when detecting that the comprehensive error between an unmanned vehicle and a preset reference line exceeds the limit, determining a target sampling point on the preset reference line, wherein the preset reference line is a vehicle running track preset in the unmanned vehicle, when detecting whether the comprehensive error between the unmanned vehicle and the preset reference line exceeds the limit, selecting a path sequence point closest to the unmanned vehicle from a plurality of path sequence points included in the preset reference line as a reference sequence point, calculating the offset between the unmanned vehicle and the reference sequence point as a comprehensive error, and judging whether the comprehensive error exceeds the limit, and the target sampling point is a point which is sampled on the preset reference line and meets a preset sampling distance with the distance between the unmanned vehicle;
generating a planned driving track of the unmanned vehicle by using a sampling track planner by taking the current position of the unmanned vehicle as a starting point and the target sampling point as a terminal point, and controlling the unmanned vehicle to advance according to the planned driving track;
and calculating a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line in real time in the process that the unmanned vehicle travels according to the planned travel track, and controlling the unmanned vehicle to travel according to the preset reference line when the calculated real-time comprehensive error is smaller than a preset error threshold value.
2. The method of claim 1, wherein when it is detected that the combined error between the unmanned vehicle and a preset reference line is exceeded, before determining the target sampling point on the preset reference line, the method further comprises:
acquiring position and posture information of the current position of the unmanned vehicle every other preset detection period, and determining the preset reference line preset in the unmanned vehicle;
reading a plurality of path sequence points included by the preset reference line, respectively calculating the distance between each path sequence point in the plurality of path sequence points and the current position of the unmanned vehicle, and determining the reference sequence point in the plurality of path sequence points, wherein the distance between the reference sequence point and the current position of the unmanned vehicle is smaller than other path sequence points except the reference sequence point in the plurality of path sequence points;
acquiring sequence point attitude coordinates of the reference sequence points, and calculating a comprehensive error between the sequence point attitude coordinates and the position attitude information;
and when the comprehensive error is larger than the preset error threshold value, determining that the comprehensive error between the unmanned vehicle and a preset reference line is detected to be out of limit.
3. The method of claim 2, wherein said calculating a composite error between said sequence point pose coordinates and said position pose information comprises:
reading the abscissa and the ordinate of the sequence point attitude coordinate, and reading the abscissa and the ordinate of the position attitude information;
calculating an abscissa difference value between the abscissa of the sequence point attitude coordinate and the abscissa of the position attitude information, and calculating an ordinate difference value between the ordinate of the sequence point attitude coordinate and the ordinate of the position attitude information;
reading a sequence point course angle of the sequence point attitude coordinate, reading a vehicle course angle of the position attitude information, and calculating a course angle difference value between the sequence point course angle and the vehicle course angle;
calculating a first square value of the horizontal coordinate difference value and a second square value of the vertical coordinate difference value, calculating a sum of the first square value and the second square value, and performing square root calculation on the sum to obtain a root value;
calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value;
and calculating the sum of the root value and the absolute value as the comprehensive error between the sequence point attitude coordinate and the position attitude information.
4. The method of claim 2, wherein after obtaining the sequence point pose coordinates of the reference sequence point and calculating the composite error between the sequence point pose coordinates and the position pose information, the method further comprises:
when the comprehensive error is smaller than or equal to the preset error threshold, controlling the unmanned vehicle to run according to the preset reference line tracking, continuously acquiring the position posture information of the current position of the unmanned vehicle again every other preset detection period, determining the preset reference line, determining a new datum sequence point again in a plurality of path sequence points included in the preset reference line, acquiring the sequence point posture coordinate of the new datum sequence point, calculating the comprehensive error between the sequence point posture coordinate of the new datum sequence point and the newly acquired position posture information, and judging whether the comprehensive error is larger than the preset error threshold.
5. The method according to claim 1, wherein the determining of the target sampling point on the preset reference line comprises:
acquiring the preset sampling distance;
and taking the current position of the unmanned vehicle as a reference, and sampling a point on the preset reference line, wherein the distance between the point and the current position meets the preset sampling distance, and the point is taken as the target sampling point.
6. The method of claim 1, wherein generating the planned travel trajectory of the unmanned vehicle using a sample trajectory planner comprises:
according to the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point, solving a curvature coefficient corresponding to a curvature function about the curve arc length in the sampling track planner by using a Newton root method;
and determining a driving curve meeting vehicle kinematic constraints between the current position of the unmanned vehicle and the target sampling points on the basis of the curvature coefficient, and taking the driving curve as a planned driving track of the unmanned vehicle.
7. The method according to claim 6, wherein the solving the curvature coefficient corresponding to the curvature function about the curve arc length in the sampling trajectory planner by using a Newton root method according to the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point comprises:
determining a curvature coefficient function with respect to a start curvature value, a first curvature, a second curvature, an end curvature value, and a curve arc length based on a start curvature value of the unmanned vehicle at the current location, an end curvature value at the target sampling point, and the curvature function with respect to a curve arc length, wherein the first curvature and the second curvature are curvatures at 1/3 curve arc length and 2/3 curve arc length, respectively;
determining an abscissa function, an ordinate function, and a course angle function with respect to a start curvature value, a first curvature, a second curvature, a curve arc length, and an end curvature value, respectively, based on the curvature coefficient function and the sampling trajectory planner;
obtaining a first initial value, a second initial value and a third initial value corresponding to a first curvature, a second curvature and a curve arc length respectively, and substituting the first initial value, the second initial value, the third initial value, the starting point curvature value and the ending point curvature value into the abscissa function, the ordinate function, the course angle function and the curvature coefficient function respectively to obtain an abscissa, an ordinate, a course angle and a curvature value corresponding to a target insertion point between the current position of the unmanned vehicle and the target sampling point;
correspondingly subtracting the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point from the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point to obtain an abscissa difference value, an ordinate difference value, a course angle difference value and a curvature difference value between the target sampling point and the target insertion point, and calculating a difference sum of the abscissa difference value, the ordinate difference value, the course angle difference value and the curvature difference value;
continuously updating and iterating the first initial value, the second initial value and the third initial value based on the difference sum until the abscissa, the ordinate, the course angle and the curvature value corresponding to the target insertion point are respectively the same as the abscissa, the ordinate, the course angle and the curvature value corresponding to the target sampling point, and outputting a first curvature value, a second curvature value and a curve arc length after final iteration updating;
and determining the curvature coefficient according to the first curvature value, the second curvature value, the curve arc length and the curvature coefficient function after the final iteration updating.
8. The method according to claim 7, wherein continuously performing update iteration on the first initial value, the second initial value, and the third initial value based on the sum of the difference values until an abscissa, an ordinate, a heading angle, and a curvature value corresponding to a target insertion point are respectively the same as an abscissa, an ordinate, a heading angle, and a curvature value corresponding to the target sampling point, and outputting a final iteratively updated first curvature value, second curvature value, and curve arc length comprises:
calculating Jacobian matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively based on the abscissa function, the ordinate function and the course angle function;
calculating inverse matrixes of Jacobian matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively, and multiplying the inverse matrixes corresponding to the first curvature, the second curvature and the curve arc length respectively with the sum of the difference values to obtain a first curvature variation, a second curvature variation and a curve arc length variation of the iteration;
updating the first initial value, the second initial value and the third initial value respectively based on the first curvature variation, the second curvature variation and the curve arc length variation;
and repeating the iterative updating process of the first initial value, the second initial value and the third initial value until the sum of the difference values is 0, and outputting the finally iteratively updated first curvature value, second curvature value and curve arc length.
9. The method of claim 1, wherein said calculating real-time composite error between the real-time position of the unmanned vehicle and the preset reference line in real-time comprises:
sampling the preset reference line in real time to obtain a plurality of sampling points, and respectively calculating the Euclidean distance between each sampling point in the plurality of sampling points and the real-time position;
extracting sampling points with Euclidean distances smaller than a preset distance threshold value from the plurality of sampling points as evaluation points, and acquiring real-time position attitude information of real-time positions and evaluation point attitude coordinates of the evaluation points in real time;
and calculating a comprehensive error between the attitude coordinate of the evaluation point and the attitude information of the real-time position as a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line.
10. The method of claim 9, wherein said separately calculating the euclidean distance between each of the plurality of sampling points and the real-time location comprises:
acquiring the abscissa and the ordinate of each sampling point in the plurality of sampling points;
reading the horizontal coordinate and the vertical coordinate of the real-time position;
calculating a horizontal coordinate difference value of the horizontal coordinate of the sampling point and the horizontal coordinate of the real-time position, and calculating a third square value of the horizontal coordinate difference value;
calculating a vertical coordinate difference value of a vertical coordinate of the sampling point and a vertical coordinate of the real-time position, and calculating a fourth square value of the vertical coordinate difference value;
and calculating the sum of the third square value and the fourth square value, performing square root calculation on the sum, and taking the obtained root value as the Euclidean distance between the currently calculated sampling point and the real-time position.
11. The method of claim 9, wherein the calculating a composite error between the evaluation point pose coordinates and the real-time position pose information as a real-time composite error between the real-time position of the unmanned vehicle and the preset reference line comprises:
reading the abscissa and the ordinate of the attitude coordinate of the evaluation point, and reading the abscissa and the ordinate of the attitude information of the real-time position;
calculating an abscissa difference value between the abscissa of the attitude coordinate of the evaluation point and the abscissa of the attitude information of the real-time position, and calculating an ordinate difference value between the ordinate of the attitude coordinate of the evaluation point and the ordinate of the attitude information of the real-time position;
reading an evaluation point course angle of the evaluation point attitude coordinate, reading a real-time course angle of the real-time position attitude information, and calculating a course angle difference value between the evaluation point course angle and the real-time course angle;
calculating a fifth square value of the horizontal coordinate difference value and a sixth square value of the vertical coordinate difference value, calculating a sum of the fifth square value and the sixth square value, and performing square root calculation on the sum to obtain a root value;
calculating the sine value of the course angle difference value, and acquiring the absolute value of the sine value;
and calculating a sum of the root value and the absolute value as the composite error, and taking the composite error as a real-time composite error between the real-time position of the unmanned vehicle and the preset reference line.
12. A path planner, comprising:
a determining unit, configured to determine a target sampling point on a preset reference line when detecting that a total error between an unmanned vehicle and the preset reference line exceeds a preset reference line, where the preset reference line is a vehicle running track preset in the unmanned vehicle, and when detecting whether the total error between the unmanned vehicle and the preset reference line exceeds the preset reference line, a path sequence point closest to the unmanned vehicle is selected as a reference sequence point from a plurality of path sequence points included in the preset reference line, an offset between the unmanned vehicle and the reference sequence point is calculated as a total error, and whether the total error exceeds the preset reference line is determined, and the target sampling point is a point where a distance between the unmanned vehicle and the target sampling point, which is sampled on the preset reference line, satisfies a preset sampling distance;
the generating unit is used for generating a planned running track of the unmanned vehicle by adopting a sampling track planner by taking the current position of the unmanned vehicle as a starting point and the target sampling point as an end point, and controlling the unmanned vehicle to move according to the planned running track;
and the calculating unit is used for calculating a real-time comprehensive error between the real-time position of the unmanned vehicle and the preset reference line in real time in the process that the unmanned vehicle advances according to the planned driving track, and controlling the unmanned vehicle to drive according to the preset reference line tracking when the calculated real-time comprehensive error is smaller than a preset error threshold value.
13. A chip, characterized in that the chip comprises at least one chip processor and a communication interface, the communication interface being coupled with the at least one chip processor, the at least one chip processor being configured to run a computer program or instructions to implement the path planning method according to any of claims 1-11.
14. A terminal, characterized in that it comprises a path planning device according to claim 12.
15. An electronic device comprising a device memory, a device processor and a computer program stored on the device memory and executable on the device processor, characterized in that the computer program, when executed by the device processor, implements the steps of the method of any one of claims 1 to 11.
16. A storage medium on which a computer program is stored, which computer program, when being executed by a media processor, carries out the steps of a method according to any one of claims 1 to 11.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211068224.7A CN115127576B (en) | 2022-09-02 | 2022-09-02 | Path planning method, device, chip, terminal, electronic equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211068224.7A CN115127576B (en) | 2022-09-02 | 2022-09-02 | Path planning method, device, chip, terminal, electronic equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115127576A CN115127576A (en) | 2022-09-30 |
| CN115127576B true CN115127576B (en) | 2022-12-13 |
Family
ID=83387666
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211068224.7A Active CN115127576B (en) | 2022-09-02 | 2022-09-02 | Path planning method, device, chip, terminal, electronic equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115127576B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116907412B (en) * | 2023-09-12 | 2023-11-17 | 农业农村部南京农业机械化研究所 | Agricultural machinery row spacing deviation detection method, device and system |
| CN117572476B (en) * | 2024-01-15 | 2024-04-09 | 智道网联科技(北京)有限公司 | Automatic driving vehicle positioning adjustment method and device based on driving track |
| CN117818665A (en) * | 2024-03-05 | 2024-04-05 | 智道网联科技(北京)有限公司 | Automatic driving vehicle control method and device, electronic equipment and storage medium |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1761980A (en) * | 2003-03-17 | 2006-04-19 | 松下电器产业株式会社 | Method and device for transmitting a run trace in probe car system |
| CN107037818A (en) * | 2017-06-22 | 2017-08-11 | 奇瑞汽车股份有限公司 | A kind of path following method of pilotless automobile |
| CN111486848A (en) * | 2020-05-25 | 2020-08-04 | 上海杰销自动化科技有限公司 | AGV visual navigation method, system, computer equipment and storage medium |
| CN111610785A (en) * | 2020-05-26 | 2020-09-01 | 三一专用汽车有限责任公司 | Motion planning method and motion planning device for unmanned vehicle |
| CN111750887A (en) * | 2020-06-11 | 2020-10-09 | 上海交通大学 | Unmanned vehicle trajectory planning method and system for reducing accident severity |
| CN112146667A (en) * | 2020-09-29 | 2020-12-29 | 广州小鹏自动驾驶科技有限公司 | Method and device for generating vehicle transition track |
| CN112859863A (en) * | 2021-01-15 | 2021-05-28 | 北京科技大学 | Prediction-based path tracking control key reference point selection method and system |
| CN113031583A (en) * | 2020-03-13 | 2021-06-25 | 青岛慧拓智能机器有限公司 | Obstacle avoidance method for structured road |
| CN113985887A (en) * | 2021-11-04 | 2022-01-28 | 北京京东乾石科技有限公司 | Method for generating motion trail of differential mobile robot and motion control device |
| CN114115299A (en) * | 2022-01-25 | 2022-03-01 | 上海仙工智能科技有限公司 | Path planning method and device for smooth regression of mobile robot to given trajectory |
| WO2022148441A1 (en) * | 2021-01-11 | 2022-07-14 | 长沙智能驾驶研究院有限公司 | Vehicle parking planning method and apparatus, and device and computer storage medium |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6643370B2 (en) * | 2018-01-23 | 2020-02-12 | 本田技研工業株式会社 | Driving reference line determination device and automatic driving device |
| CN110749333B (en) * | 2019-11-07 | 2022-02-22 | 中南大学 | Unmanned vehicle motion planning method based on multi-objective optimization |
-
2022
- 2022-09-02 CN CN202211068224.7A patent/CN115127576B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1761980A (en) * | 2003-03-17 | 2006-04-19 | 松下电器产业株式会社 | Method and device for transmitting a run trace in probe car system |
| CN107037818A (en) * | 2017-06-22 | 2017-08-11 | 奇瑞汽车股份有限公司 | A kind of path following method of pilotless automobile |
| CN113031583A (en) * | 2020-03-13 | 2021-06-25 | 青岛慧拓智能机器有限公司 | Obstacle avoidance method for structured road |
| CN111486848A (en) * | 2020-05-25 | 2020-08-04 | 上海杰销自动化科技有限公司 | AGV visual navigation method, system, computer equipment and storage medium |
| CN111610785A (en) * | 2020-05-26 | 2020-09-01 | 三一专用汽车有限责任公司 | Motion planning method and motion planning device for unmanned vehicle |
| CN111750887A (en) * | 2020-06-11 | 2020-10-09 | 上海交通大学 | Unmanned vehicle trajectory planning method and system for reducing accident severity |
| CN112146667A (en) * | 2020-09-29 | 2020-12-29 | 广州小鹏自动驾驶科技有限公司 | Method and device for generating vehicle transition track |
| WO2022148441A1 (en) * | 2021-01-11 | 2022-07-14 | 长沙智能驾驶研究院有限公司 | Vehicle parking planning method and apparatus, and device and computer storage medium |
| CN112859863A (en) * | 2021-01-15 | 2021-05-28 | 北京科技大学 | Prediction-based path tracking control key reference point selection method and system |
| CN113985887A (en) * | 2021-11-04 | 2022-01-28 | 北京京东乾石科技有限公司 | Method for generating motion trail of differential mobile robot and motion control device |
| CN114115299A (en) * | 2022-01-25 | 2022-03-01 | 上海仙工智能科技有限公司 | Path planning method and device for smooth regression of mobile robot to given trajectory |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115127576A (en) | 2022-09-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115127576B (en) | Path planning method, device, chip, terminal, electronic equipment and storage medium | |
| US10591926B2 (en) | Smooth road reference for autonomous driving vehicles based on 2D constrained smoothing spline | |
| CN105292116B (en) | The lane changing path planning algorithm of automatic driving vehicle | |
| US10816977B2 (en) | Path and speed optimization fallback mechanism for autonomous vehicles | |
| US10515321B2 (en) | Cost based path planning for autonomous driving vehicles | |
| US10948919B2 (en) | Dynamic programming and gradient descent based decision and planning for autonomous driving vehicles | |
| US10754339B2 (en) | Dynamic programming and quadratic programming based decision and planning for autonomous driving vehicles | |
| EP3861291B1 (en) | Spline curve and spiral curve based reference line smoothing method | |
| CN105270410B (en) | Exact curvature algorithm for estimating for the path planning of autonomous land vehicle | |
| US10800427B2 (en) | Systems and methods for a vehicle controller robust to time delays | |
| EP3524934A1 (en) | Systems and methods for determining a projection of an obstacle trajectory onto a reference line of an autonomous vehicle | |
| CN113847920A (en) | Two-stage path planning for autonomous vehicles | |
| CN110345957A (en) | Vehicle route identification | |
| CN112394725B (en) | Prediction and reaction field of view based planning for autopilot | |
| CN112477849B (en) | Parking control method and device for automatic driving truck and automatic driving truck | |
| EP3992732A1 (en) | Method and apparatus for predicting motion trajectory | |
| CN111813112A (en) | Vehicle track point determination method and device, vehicle and storage medium | |
| CN111123947A (en) | Robot traveling control method and device, electronic device, medium, and robot | |
| US20210188308A1 (en) | A qp spline path and spiral path based reference line smoothing method for autonomous driving | |
| CN211427151U (en) | Automatic guide system applied to unmanned freight vehicle in closed field | |
| CN113085868A (en) | Method, device and storage medium for operating an automated vehicle | |
| CN115123217B (en) | Mine obstacle vehicle driving track generation method and device and computer equipment | |
| CN115214775B (en) | Steering wheel neutral position adjusting method, device, equipment and medium | |
| US20230391356A1 (en) | Dynamic scenario parameters for an autonomous driving vehicle | |
| CN115303291B (en) | Trailer trajectory prediction method and device for towed vehicle, electronic device and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |