CN116501048A - Self-mobile equipment ground penetrating path planning method - Google Patents
Self-mobile equipment ground penetrating path planning method Download PDFInfo
- Publication number
- CN116501048A CN116501048A CN202310467918.6A CN202310467918A CN116501048A CN 116501048 A CN116501048 A CN 116501048A CN 202310467918 A CN202310467918 A CN 202310467918A CN 116501048 A CN116501048 A CN 116501048A
- Authority
- CN
- China
- Prior art keywords
- path
- point
- planning
- map
- ground penetrating
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000000149 penetrating effect Effects 0.000 title claims abstract description 60
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000001514 detection method Methods 0.000 claims abstract description 57
- 239000013598 vector Substances 0.000 claims abstract description 12
- 238000012545 processing Methods 0.000 claims description 9
- 230000003247 decreasing effect Effects 0.000 claims description 6
- 238000004904 shortening Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 abstract description 10
- 238000013461 design Methods 0.000 abstract description 3
- 201000010099 disease Diseases 0.000 abstract description 2
- 208000037265 diseases, disorders, signs and symptoms Diseases 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 230000008447 perception Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 238000000354 decomposition reaction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 101100393312 Lactobacillus delbrueckii subsp. bulgaricus (strain ATCC 11842 / DSM 20081 / BCRC 10696 / JCM 1002 / NBRC 13953 / NCIMB 11778 / NCTC 12712 / WDCM 00102 / Lb 14) gpsA1 gene Proteins 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000002591 computed tomography Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 101150095733 gpsA gene Proteins 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000002653 magnetic therapy Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000011144 upstream manufacturing Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0225—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving docking at a fixed facility, e.g. base station or loading bay
Abstract
The invention relates to a self-mobile equipment ground penetrating path planning method, which comprises the steps of creating map information and generating map data; self-adaptive detection of the operation breadth; planning an operation path and forming a recharging path; executing a ground penetrating task according to the operation path; after the ground penetrating task is executed, moving to a target position according to the recharging path; creating map information includes: storing three-dimensional information of map data under the same attribute into a vector container, and marking corresponding attributes; map data information of different attributes is stored in the structure body and marked as path_map; the self-adaptive setting detection operation width comprises the following steps: traversing path_map, and searching three-dimensional coordinate information of all boundary points; calculating a map gradient according to the three-dimensional coordinate information; setting a first detection breadth; adjusting the first detection breadth according to the calculated map gradient to obtain a second detection breadth adapting to the map gradient; the unmanned vehicle and the ground penetrating radar are combined to design the self-propelled ground penetrating radar, so that the process of exploring the diseases in the dam body can be realized.
Description
Technical Field
The invention relates to the field of path planning and detection, in particular to a self-moving equipment ground penetrating path planning method suitable for dam safety detection.
Background
The dam CT technology is an application of a computer tomography technology in dam safety monitoring, electromagnetic waves with a certain frequency range are transmitted in a dam body to form detection data of an internal section of a detection area, and analysis and processing are carried out by a computer to quantitatively reflect magnetic therapy property distribution and aging conditions, diseases and defect positions of the dam body. At present, the common dam CT technology has the following problems:
firstly, the exploration is usually carried out by adopting manual cooperation with a ground penetrating radar, the detection efficiency is low, the labor cost is high, time and labor are wasted, and the method is particularly suitable for dam bodies comprising a plane, an upstream surface and a back surface, and has high construction and detection difficulty and potential safety hazard.
And (II) how to combine the ground penetrating of the intelligent unmanned vehicle with the CT technology of the dam is an important research direction of research personnel in the industry, even if the ground penetrating of the intelligent unmanned vehicle is adopted, the safety and reliability of autonomous detection of the intelligent unmanned vehicle can be reduced if the ground penetrating path is not planned due to severe environment or complex detection scene, so that how to ensure that the intelligent unmanned vehicle can plan the ground penetrating path in the ground penetrating process and realize the treatment of the conditions of obstacle detection, vehicle slipping and the like in the ground penetrating process is an important direction of research in the field.
Disclosure of Invention
Compared with the prior art, the invention combines the ground penetrating radar with the intelligent unmanned vehicle to design, integrates the exploration function of the ground penetrating radar and the autonomous patrol function of the intelligent unmanned vehicle, carries out self-adaptive breadth path planning on the area within the boundary range of the online calibration map, and can treat dynamic barriers and slipping in the ground penetrating process.
In order to achieve the above purpose, the technical scheme of the invention is as follows:
a self-mobile equipment ground penetrating path planning method comprises the following steps:
creating map information and generating map data;
self-adaptive detection of the operation breadth;
planning an operation path and forming a recharging path;
executing a ground penetrating task according to the operation path;
and after the ground penetrating task is executed, the ground penetrating task moves to the initial position according to the recharging path.
Optionally, in the method for planning a ground penetrating path of a self-mobile device, the creating map information includes the steps of:
storing three-dimensional information of map data under the same attribute into a vector container, and marking corresponding attributes;
map data information of different attributes is stored in the structure, and is labeled path_map.
Optionally, in the method for planning a ground penetrating path of a self-mobile device, the adaptively setting the detection job width includes the following steps:
traversing path_map, and searching three-dimensional coordinate information of all boundary points;
calculating a map gradient according to the three-dimensional coordinate information;
setting a first detection breadth;
adjusting the first detection breadth according to the calculated map gradient to obtain a second detection breadth adapting to the map gradient;
and storing the second detection breadth.
Optionally, in the method for planning a ground penetrating path of a self-mobile device, the planning the working path and forming the recharging path includes the following steps:
establishing a grid map and a cost map;
taking a certain vertex enveloped by the cost map rectangle as a basic three-dimensional coordinate value;
starting from the basic three-dimensional coordinate values, collecting according to a planned initial path, and storing and forming a data pair structure with a plurality of three-dimensional coordinate values;
sequentially connecting a second three-dimensional coordinate value of each data pair in the initial path set with a first three-dimensional coordinate value of the next data pair through a planning algorithm to form a planning path with a plurality of path segments;
taking the last three-dimensional coordinate value in the initial path set as a starting point, taking the three-dimensional coordinate value of the current equipment as an end point, and connecting and generating a recharging path through a planning algorithm;
and storing all planned path points into a vector container in sequence to complete global path planning.
Optionally, in the method for planning a ground penetrating path of a self-mobile device, the initial path planning includes the following steps:
starting from the base point, repeatedly searching all the reachable points in the reachable region through the grid according to a column of increasing searching and a column of decreasing searching, and acquiring an initial path set based on the reachable points.
Optionally, in the method for planning a ground penetrating path of a self-mobile device, the forming a planned path with a plurality of path segments includes the following steps:
judging whether collision occurs at the start and stop points of each path segment;
if no collision exists, generating a current path segment;
if collision exists, the current path section is re-planned to be a second path section;
the second path segment is planned as follows:
shortening the distance between the starting point coordinate and the end point coordinate in the current path segment data pair to form a second path segment;
updating the second path segment to a path set;
judging whether the starting point and the end point in the second path section collide or not;
if not, generating a current path segment;
if so, the Nth path segment is re-planned until no collision occurs.
Optionally, in the method for planning a ground penetrating path of a self-mobile device, the processing of the dynamic obstacle avoidance includes the following steps:
acquiring a planned path in a set length in front of a current control point of the mobile equipment in real time as a perceived path and point cloud data around the mobile equipment;
acquiring dynamic obstacle detection frame information according to the point cloud data and a clustering algorithm;
updating the cost map;
judging whether each path point in the sensing path collides with the dynamic obstacle detection frame or not;
if collision occurs, acquiring a safety point, and planning an obstacle avoidance path according to the safety point and a control point;
if collision does not occur, updating the sensing path in real time and continuously acquiring point cloud data and obstacle detection frame information to perform collision detection.
Optionally, in the method for planning a ground penetrating path of a self-mobile device, the perceived path acquisition includes the following steps:
acquiring a current control point of a vehicle and a starting point and an ending point of an operation path where the current control point is positioned;
judging the relation between the distance between the control point and the end point in the working path and the set parameter value, and updating the length of the sensing path;
and repeatedly judging that the updated perceived path length exceeds a set threshold or reaches the end point of the working path.
Optionally, in the self-mobile device ground penetrating path planning method, the slipping processing includes the following steps:
setting a threshold value, and judging whether the vehicle is in a slipping state or not;
if slipping occurs, performing escaping treatment;
if no slip occurs, continuously controlling the unmanned vehicle to normally run along the global planned path.
Optionally, in the method for planning a ground penetrating path of a self-mobile device, the escaping processing includes the following steps:
taking the slipping point as a circle center, making a circle according to a set radius value, and selecting at least one position on the circumference as a slipping back end point as an alternative point;
sequentially judging whether the alternative points are safety points or not;
if no safety point is judged, replacing the manual remote control to get rid of the trouble;
if the safety point is judged, planning a reverse path from the slipping point to the safety point by taking the slipping point as a starting point, and controlling the self-moving equipment to the safety point;
updating the cost map;
after searching the updated cost map, selecting a safety point closest to the slipping point on the original planning path, planning an additional path from the alternative point to the safety point, and bypassing the slipping area.
Optionally, in the self-mobile device ground penetrating path planning method, the position of the candidate point on the circumference is divided into any one direction of right rear, left rear, right left side and right side of the slip point according to priority.
Compared with the prior art, the invention has the following beneficial technical effects:
according to the self-mobile equipment ground penetrating path planning method, the unmanned vehicle and the ground penetrating radar are combined to design the self-propelled ground penetrating radar, so that a full-automatic dam exploration process can be realized; the full-coverage path planning method is provided, the path planning with self-adaptive breadth is carried out on the area within the boundary range of the online calibration map, and meanwhile, the dynamic obstacle and slipping situations in the ground detection process are processed; therefore, the coverage rate and the working efficiency of full-coverage type inspection operation can be effectively improved, and the exploration safety is improved; dynamic obstacle avoidance can be performed in real time to improve the safety and reliability of autonomous detection; the device can automatically get rid of the slip condition to adapt to severe environments or complex detection scenes.
Drawings
Fig. 1 shows a flowchart of a method for planning a ground penetrating path of a self-mobile device according to an embodiment of the present invention.
Fig. 2 shows a schematic diagram of a planned path in a method for planning a ground penetrating path of a self-mobile device according to an embodiment of the present invention.
Fig. 3 is a schematic diagram of a dynamic obstacle avoidance method in a ground penetrating path planning method of a self-mobile device according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of a slip escaping method in a ground penetrating path planning method of a self-mobile device according to an embodiment of the invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the following describes a method for planning a ground penetrating path of a self-mobile device in detail with reference to the accompanying drawings and detailed description. The advantages and features of the present invention will become more apparent from the following description. It should be noted that the drawings are in a very simplified form and are all to a non-precise scale, merely for the purpose of facilitating and clearly aiding in the description of embodiments of the invention. For a better understanding of the invention with objects, features and advantages, refer to the drawings. It should be understood that the structures, proportions, sizes, etc. shown in the drawings are for illustration purposes only and should not be construed as limiting the invention to the extent that any modifications, changes in the proportions, or adjustments of the sizes of structures, proportions, or otherwise, used in the practice of the invention, are included in the spirit and scope of the invention which is otherwise, without departing from the spirit or essential characteristics thereof.
Examples:
a self-mobile equipment ground penetrating path planning method, as shown in figure 1, comprises the following steps:
step S1, creating map information and generating map data.
Specifically, a remote control unmanned vehicle is controlled to move to a required dotting position by taking a handheld remote controller as an example in a working scene, a map is created through an application program in an android platform carried by the remote controller, corresponding map attributes (boundaries, barriers, recharging paths and the like) are selected, the current position is recorded, and the map attributes are transmitted to a controller carried by the unmanned vehicle in a network communication mode to generate and store corresponding map data. The specific steps of generating map data are as follows:
and S11, storing three-dimensional information of map data under the same attribute into a vector container, and marking the corresponding attribute.
In step S12, map data information with different attributes is stored in the structure, and labeled as path_map.
Step S2, self-adaptive detection of the operation breadth.
Specifically, the map gradient is calculated according to the map boundary information, and the detection operation breadth is set in a self-adaptive mode. The method for adaptively setting the detection operation breadth comprises the following specific steps:
step S21, traversing path_map, finding out three-dimensional coordinate information { P } of related boundary points under the operation boundary attribute 1 ,P 2 ,...,P n }。
Step S22, calculating the gradient of the map according to the three-dimensional information of the operation boundary points. Specifically, the calculated gradient is according to the following formula:
A i ， 1-3 ＝[P i，x -C x ,P i，y -C y ,P i，z -C z ],i＝1～n
B＝[svd(A) 1,2 ,svd(A) 2,2 ,svd(A) 3,2 ]
D＝[0,0,1],
slant_angle＝arccoss(v)*180/π
wherein P is i,x/y/z The coordinate value of x/y/z of the ith boundary point is represented by n, and the number of the boundary points is represented by n; a is an n×3 matrix, and elements 1-3 in the ith row are P respectively i,x -C x 、P i,y -C y 、P i,z -C z The method comprises the steps of carrying out a first treatment on the surface of the By using 3-dimensional spatial point P i And covariance A of the center point C, and combining SVD decomposition to obtain a feature vector corresponding to the minimum feature value, so as to obtain a plane normal vector B. Wherein svd (A) i,j Representing the ith row and jth column elements of the matrix obtained by performing svd decomposition on the matrix A; finally, using the plane normal vector B and the single pointing in the z directionAnd obtaining a map angle cosine value v in a mode of calculating an included angle between bit vectors D, and obtaining a map gradient slope_angle through an inverse trigonometric function and an radian rotation angle formula, wherein the angle represents a calculated vector modular length and the vector dot product.
Step S23, setting an initial detection width w=d according to the vehicle wheel distance d.
Step S24, the first detection breadth is adjusted according to the calculated map gradient so as to obtain a second detection breadth adapting to the map gradient. Specifically, the formula of the adaptive width adjustment is as follows:
specifically, d is the wheel spacing, w is the detection width, and slope_angle is the map gradient.
And S25, storing the second detection breadth for planning a full-coverage detour path.
Step S3, the operation path is planned and the recharging path is constituted, as shown in FIG. 2. Specifically, according to the operation width w, a full-coverage detour path is planned by using a hybrid a algorithm, and the coverage path and the starting point are connected to form a recharging path. Specifically, the path planning method specifically comprises the following steps:
step S31, finding a rectangular envelope according to the three-dimensional coordinate information of the related boundary points under the operation boundary attribute in the path_map, and covering the operation map formed by the boundary points to form a world map.
Step S32, using the world map, a grid map is built according to the set map resolution r.
Step S33, further, a cost map is built according to the grid map, and the cost and the attribute of the reachable area in the operation boundary, the cost and the attribute of the unreachable area such as the obstacle outside the operation boundary and inside the boundary are set.
Step S34, setting all grid coordinates (x) in the cost map with a certain vertex of the cost map as a base point base_point (0, 0) i ,y i )。
Step S35, starting from the base point, planning an initial path set according to the y direction. Specifically, the initial path planning method comprises the following steps:
step S351, first, starting from the base point, finding the grid G of the first reachable region in the y increasing direction 1 Calculating the center coordinates P of grid points 1 And putting the first data pair of the path set into the position of the first three-dimensional coordinate point.
Step S352, continuing to search the next grid G according to the y increasing direction 2 If G 2 For the reachable point, calculate G 2 Center coordinates P 2 And placing the first data of the path set into the position of the second three-dimensional coordinate point.
Step S353, repeating step S352, and updating the position coordinates of the second three-dimensional coordinate point to the latest reachable point until the currently found y value reaches y max Or find unreachable attribute grid points.
Specifically, if y is not currently found max And find an unreachable grid, the search for a set of data pairs is ended. Continuing with the current column, find the next reachable grid G 3 Calculating the center coordinates P of grid points 3 And placing the next data pair of the current path set into the position of the first three-dimensional coordinate point. And continuously searching the coordinates of the current data to the second three-dimensional coordinate point according to the y increasing direction.
If y is currently found max Indicating that the current column is found, and finishing the finding of the current path set.
Step S354, after the search of a row is completed, calculating the x coordinate of the next row of paths according to the set operation width: x+ =w, and from y=y max Initially, according to the decreasing y direction, the first reachable grid G in the current column is searched n Calculate G n Center coordinates P n And placing the first data pair of the second path set into the first three-dimensional coordinate point position.
Step S355, updating the coordinates of the second three-dimensional coordinate point to the latest reachable point according to the decreasing direction of y until the currently searched y value reaches 0 or the unreachable attribute grid point is searched.
Specifically, if y=0 is not currently found and an unreachable grid point is found, the search of a set of data pairs is ended. Continuing with the current column, find the next reachable grid G k Calculating grid center coordinates P k And placing the next data pair of the current path set into the position of the first three-dimensional coordinate point. And continuing to search the coordinates of the current data to the second three-dimensional coordinate point according to the decreasing direction of y.
If y=0 is found currently, it indicates that the current column search is completed. The current set of paths is found.
Step S356, repeating steps S352-S355 in a column of increasing search and a column of decreasing search until x is greater than or equal to x max And ending the recursion and acquiring an initial path set.
Step S36, after the initial path set is obtained according to the mode of S35, the second three-dimensional coordinate point of each data pair in the path and the first three-dimensional coordinate point of the next data pair are sequentially connected by utilizing the mixed A to form a planning path. Specifically, the method for constructing the planned path comprises the following steps:
step S361, budgeting whether collision will occur at each path segment start and stop point according to the vehicle model and the AABB collision detection algorithm.
In step S362, specifically, if no collision occurs, taking the end point of the corresponding path segment and the start point of the next path segment as the start point and the end point of the planning algorithm, first, trying to directly generate the dubins curve connection start point, and detecting whether there is a collision possibility at the path points in the planned connection path through the AABB algorithm.
If no collision is possible, corresponding paths are generated, and paths which are connected by using the mixed A are sequentially stored in the path container set again according to the form of three-dimensional coordinate values of single path points.
If collision is possible, after one-step path planning is performed according to the step length by utilizing the mixed A, updating a planning starting point to be a mixed A one-step planning end point, recursively trying dubins curves until a complete corresponding path planning curve is planned, and storing the complete corresponding path planning curve into a path container set.
In step S363, specifically, if a collision occurs, the distance between the start point and the end point in the corresponding data pair is shortened according to the set parameters, and the corresponding path set is updated. And repeating S361 and S362 until path points in all path sets are planned to be connected, and storing the path points in the path container set in sequence.
Step S37, using the last three-dimensional coordinate point in the path as a starting point and the current position of the vehicle as an end point, generating a recharging path by using the mixed A and adding the recharging path into the original path.
And step S38, storing all planned path points into a path container set to complete global path planning.
And S4, executing the ground penetrating task according to the operation path.
Specifically, in the process of executing the ground penetrating task, the condition of complex detection environment may exist, so that dynamic obstacle avoidance is required to be performed on the obstacles in the task process, and the slip condition which may be generated is subjected to escaping treatment;
specifically, a schematic diagram of the dynamic obstacle avoidance is shown in a third diagram, and the specific steps are as follows:
step A1, acquiring a current control point P of the unmanned vehicle in real time 1 The planned path within the front L length is taken as the perceived path L 1 And acquiring the cloud data of the points around the unmanned vehicle body in real time by utilizing the laser radar.
And A2, acquiring dynamic obstacle detection frames { O1, O2, & gton }, by adopting a ground segmentation algorithm and a grid clustering algorithm. Including obstacle size and three-dimensional coordinate information.
Step A3, updating a cost map and placing a dynamic obstacle detection frame { O }, wherein the cost map is a cost map 1 ,O 2 ,...O n And setting the grids in the corresponding areas as unreachable areas.
Specifically, in order to make the planned path far away from the obstacle detection frame as far as possible, when the cost value of the dynamic obstacle cost map is set, a stepwise cost reduction method is adopted. The method comprises the following steps: the cost (cost value) in the obstacle detection frame is set to be unreachable cost1 (generally set to be the maximum value 255), the grid cost value at the position close to the boundary of the obstacle detection frame is inversely proportional to the boundary distance of the obstacle in a similar contour line mode, so that the grid cost value around the whole obstacle shows a layer-by-layer descending trend until the cost is lowered to be 0 (reachable area cost value).
Step A4, combining specific vehicle model information, predicting and judging by utilizing an AABB detection algorithm, and simulating an unmanned vehicle control point to reach a perception path L 1 Whether collision is generated with the obstacle detection frame or not at each sensing point of the image sensor. Specifically, the sensing path acquisition method comprises the following steps:
step A41, acquiring the current control point P of the vehicle 1 P 1 Start and stop point S of global path 1 、S 2 。
Step A42, judging P 1 Global path end point S where distance is located 2 Is a distance d of (a).
If d<0.1m, then directly S 2 Put in the sense path and update the sense path length to l+=d.
If d is more than or equal to 0.1m, P is taken according to a fixed step length s 1 The front s length, the perceived path length l+=s is updated and will be mapped on the global path and distance P 1 Point S of length S 3 Put into the perception path.
Step A43, the latest point of the sensing path is updated as the last point in the sensing path, and the step B2 is repeated until the sensing path length exceeds the set threshold or reaches the global path end point (recharging point).
And step A5, if collision occurs, performing collision processing. Specifically, the processing steps are as follows:
step A51, searching the original global path through an AABB detection algorithm, and at the current control point P 1 The first route point without collision in front is taken as a safety point P 2 。
Step A52, re-planning from the current control point P using the hybrid A-and dubins curves 1 To the safety point P 2 Obstacle avoidance path L of (1) 2 And adds it as an additional planned path to the original global planned path.
Further, performing mixed A-scale planning, scaling the calculated obstacle cost when calculating the cost value of each one-step prediction planning point, and adding the scaled obstacle cost into the calculation of the cost value of the mixed A-scale judgment planning path point, so that the planned obstacle avoidance path is far away from the obstacle as far as possible, and the generation of collision is avoided.
And step A53, continuously updating an obstacle detection frame, acquiring a perception path and detecting collision in real time in the obstacle avoidance process.
And step A6, if no collision occurs, continuously controlling the unmanned vehicle to run along the global planning path, and updating the sensing path in real time to continuously perform collision detection.
Specifically, the schematic diagram of the slip treatment is shown in fig. four, and the specific steps are as follows:
and B1, judging whether the unmanned vehicle is in the situation that the odometer count is continuously increased but no obvious change occurs in positioning for a certain time by utilizing GPS high-precision positioning and an odometer, and judging whether the vehicle is in a skidding process. Specifically, the slip determination is based on the following formula:
wherein:
current_odom_pose x/y/z positioning x/y/z coordinates for the current odometer;
last_odom_pose x/y/z positioning x/y/z coordinates obtained for the odometer at the previous moment;
current_gps_pose x/y/z positioning x/y/z coordinates for the current gps;
last_gps_pose x/y/z the position x obtained for the last moment gpsA/y/z coordinate;
current_odom_orientation w/x/y/z the value of the quaternion obtained for the current odometer;
last_odom_orientation w/x/y/z the value of the quaternion obtained by the odometer at the last moment;
current_gps_orientation w/x/y/z the value of the quaternion obtained for the current gps;
last_gps_orientation w/x/y/z the value of the quaternion obtained for the last moment gps.
Specifically, the slip judgment conditions are as follows: i delta O p -ΔT p |>T 1 Or |ΔO o -ΔT o |>T 2 Wherein T1 and T2 are set judgment thresholds. The judging method specifically comprises the following steps: the odometer records the rotation state of the wheels of the vehicle, and if the wheels of the vehicle are detected to continuously rotate within a fixed time interval, but the GPS positioning of the vehicle is not changed, the vehicle is judged to be slipping.
And step B2, if slipping occurs, performing escaping treatment.
Specifically, the slipping and escaping steps are as follows:
step B21, according to the slip point P 3 The circle with the length s as the radius is set as the point E with the priorities of right rear, left rear, right left side and right side 1-5 As an end point of slip-back. Specifically, the back end point with the highest priority is right back, and the lowest is right left/right, so as to facilitate the back of the vehicle.
Step B22, judging the E in turn 1-5 Whether or not it is a safe point. And simulating the movement of the vehicle to the backward end point, and detecting whether the vehicle model collides with the map boundary, the dynamic obstacle and the like by utilizing the AABB collision. Finding the first safety point according to the priority. If the safety point does not exist, the corresponding mobile phone application prompts slipping and getting rid of poverty failure, and the mobile phone application is replaced by manual remote control operation to get rid of poverty.
Step B23, E of judging the safety point 1/2/3/4/5 Set as the route planning end point, slip point P 3 To plan the path start, a hybrid A and dubin curve is used to plan the current slip point from the vehicleP 3 Reverse path to the reverse safe point and control the vehicle to travel to the reverse safe point.
Step B24, updating the cost map, and putting the cost map into the slip point P 3 The circle with the radius slightly smaller than the s length is used as an obstacle. As shown by the grey areas in fig. 4. Wherein, the length slightly smaller than s is set as a radius to be an obstacle, in order to avoid the situation of the backward end point E 1/2/3/4/5 And detecting collision, so that a mixed A-algorithm is allowed to draw an obstacle avoidance path without regulation.
Step B25, searching the slip point P on the original global planning path after updating the cost map 3 A nearest front safety point P 4 Setting the same as a path planning end point, and planning a backward end point E by using a mixed A and dubin curve 1/2/3/4/5 To the safety point P 4 Is routed around the slip region.
After updating the cost map, the virtual obstacle in the slipping area is not cleared, and the task is reserved until the task is completed, so that slipping caused by reentry of the area is avoided.
And step B3, if no slip occurs, continuously controlling the unmanned vehicle to normally run along the global planned path.
And S5, after the ground penetrating task is executed, moving to the initial position according to the recharging path.
The technical features of the above-described embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above-described embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.
The above examples illustrate only a few embodiments of the invention, which are described in detail and are not to be construed as limiting the scope of the invention. It should be noted that it will be apparent to those skilled in the art that several variations and modifications can be made without departing from the spirit of the invention, which are all within the scope of the invention. Accordingly, the scope of protection of the present invention is to be determined by the appended claims.
Claims (11)
1. The ground penetrating path planning method for the self-mobile equipment is characterized by comprising the following steps of:
creating map information and generating map data;
self-adaptive detection of the operation breadth;
planning an operation path and forming a recharging path;
executing a ground penetrating task according to the operation path;
and after the ground penetrating task is executed, the ground penetrating task moves to the initial position according to the recharging path.
2. The method for planning a ground penetrating path of a self-mobile device according to claim 1, wherein: the map information creation includes the steps of:
storing three-dimensional information of map data under the same attribute into a vector container, and marking corresponding attributes;
map data information of different attributes is stored in the structure, and is labeled path_map.
3. The method for planning a ground penetrating path of a self-mobile device according to claim 2, wherein: the self-adaptive setting detection operation breadth comprises the following steps:
traversing path_map, and searching three-dimensional coordinate information of all boundary points;
calculating a map gradient according to the three-dimensional coordinate information;
setting a first detection breadth;
adjusting the first detection breadth according to the calculated map gradient to obtain a second detection breadth adapting to the map gradient;
and storing the second detection breadth.
4. A method for planning a ground penetrating path of a self-mobile device as recited in claim 3, wherein: the planning operation path and the forming recharging path comprise the following steps:
establishing a grid map and a cost map;
taking a certain vertex enveloped by the cost map rectangle as a basic three-dimensional coordinate value;
starting from the basic three-dimensional coordinate values, planning an initial path set, and storing and forming a data pair structure with a plurality of three-dimensional coordinate values;
sequentially connecting a second three-dimensional coordinate value of each data pair in the initial path set with a first three-dimensional coordinate value of the next data pair through a planning algorithm to form a planning path with a plurality of path segments;
taking the last three-dimensional coordinate value in the initial path set as a starting point, taking the three-dimensional coordinate value of the current equipment as an end point, and connecting and generating a recharging path through a planning algorithm;
and storing all planned path points into a vector container in sequence to complete global path planning.
5. The method for planning a ground penetrating path of a self-mobile device according to claim 4, wherein: the initial path set planning comprises the following steps:
starting from the base point, repeatedly searching all the reachable points in the reachable region through the grid according to a column of increasing searching and a column of decreasing searching, and acquiring an initial path set based on the reachable points.
6. The method for planning a ground penetrating path of a self-mobile device according to claim 4, wherein: the forming of the planned path with a plurality of path segments comprises the steps of:
judging whether collision occurs at the start and stop points of each path segment;
if no collision exists, generating a current path segment;
if collision exists, the current path section is re-planned to be a second path section;
the second path segment is planned as follows:
shortening the distance between the starting point coordinate and the end point coordinate in the current path segment data pair to form a second path segment;
updating the second path segment to a path set;
judging whether the starting point and the end point in the second path section collide or not;
if not, generating a current path segment;
if so, the Nth path segment is re-planned until no collision occurs.
7. The method for planning a ground penetrating path of a self-mobile device according to claim 1, wherein: the method for performing the ground penetrating task according to the working path further comprises the step of processing dynamic obstacle avoidance, wherein the processing of the dynamic obstacle avoidance comprises the following steps:
acquiring a planned path in a set length in front of a current control point of the mobile equipment in real time as a perceived path and point cloud data around the mobile equipment;
acquiring dynamic obstacle detection frame information according to the point cloud data and a clustering algorithm;
updating the cost map;
judging whether each path point in the sensing path collides with the dynamic obstacle detection frame or not;
if collision occurs, acquiring a safety point, and planning an obstacle avoidance path according to the safety point and a control point;
if collision does not occur, updating the sensing path in real time and continuously acquiring point cloud data and obstacle detection frame information to perform collision detection.
8. The method for planning a ground penetrating path of a self-mobile device according to claim 7, wherein: the sensing path acquisition comprises the following steps:
acquiring a current control point of a vehicle and a starting point and an ending point of an operation path where the current control point is positioned;
judging the relation between the distance between the control point and the end point in the working path and the set parameter value, and updating the length of the sensing path;
and repeatedly judging that the updated perceived path length exceeds a set threshold or reaches the end point of the working path.
9. The method for planning a ground penetrating path of a self-mobile device according to claim 1, wherein: the ground penetrating task is executed according to the working path, and the slip processing comprises the following steps:
setting a threshold value, and judging whether the vehicle is in a slipping state or not;
if slipping occurs, performing escaping treatment;
if no slip occurs, continuously controlling the unmanned vehicle to normally run along the global planned path.
10. The method for planning a ground penetrating path of a self-mobile device according to claim 9, wherein: the escaping treatment comprises the following steps:
taking the slipping point as a circle center, making a circle according to a set radius value, and selecting at least one position on the circumference as a slipping back end point as an alternative point;
sequentially judging whether the alternative points are safety points or not;
if no safety point is judged, replacing the manual remote control to get rid of the trouble;
if the safety point is judged, planning a reverse path from the slipping point to the safety point by taking the slipping point as a starting point, and controlling the self-moving equipment to the safety point;
updating the cost map;
after searching the updated cost map, selecting a safety point closest to the slipping point on the original planning path, planning an additional path from the alternative point to the safety point, and bypassing the slipping area.
11. The method for planning a ground penetrating path of a self-mobile device according to claim 10, wherein: the positions of the candidate points on the circumference are prioritized in any one of the directions right behind, left behind, right left side and right side of the slip point.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310467918.6A CN116501048B (en) | 2023-04-26 | 2023-04-26 | Self-mobile equipment ground penetrating path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310467918.6A CN116501048B (en) | 2023-04-26 | 2023-04-26 | Self-mobile equipment ground penetrating path planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116501048A true CN116501048A (en) | 2023-07-28 |
| CN116501048B CN116501048B (en) | 2023-09-12 |
Family
ID=87329779
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310467918.6A Active CN116501048B (en) | 2023-04-26 | 2023-04-26 | Self-mobile equipment ground penetrating path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116501048B (en) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2016117713A1 (en) * | 2016-02-29 | 2016-07-28 | 株式会社小松製作所 | Control system for work machine, work machine, and management system for work machine |
| CN106371445A (en) * | 2016-11-17 | 2017-02-01 | 浙江大学 | Unmanned vehicle planning control method based on topology map |
| CN106950952A (en) * | 2017-03-10 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | For the unpiloted farm environment cognitive method of agricultural machinery |
| CN107015238A (en) * | 2017-04-27 | 2017-08-04 | 睿舆自动化（上海）有限公司 | Unmanned vehicle autonomic positioning method based on three-dimensional laser radar |
| CN108983781A (en) * | 2018-07-25 | 2018-12-11 | 北京理工大学 | A kind of environment detection method in unmanned vehicle target acquisition system |
| CN109934910A (en) * | 2019-03-14 | 2019-06-25 | 北京邮电大学 | A kind of autonomous Selection Strategy of sensing point based on three dimensional environmental model |
| CN110068836A (en) * | 2019-03-20 | 2019-07-30 | 同济大学 | A kind of laser radar curb sensory perceptual system of intelligent driving electric cleaning car |
| CN113916246A (en) * | 2021-09-26 | 2022-01-11 | 江苏徐工工程机械研究院有限公司 | Unmanned obstacle avoidance path planning method and system |
| CN114627160A (en) * | 2022-03-31 | 2022-06-14 | 江南工业集团有限公司 | Underwater environment detection method |
| CN114812581A (en) * | 2022-06-23 | 2022-07-29 | 中国科学院合肥物质科学研究院 | Cross-country environment navigation method based on multi-sensor fusion |
-
2023
- 2023-04-26 CN CN202310467918.6A patent/CN116501048B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2016117713A1 (en) * | 2016-02-29 | 2016-07-28 | 株式会社小松製作所 | Control system for work machine, work machine, and management system for work machine |
| CN106104206A (en) * | 2016-02-29 | 2016-11-09 | 株式会社小松制作所 | The management system of the control system of Work machine, Work machine and Work machine |
| CN106371445A (en) * | 2016-11-17 | 2017-02-01 | 浙江大学 | Unmanned vehicle planning control method based on topology map |
| CN106950952A (en) * | 2017-03-10 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | For the unpiloted farm environment cognitive method of agricultural machinery |
| CN107015238A (en) * | 2017-04-27 | 2017-08-04 | 睿舆自动化（上海）有限公司 | Unmanned vehicle autonomic positioning method based on three-dimensional laser radar |
| CN108983781A (en) * | 2018-07-25 | 2018-12-11 | 北京理工大学 | A kind of environment detection method in unmanned vehicle target acquisition system |
| CN109934910A (en) * | 2019-03-14 | 2019-06-25 | 北京邮电大学 | A kind of autonomous Selection Strategy of sensing point based on three dimensional environmental model |
| CN110068836A (en) * | 2019-03-20 | 2019-07-30 | 同济大学 | A kind of laser radar curb sensory perceptual system of intelligent driving electric cleaning car |
| CN113916246A (en) * | 2021-09-26 | 2022-01-11 | 江苏徐工工程机械研究院有限公司 | Unmanned obstacle avoidance path planning method and system |
| CN114627160A (en) * | 2022-03-31 | 2022-06-14 | 江南工业集团有限公司 | Underwater environment detection method |
| CN114812581A (en) * | 2022-06-23 | 2022-07-29 | 中国科学院合肥物质科学研究院 | Cross-country environment navigation method based on multi-sensor fusion |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116501048B (en) | 2023-09-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11877716B2 (en) | Determining region attribute | |
| CN106643719B (en) | Path planning algorithm of intelligent mowing vehicle | |
| CN112612273B (en) | Routing inspection robot obstacle avoidance path planning method, system, equipment and medium | |
| CN109238298A (en) | A kind of unmanned paths planning method with avoidance | |
| EP4033324B1 (en) | Obstacle information sensing method and device for mobile robot | |
| CN111596654A (en) | Cable trench robot navigation obstacle avoidance method based on improved D-star path planning algorithm | |
| CN113587930B (en) | Indoor and outdoor navigation method and device of autonomous mobile robot based on multi-sensor fusion | |
| CN109933068A (en) | Driving path planing method, device, equipment and storage medium | |
| US20230063845A1 (en) | Systems and methods for monocular based object detection | |
| CN108196538B (en) | Three-dimensional point cloud model-based field agricultural robot autonomous navigation system and method | |
| CN111123953B (en) | Particle-based mobile robot group under artificial intelligence big data and control method thereof | |
| CN113607166B (en) | Indoor and outdoor positioning method and device for autonomous mobile robot based on multi-sensor fusion | |
| CN116501048B (en) | Self-mobile equipment ground penetrating path planning method | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| CN113405557B (en) | Path planning method and related device, electronic equipment and storage medium | |
| Ribeiro et al. | 3D monitoring of woody crops using an unmanned ground vehicle | |
| CN116327570A (en) | Active guiding-oriented multi-mode autonomous driving blind guiding method and device | |
| CN115877853A (en) | Intelligent storage flow path planning system and method | |
| CN115047871A (en) | Multi-unmanned vehicle collaborative search method, device, equipment and medium for dynamic target | |
| CN111596668B (en) | Mobile robot anthropomorphic path planning method based on reverse reinforcement learning | |
| CN114266821A (en) | Online positioning method and device, terminal equipment and storage medium | |
| Wang et al. | Dynamic path planning algorithm for autonomous vehicles in cluttered environments | |
| CN117553804B (en) | Path planning method, path planning device, computer equipment and storage medium | |
| CN115855068B (en) | BIM-based robot path autonomous navigation method and system | |
| CN115755890B (en) | Weeding task path planning method and device based on data processing |
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 |