CN-115933651-B - Inspection unmanned vehicle improvement method based on dynamic path planning

CN115933651BCN 115933651 BCN115933651 BCN 115933651BCN-115933651-B

Abstract

An improved inspection vehicle based on dynamic path planning is characterized in that a laser radar, an odometer and IMU information are fused to obtain the position of the inspection vehicle, the speed of the inspection vehicle is sampled, the speed of a moving obstacle is collected, whether the inspection vehicle and the moving obstacle can run in the same direction or not is judged by judging the included angle between the movement speed direction of the inspection vehicle and the movement speed direction of the moving obstacle, the advancing direction of the inspection vehicle is optimized, then the track space of the inspection vehicle is generated, the track space is evaluated, whether the inspection vehicle and the obstacle can collide or not is judged, if yes, the track route is regenerated, if no, the optimal track is selected, the optimal path is generated, and the path is ended. The invention realizes the real-time dynamic obstacle avoidance of the unmanned inspection vehicle on the moving obstacle and the selection of the optimal track, has low algorithm complexity and simple realization, and can be applied to the inspection work of the unmanned inspection vehicle in the actual industrial field.

Inventors

ZHAO ZHIGUO
WAN XIAOKANG
PANG MIN
WEI XIAOQIAN
MAO KANGKANG
WANG RUI
XU ZHEN
XIE DONG

Assignees

淮阴工学院

Dates

Publication Date: 20260505
Application Date: 20221128

Claims (8)

1. The improved inspection unmanned vehicle method based on dynamic path planning is characterized by comprising the steps of obtaining the position of an inspection unmanned vehicle and sampling the speed of the inspection unmanned vehicle through fusion of laser radar, an odometer and IMU information, collecting the speed of a moving obstacle, judging whether the inspection unmanned vehicle and the moving obstacle can run in the same direction or not through judging the included angle between the movement speed direction of the inspection unmanned vehicle and the movement speed direction of the moving obstacle, optimizing the advancing direction of the inspection unmanned vehicle, generating a track space of the inspection unmanned vehicle and evaluating the track space, judging whether the inspection unmanned vehicle and the obstacle collide, regenerating a track route if the inspection unmanned vehicle and the obstacle can not collide, and selecting an optimal track and generating an optimal path if the inspection unmanned vehicle and the moving obstacle do not collide, wherein the method comprises the following specific operation modes: step 1, collecting data and fusing the data; The method comprises the steps of fusing laser radar, an odometer and IMU information which are arranged on a vehicle body, analyzing and determining the position of the inspection unmanned vehicle according to acquired data information, and sampling the speed of the inspection unmanned vehicle; Step 2, judging the running direction of the inspection unmanned vehicle and the moving obstacle; Acquiring the speed of the moving obstacle according to the laser radar, and judging whether the patrol unmanned vehicle and the moving obstacle can run in the same direction or not through an included angle formed by the direction of the patrol unmanned vehicle speed and the direction of the moving obstacle speed; Step 3, optimizing the advancing direction of the inspection unmanned vehicle according to the judging result; When the included angle between the combined speed direction of the movement speed of the inspection unmanned vehicle and the movement speed of the movement obstacle and the movement speed direction of the movement obstacle is an acute angle, judging that the inspection unmanned vehicle and the movement obstacle can run in the same direction; the method comprises the steps of aiming at the prediction of the patrol unmanned vehicle on the moving obstacle in the path planning process, adding an evaluation function barr (vc, vb) of the combined speed into the evaluation function of the patrol unmanned vehicle, namely evaluating the combined speed of the movement speed of the patrol unmanned vehicle and the movement speed of the moving obstacle and the moving speed of the moving obstacle, wherein the evaluation function of the patrol unmanned vehicle is as follows: ; normalization processing is carried out on the assessment function to obtain the following steps: ; ; ; ; In the formula, The method comprises the steps of evaluating the total speed of the movement speed of the inspection unmanned vehicle, the total speed of the movement speed of the moving obstacle and the included angle of the travelling crane of the movement speed of the moving obstacle as an evaluation function of the total speed; Step 4, generating a track space of the inspection unmanned vehicle and evaluating the track space; Judging whether the inspection unmanned vehicle collides with the obstacle, and regenerating a track route if the inspection unmanned vehicle collides with the obstacle; And 5, selecting an optimal track and generating an optimal path when the inspection unmanned vehicle traverses to the target point, and ending path planning.
2. The improved method for the unmanned aerial vehicle based on dynamic path planning is characterized in that the speed of the unmanned aerial vehicle is sampled in the step 1, the specific operation mode is that the radius of wheels of the unmanned aerial vehicle is known to be r, the speed of the unmanned aerial vehicle is the average speed of the wheels of the unmanned aerial vehicle, a rotating speed measuring unit is arranged on each of the left wheel and the right wheel in a chassis of the unmanned aerial vehicle, the rotating speeds of the two wheels of the unmanned aerial vehicle at the moment can be obtained through the rotating speed measuring unit, and therefore the running speed of the unmanned aerial vehicle is obtained, and the specific formula is as follows: ; ; ; In the above formula, c is the circumference of the wheel, that is, the distance travelled by the wheel in one revolution, a l 、a r is the rotational speed of the left and right wheels, v l 、v r is the speed of the left and right wheels, and v c is the speed of the inspection vehicle at the moment.
3. The improved method for the unmanned inspection vehicle based on dynamic path planning is characterized in that in the step 2, the speed of the movable obstacle is collected according to the laser radar, the specific operation mode is that the unmanned inspection vehicle scans and builds costmap grid map through the laser radar, the laser radar probe continuously emits laser to the periphery, the interval time from the emission of the laser by the emitter to the return of the laser to the receiver after the laser contacts the movable obstacle is t, the position of the movable obstacle scanned by the laser on the grid map is set as (x, y), the laser probe continuously emits the laser to the periphery and collects data, the position reached after the movable obstacle is limited can be obtained, the more the number of position sampling is, the more accurate the collected speed is, and the collection formula of the speed is as follows: ; ; ; Wherein, X ti is the distance between the previous time position and the next time position, namely the distance travelled by the moving obstacle in the time ti, v ti is the instantaneous speed of the moving obstacle in the time ti, v b is the average speed travelled by the moving obstacle, the more the positions of the moving obstacle are collected, the more accurate the value of v b is, and the smaller the obtained error is; Therefore, the speed and the direction of the inspection vehicle and the moving obstacle can be known, the speed at the moment is a vector, an included angle is formed by the extending lines of the speed direction and the direction of the inspection vehicle and the moving obstacle through vertical translation on the same two-dimensional plane, and the possibility of whether the inspection vehicle and the moving obstacle run in the same direction is judged according to the included angle.
4. The improvement method of the inspection vehicle based on the dynamic path planning of claim 1, wherein the step 3 optimizes the advancing direction of the inspection vehicle according to the judgment result, and the specific operation mode is as follows: the first step, establishing a rectangular coordinate system by taking an intersection point as an origin of coordinates at an included angle according to the known speed and direction of the inspection unmanned vehicle and the moving obstacle, and calculating to obtain the combined speed and direction of the inspection unmanned vehicle and the moving obstacle by utilizing a parallelogram rule; judging whether the same-direction running occurs or not, wherein in a rectangular coordinate system established at the included angle, if the directions of the two speeds are positioned in adjacent or same quadrant in I, II, III, IV four quadrant coordinate systems, the possibility of the same-direction running is considered, and the included angle between the combined speed of the two speeds and the speed of the moving obstacle is an acute angle; And thirdly, when the possibility of the same-direction running occurs, knowing the speed and the direction of the moving obstacle, changing the running direction of the inspection unmanned vehicle by optimizing, and changing the direction of the combined speed according to the parallelogram rule so as to change the included angle between the combined speed and the moving obstacle, and adjusting the included angle into an obtuse angle by optimizing and controlling the running direction of the inspection unmanned vehicle, so that the possibility of the same-direction running of the inspection unmanned vehicle and the moving obstacle can be avoided, and the purpose of avoiding the collision is achieved.
5. The method for improving the unmanned aerial vehicle based on dynamic path planning, as set forth in claim 1, wherein the continuous scanning of the environment by the laser radar records the moving speed of the moving obstacle, and when the included angle between the moving speed of the unmanned aerial vehicle and the moving speed of the moving obstacle is an acute angle, the moving speed of the unmanned aerial vehicle is taken Travel speed with dynamic obstacle The combination speed of (2) is Taking out Is the normal vector of (2) The improved travelling speed of the inspection unmanned vehicle And (3) with The combination speed of (2) is Solving the speed of the inspection unmanned vehicle after improvement The rotation speed of the four wheels of the inspection unmanned vehicle can be obtained through the target speed, and the rotation speed of the four wheels of the inspection unmanned vehicle is controlled through the raspberry group to realize the optimization of the running direction of the inspection unmanned vehicle, wherein the optimization formula is as follows: ; ; ; ; In the formula, In order to inspect the travel speed of the unmanned vehicle, For the travelling speed of the dynamic obstacle, Is that And (3) with Is used for the combination of the speed of the two, Is that Is characterized by a normal vector of (c), In order to improve the travelling speed of the rear inspection unmanned vehicle.
6. The improved method for the unmanned aerial vehicle for patrol inspection based on dynamic path planning of claim 5, wherein the track space of the unmanned aerial vehicle for patrol inspection is generated in the step 4, and the specific operation mode is that according to the running direction of the unmanned aerial vehicle for patrol inspection in the step 3, potential safety hazards caused by collision when the unmanned aerial vehicle runs in the same direction with a moving obstacle are avoided, a new running route is planned while the moving obstacle is avoided, the unmanned aerial vehicle for patrol inspection can continue to scan the moving obstacle to plan the running route to avoid the obstacle when running according to the planned new running route, and the new running route generated by obstacle avoidance of the unmanned aerial vehicle for patrol inspection is the track space.
7. The improved method for the inspection vehicle based on the dynamic path planning is characterized by comprising the step 4 of evaluating the track route in the generated track space, wherein the specific operation mode is that whether the running route of the inspection vehicle generated in the track space collides with a moving obstacle is judged, if the running route collides with the moving obstacle, the step 3 is executed again to generate the latest track space, and if the running route does not collide, the track route with the shortest route and the best obstacle avoidance effect is selected.
8. The method for improving the unmanned aerial vehicle for inspection based on dynamic path planning, which is characterized in that the method for improving the unmanned aerial vehicle for inspection based on dynamic path planning is characterized in that the method for producing the optimal path in the step 5 is characterized in that the method for producing the optimal path comprises the following steps of evaluating and judging according to the optimal track selected in the step 4, judging whether the optimal track travelled by the unmanned aerial vehicle for inspection reaches a target point, if so, producing the optimal path, otherwise, producing a track space diagram, and repeatedly executing the step 3 and the step 4 until the unmanned aerial vehicle for inspection safely reaches the target point.

Description

Inspection unmanned vehicle improvement method based on dynamic path planning Technical Field The invention relates to the technical field of unmanned vehicle obstacle avoidance, in particular to an inspection unmanned vehicle improvement method based on dynamic path planning. Background In recent years, path planning and navigation positioning technology of an inspection unmanned vehicle are mature gradually, compared with the implementation of obstacle avoidance of fixed obstacles in the process of path planning of unmanned vehicles, how to realize real-time obstacle avoidance and path planning of dynamic obstacles becomes a technical problem, while a DWA algorithm starts to be widely applied to development and application of local path planning by virtue of the advantages of low computational complexity, real-time obstacle avoidance and the like, the implementation principle of the algorithm is to introduce an evaluation function into a generated track route to evaluate the track so as to select an optimal track, but compared with global planning algorithms such as A, dijkstra and the like, the algorithm only selects the optimal path at the current moment when the dynamic obstacles are in real time and does not take the optimal path based on global consideration, so that the finally generated track route is not the optimal track route under the global condition. The algorithm performs normalization processing on the evaluation function to avoid the situation that the path track finally selected is not the optimal track because a certain single item does not dominate the evaluation function, but performs error processing and error reduction on the path track obtained by the DWA algorithm on the basis of the path track, and does not change substantially. Disclosure of Invention In order to solve the technical problems, the invention provides an inspection unmanned vehicle improvement method based on dynamic path planning, which improves the evaluation function, aims at the prediction of the inspection unmanned vehicle on dynamic obstacles in the path planning process, adds an evaluation function barr (v c,vb) which is the sum of speeds into the evaluation function to optimize the errors of dynamic obstacle avoidance and path planning, can better reduce the errors to obtain an optimal track which tends to be considered globally, and can effectively solve the technical problems. The invention is realized by the following technical scheme: The improved inspection vehicle method based on dynamic path planning comprises the steps of obtaining the position of an inspection vehicle through fusion of laser radar, an odometer and IMU information, sampling the speed of the inspection vehicle, collecting the speed of a moving obstacle, judging whether the inspection vehicle and the moving obstacle can run in the same direction or not by judging the included angle between the movement speed direction of the inspection vehicle and the movement speed direction of the moving obstacle, optimizing the advancing direction of the inspection vehicle, generating a track space of the inspection vehicle, evaluating the track space, judging whether the inspection vehicle and the obstacle collide, regenerating a track route if the inspection vehicle and the obstacle collide, and selecting an optimal track and generating an optimal path if the inspection vehicle and the moving obstacle do not collide, wherein the method comprises the following specific operation modes: step 1, collecting data and fusing the data; The method comprises the steps of fusing laser radar, an odometer and IMU information which are arranged on a vehicle body, analyzing and determining the position of the inspection unmanned vehicle according to acquired data information, and sampling the speed of the inspection unmanned vehicle; Step 2, judging the running direction of the inspection unmanned vehicle and the moving obstacle; Acquiring the speed of the moving obstacle according to the laser radar, and judging whether the patrol unmanned vehicle and the moving obstacle can run in the same direction or not through an included angle formed by the direction of the patrol unmanned vehicle speed and the direction of the moving obstacle speed; Step 3, optimizing the advancing direction of the inspection unmanned vehicle according to the judging result; When the included angle between the combined speed direction of the movement speed of the inspection unmanned vehicle and the movement speed of the movement obstacle and the movement speed direction of the movement obstacle is an acute angle, judging that the inspection unmanned vehicle and the movement obstacle can run in the same direction; Step 4, generating a track space of the inspection unmanned vehicle and evaluating the track space; Judging whether the inspection unmanned vehicle collides with the obstacle, and regenerating a track route if the inspection unmanned vehicle collides with the obstacle; And 5, s