CN111813100B - Local path planning algorithm and device - Google Patents

Local path planning algorithm and device Download PDF

Info

Publication number
CN111813100B
CN111813100B CN201910602244.XA CN201910602244A CN111813100B CN 111813100 B CN111813100 B CN 111813100B CN 201910602244 A CN201910602244 A CN 201910602244A CN 111813100 B CN111813100 B CN 111813100B
Authority
CN
China
Prior art keywords
gap
mobile device
mobile equipment
destination
max
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
Application number
CN201910602244.XA
Other languages
Chinese (zh)
Other versions
CN111813100A (en
Inventor
秦家虎
高炤
王帅
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Science and Technology of China USTC
Original Assignee
University of Science and Technology of China USTC
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Science and Technology of China USTC filed Critical University of Science and Technology of China USTC
Priority to CN201910602244.XA priority Critical patent/CN111813100B/en
Publication of CN111813100A publication Critical patent/CN111813100A/en
Application granted granted Critical
Publication of CN111813100B publication Critical patent/CN111813100B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory

Abstract

A local path planning algorithm and a device are used for path planning of mobile equipment, the mobile equipment is loaded with a sensor, and the algorithm comprises the following steps: s1, judging whether the mobile equipment arrives at the destination, if so, ending, otherwise, executing the step S2; s2, obtaining a candidate gap set according to the sensor data; s3, selecting a nearest gap from the candidate gap set as a target gap; s4, determining the moving direction of the mobile equipment according to the target gap; s5, controlling the mobile equipment to move along the target direction by adopting an improved dynamic window method; s6, repeating the steps S1-S5 until the mobile device reaches the destination or no feasible gap exists. And planning the path of the mobile equipment by adopting a clearance-based and improved dynamic window method so as to reduce the condition of local extreme values and improve the obstacle avoidance performance.

Description

Local path planning algorithm and device
Technical Field
The invention relates to the technical field of path planning of mobile equipment, in particular to a local path planning algorithm and a local path planning device.
Background
The path planning algorithm mainly comprises global path planning and local path planning, a dynamic window method is a typical local path planning algorithm, the dynamic window method considers the actual dynamic constraint and the kinematic constraint of the robot, samples a plurality of groups of speeds in a speed window in a speed space, predicts the formed tracks in a certain time, evaluates the tracks by using an evaluation function, and finally selects the speed corresponding to the track with the highest evaluation as the speed input of the current moment. The dynamic window method mainly utilizes an evaluation function to evaluate each track, and then selects a speed corresponding to a track with the highest evaluation as a speed input, so that the selection of the final speed has a direct relation with the evaluation function, and the evaluation is higher as the included angle between the track direction and the target direction is smaller, so that the dynamic window method is more inclined to select the track moving towards the target direction, however, when an obstacle exists in front of the robot, the expected track not only needs to move towards the target but also needs to be far away from the obstacle, and the generation of the contradiction causes the robot to fall into a local minimum point, which is particularly represented by continuous cyclic reciprocation or even stagnation near the position; in addition, when the dynamic window method evaluates the track, the speed corresponding to the track crossing the obstacle is not directly eliminated, but a low evaluation is given, so that the situation that the evaluation of other evaluation function items is still high can occur, and the optimal track is obtained to be generated by the group of speeds, so that the robot collides with the obstacle with a high probability.
Disclosure of Invention
Technical problem to be solved
Based on the technical problem, the invention provides a local path planning algorithm and a device, which plan a path of a mobile device by adopting a method based on a gap and an improved dynamic window so as to reduce the situation of a local extreme value and improve the obstacle avoidance performance.
(II) technical scheme
In a first aspect, the present invention provides a local path planning algorithm for path planning of a mobile device, the mobile device being loaded with a sensor, the algorithm comprising: s1, judging whether the mobile equipment arrives at the destination, if so, ending, otherwise, executing step S2; s2, obtaining a candidate gap set according to the sensor data; s3, selecting a nearest gap from the candidate gap set as a target gap; s4, determining the moving direction of the mobile equipment according to the target gap; s5, controlling the mobile equipment to move along the target direction by adopting an improved dynamic window method; s6, repeating the steps S1-S5 until the mobile device reaches the destination or no feasible gap exists.
Optionally, step S2 specifically includes: s21, analyzing the sensor data, obtaining the clearance between the obstacles in the sensor data, and obtaining a plurality of clearance data, wherein the clearance data form a clearance set; and S22, deleting the gaps with the width smaller than the diameter of the mobile equipment in the gap set to obtain a candidate gap set.
Optionally, step S21 is specifically: the orientation of the mobile device is taken as a starting point, sampling points are scanned at preset angles at intervals in the counterclockwise direction, and the sampling points are numbered as 0, 1, …, i +1, … and N-1, wherein if the ith sampling point and the (i + 1) th sampling point meet the following conditions: ρ is a unit of a gradienti＜Rmax&&ρi+1＝RmaxIf the sampling point is the first type sampling point, the ith sampling point is the first type sampling point; if the ith sampling point and the (i + 1) th sampling point meet the following conditions: ρ is a unit of a gradienti＝Rmax&&ρi+1＜RmaxThe i +1 th sample point is the second type of sample point, where RmaxFor the maximum distance, p, scanned by the sensoriIs the distance, rho, of the ith sample point from the mobile devicei+1The distance between the (i + 1) th sampling point and the mobile equipment is calculated; to be adjacent and in order between the first type of sample point and the second type of sample pointThe gaps are sequentially set as a first gap, a second gap, … … and an Mth gap, and the first gap, the second gap and … … form a gap set.
Optionally, step S3 specifically includes: judging the relation between the destination and the gaps in the candidate gap set: if the destination is within a gap of the candidate gap set, the gap is the target gap; and if the destination is not in any gap of the candidate gap set, setting the gap with the minimum included angle between the mobile equipment and the destination as a target gap.
Optionally, setting the gap having the smallest included angle with the connection line between the mobile device and the destination as the target gap specifically includes: and judging the included angle between the boundary of the two gaps and the connecting line, and setting the gap corresponding to the boundary with the smaller included angle as a target gap.
Optionally, step S4 specifically includes: s41, obtaining a critical deviation angle, wherein the critical deviation angleδThe calculation formula of (c) is:
Figure BDA0002118847830000031
wherein R isrobRadius of the mobile device, DsFor a safe distance, θgIs the angular width of the gap, DgThe distance width between the obstacles forming the gap and the obstacles; s42, determining a safety region according to the critical deviation angle; s43, determining the moving direction of the mobile device according to the safety area.
Optionally, the determining the safety region according to the critical deviation angle in step S42 specifically includes: two boundaries of the gap move delta towards the gap to form two new boundaries, and the area between the two new boundaries is a safe area.
Optionally, step S43 specifically includes: if the connection direction of the mobile equipment and the destination is in a safe area, directly taking the connection direction as the moving direction; otherwise, acquiring an included angle between the two new boundaries and a connecting line between the mobile equipment and the destination, and taking the new boundary with a smaller included angle as the moving direction of the mobile equipment.
Optionally, an improved dynamic windowing packageComprises the following steps: s51, determining a speed sampling space according to the speed constraint and the environment constraint of the mobile equipment, wherein the speed sampling space is VrThe calculation formula of (2) is as follows: vr＝Vs∩Va∩VdWherein, Vs＝{(v，w)|v∈[0，vmax]∧w∈[wmin，wmax]}，vmaxMaximum linear velocity, w, of the mobile deviceminAnd wmaxRespectively a minimum angular velocity and a maximum angular velocity of the mobile device;
Figure BDA0002118847830000032
dist (v, w) is the stopping distance of the mobile device,
Figure BDA0002118847830000033
and
Figure BDA0002118847830000034
maximum acceleration and maximum angular acceleration of the mobile device, respectively;
Figure BDA0002118847830000035
vaand waLinear velocity and angular velocity of the current mobile equipment; s52, performing track prediction on each speed in the speed sampling space; and S53, evaluating the tracks according to an evaluation function, taking the speed corresponding to the track with the highest evaluation as the speed of the mobile equipment, wherein the calculation formula of the evaluation function is as follows: g (v, w) ═ α · leading (v, w) + β · obs (v, w) + γ · vel (v, w), where α, β, γ are coefficients,
Figure BDA0002118847830000041
theta is an included angle between the orientation of the mobile equipment at the track end position and the destination direction;
Figure BDA0002118847830000042
Robsis the minimum distance, R, from the obstacle to the center of the mobile device trajectoryrobIs the radius of the mobile device, if RobsLess than RrobThen delete the corresponding speed of the track, DsIn order to be a safe distance from the user,
Figure BDA0002118847830000043
Figure BDA0002118847830000044
in another aspect, the present invention provides a local path planning apparatus, including: the judging module is used for judging whether the mobile equipment reaches the destination or not, if so, ending, otherwise, executing the obtaining module; the acquisition module is used for acquiring a candidate gap set according to the sensor data; a selection module for selecting a nearest gap from the candidate gap set as a target gap; a determining module, configured to determine a moving direction of the mobile device according to a target gap; the control module is used for controlling the mobile equipment to move along the target direction by adopting an improved dynamic window method; and the repeating module is used for repeatedly executing the functions of the judging module, the obtaining module, the selecting module, the determining module and the control module until the mobile equipment reaches the destination or no feasible gap exists.
(III) advantageous effects
The invention provides a local path planning algorithm and a device, which at least achieve the following beneficial effects:
the method comprises the steps of loading a sensor on the mobile equipment to detect obstacles in the surrounding environment, obtaining a gap which can be passed by the current mobile equipment by analyzing data collected by the sensor, then obtaining a gap which is closest to a destination, further calculating the moving direction of the mobile equipment, and finally moving the mobile equipment towards the moving direction by using an improved dynamic window method, so that the mobile equipment does not always move towards the destination, and more depends on the current obstacle distribution, thereby greatly reducing the situation of falling into local minimum points;
the evaluation function of a typical dynamic window method is optimized, the evaluation item of the predicted track direction is changed into the size of the included angle between the track terminal direction and the moving direction by the algorithm, meanwhile, in order to improve the safety of obstacle avoidance, the group of speeds of the predicted track blocked by obstacles are directly eliminated, and the minimum distance between the obstacle and the track in the track safety range is used as the evaluation item, so that the performance of the dynamic window method is improved.
Drawings
FIG. 1 schematically illustrates a motion environment of a mobile device of an embodiment of the disclosure;
FIG. 2 schematically illustrates a flow chart of a local path planning algorithm of an embodiment of the present disclosure;
FIG. 3 schematically illustrates a step diagram of a local path planning algorithm of an embodiment of the present disclosure;
FIG. 4 is a schematic diagram illustrating a scanning range of a mobile device at a time according to an embodiment of the disclosure;
FIG. 5 schematically illustrates a safety zone calculation schematic of travel of a mobile device of an embodiment of the disclosure;
FIG. 6 schematically illustrates a computational schematic of an evaluation function of an embodiment of the present disclosure;
fig. 7 schematically shows a schematic path of a robot through the local path planning method of an embodiment of the present disclosure.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail with reference to the following embodiments and the accompanying drawings.
According to the embodiment of the invention, the mobile device is taken as a robot as an example to explain the scheme in detail, when the robot moves on the ground, a plurality of scattered obstacles (as shown in fig. 1) often exist in the surrounding environment, so that the robot needs to effectively avoid the obstacles in the moving process to reach a destination.
In a first aspect, the present invention provides a local path planning algorithm for path planning of a mobile device, the mobile device being loaded with a sensor, see fig. 2 and 3, the algorithm comprising: s1, judging whether the mobile equipment arrives at the destination, if so, ending, otherwise, executing the step S2; s2, obtaining a candidate gap set according to the sensor data; s3, selecting a nearest gap from the candidate gap set as a target gap; s4, determining the moving direction of the mobile equipment according to the target gap; s5, controlling the mobile equipment to move along the target direction by adopting an improved dynamic window method; s6, repeating the steps S1-S5 until the mobile device reaches the destination or no feasible gap exists. The following detailed description will be given by way of examples.
S1, judging whether the mobile equipment arrives at the destination, if so, ending, otherwise, executing step S2;
specifically, the robot needs to be initialized, data in the robot sensor is read for subsequent reference, and whether the current position is the destination or not is determined, if yes, the robot does not move any more, and if not, the following step S2 is executed.
S2, obtaining a candidate gap set according to the sensor data;
if the robot is initialized and is not found at the destination, the path of the robot needs to be planned, so that the robot can effectively avoid obstacles to reach the destination. The robot is in the process of marcing and is constantly scanning in order to collect surrounding environment data to surrounding environment, as shown in fig. 4, the sensor sets up the sampling point, obtains sensor data through scanning the sampling point, and the concrete analytic mode is as follows:
s21, analyzing the sensor data, obtaining the clearance between the obstacles in the sensor data, and obtaining a plurality of clearance data, wherein the clearance data form a clearance set;
as shown in fig. 4, the sampling points are scanned at preset angles at intervals in the counterclockwise direction with the orientation of the robot at the moment as a starting point, and are numbered as 0, 1, …, i +1, …, and N-1, the distance from the obtained scanning point to the sensor is smaller than the scanning radius of the sensor if the scanning point is on the obstacle, and the obtained sampling point is the scanning radius of the sensor if the scanning point is in the gap between the obstacle and the obstacle, so the boundary of the obstacle becomes the important point to be analyzed, and therefore, it is defined that if the ith sampling point and the (i + 1) th sampling point satisfy: rhoi＜Rmax&&ρi+1＝RmaxThe ith sampleThe point is a first type sampling point, namely the ith sampling point is the boundary of the obstacle, and the (i + 1) th sampling point is in the gap, so that the sampling point is called as the first type sampling point; if the ith sampling point and the (i + 1) th sampling point meet the following conditions: rhoi＝Rmax&&ρi+1＜RmaxThen the (i + 1) th sampling point is the second kind of sampling point, that is, the (i) th sampling point is the point in the gap, and the (i + 1) th point is on the obstacle, where RmaxIs the maximum distance, p, scanned by the sensoriIs the distance, rho, of the ith sampling point from the roboti+1The distance between the (i + 1) th sampling point and the robot. Thereby a gap between the obstacle and the obstacle is obtained. The gap within the sensor scanning range at a certain time can be obtained by analyzing the sensor data at that time.
Gaps which are adjacent and sequentially arranged between the first type of sampling point and the second type of sampling point are sequentially set as a first gap, a second gap, … … and an Mth gap, and the first gap, the second gap and … … form a gap set. In the embodiment of the present invention, for convenience of description, if the ith point is the first-type scanning point, the jth point is set as the second-type scanning point, that is, the jth-1 point is in the gap, and the jth point is on the obstacle, as can be seen from the above, j equals i + m, where the number of scanning points in the gap is m-1, and thus the gap (i, j) can be obtained (see fig. 4).
And S22, deleting the gaps with the width smaller than the diameter of the mobile equipment in the gap set to obtain a candidate gap set.
Because the gap robot with the gap smaller than the diameter of the robot is definitely unable to go, the gap with the gap width smaller than the diameter of the robot is directly deleted so as to preliminarily screen the gaps in the gap set, reduce subsequent calculation amount and obtain a candidate gap set.
S3, selecting a nearest gap from the candidate gap set as a target gap;
specifically, since the candidate gap set is obtained in step S2, it is necessary to plan the path of the robot and determine which gap to pass through, and therefore it is necessary to determine the relationship between the destination and the gap in the candidate gap set:
if the destination is in a certain gap in the candidate gap set, the gap is the target gap;
and if the destination is not in any gap in the candidate gap set, setting the gap with the minimum included angle between the robot and the destination as the target gap.
Specifically, if the destination is within a sector range formed by two boundaries of a certain gap in the gap set, the gap is a target gap, and the robot can pass through the target gap directly.
And if the destination is not in any gap in the candidate gap set, setting the gap with the minimum included angle between the gap boundary and the connecting line of the robot and the destination as the target gap.
S4, determining the moving direction of the mobile equipment according to the target gap;
as can be seen from step S3, since a certain safe distance needs to be maintained with the obstacle when the vehicle passes through the target gap, the safe distance needs to be calculated, which specifically includes the following steps:
s41, obtaining a critical offset angle, wherein the calculation formula of the critical offset angle delta is as follows:
Figure BDA0002118847830000081
wherein R isrobRadius of the robot, DsFor a safe distance, θgIs the angular width of the gap, DgThe distance width between the obstacles forming the gap and the obstacles;
s42, determining a safety region according to the critical deviation angle;
two new boundaries are formed after the two boundaries of the gap rotate delta towards the gap by taking the sensor as a circle center, and a sector area between the two new boundaries is a safe area.
As shown in FIG. 5, for the gap (i, j), if the angle of the right side boundary of the gap is θcAngle of the left side boundary of the gap is thetafThen the angles of the new boundary are respectively: theta'c＝θc+δ，θ′f＝θf- δ, thus obtainingAngle of safety region of [ theta'c，θ′f]。
And S43, determining the moving direction of the mobile device according to the safety area.
After the safe area is obtained, particularly when the safe area is large, a path needs to be further planned to enable the driving path of the robot to be shortest, an included angle between the two new boundaries and a connecting line between the robot and the destination needs to be further obtained, the new boundary with the smaller included angle is used as the moving direction of the robot, and therefore the driving path of the robot is shortest. If the connection direction of the mobile equipment and the destination is in a safe area, directly taking the connection direction as the moving direction; otherwise, acquiring an included angle between the two new boundaries and a connecting line between the mobile equipment and the destination, and taking the new boundary with the smaller included angle as the moving direction of the mobile equipment.
From the above, the moving direction θ of the robotsgComprises the following steps:
Figure BDA0002118847830000082
wherein, thetagoalIs the angle of the destination.
S5, controlling the mobile equipment to move along the target direction by adopting an improved dynamic window method;
specifically, the improved dynamic window method comprises:
s51, determining a speed sampling space according to the speed constraint and the environment constraint of the mobile equipment, wherein the speed sampling space is VrThe calculation formula of (2) is as follows:
Vr＝Vs∩Va∩Vd
wherein, VsThe possible linear and angular velocity combinations for the robot, whose range is determined by the maximum linear velocity, the minimum linear velocity, the maximum angular velocity and the minimum angular velocity, Vs＝{(v，w)|v∈[0，vmax]∧w∈['wmin，wmax]}，vmaxMaximum linear velocity of the robot, wminAnd wmaxRespectively the minimum angular velocity and the maximum angular velocity of the robot; vaIn order to take possible speed and angular speed combinations after the braking distance of the robot into consideration, the robot can be guaranteed to stop before colliding with the obstacle in the range,
Figure BDA0002118847830000091
dist (v, w) is the braking distance of the robot,
Figure BDA0002118847830000092
and
Figure BDA0002118847830000093
maximum acceleration and maximum angular acceleration of the robot, respectively;
Figure BDA0002118847830000094
vaand waThe linear velocity and the angular velocity of the current robot.
S52, performing track prediction on each speed in the speed sampling space;
s53, evaluating the trajectory according to an evaluation function, and taking a speed corresponding to the trajectory with the highest evaluation as the speed of the mobile device, where a calculation formula of the evaluation function is:
G(v，w)＝α·heading(v，w)+β·obs(v，w)+γ·vel(v，w)
wherein alpha, beta and gamma are coefficients,
Figure BDA0002118847830000095
theta is an included angle between the orientation of the robot at the track end position and the destination direction, as shown in fig. 6;
Figure BDA0002118847830000096
Robsis the minimum distance, R, of an obstacle to the center of the robot trajectoryrobIs the radius of the robot, if RobsLess than RrobThen the speed corresponding to the track is deleted, DsIn order to be a safe distance from the user,
Figure BDA0002118847830000097
s6, repeating the steps S1-S5 until the mobile device reaches the destination or no feasible gap exists.
As shown in fig. 7, a schematic path of the local path planning method is used for the robot.
In a second aspect, the present application discloses a local path planning apparatus, which includes a determining module, an obtaining module, a selecting module, a determining module, a controlling module, and a repeating module, wherein:
the judging module is used for judging whether the mobile equipment reaches the destination, if so, the judgment is finished, otherwise, the obtaining module is executed;
the acquisition module is used for acquiring a candidate gap set according to the sensor data;
the selection module is used for selecting one nearest gap from the candidate gap set as a target gap;
the determining module is used for determining the moving direction of the mobile equipment according to the target gap;
the control module is used for controlling the mobile equipment to move along the target direction by adopting an improved dynamic window method;
and the repeating module is used for repeatedly executing the functions of the judging module, the obtaining module, the selecting module, the determining module and the control module until the mobile equipment reaches the destination or no feasible gap exists.
The above-mentioned embodiments are intended to illustrate the objects, technical solutions and advantages of the present invention in further detail, and it should be understood that the above-mentioned embodiments are only exemplary embodiments of the present invention, and are not intended to limit the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (8)

1. A local path planning algorithm for path planning for a mobile device, the mobile device being loaded with sensors, the algorithm comprising:
s1, judging whether the mobile equipment arrives at the destination, if so, ending, otherwise, executing step S2;
s2, obtaining a candidate gap set according to the sensor data;
s3, selecting a nearest gap from the candidate gap set as a target gap;
s4, determining the moving direction of the mobile equipment according to the target gap;
the step S4 specifically includes:
s41, obtaining a critical offset angle, where the critical offset angle δ is calculated by:
Figure FDA0003579998210000011
wherein R isrobRadius of the mobile device, DsFor a safe distance, θgIs the angular width of the gap, DgThe distance width between the obstacles forming the gap and the obstacles;
s42, determining a safety region according to the critical deviation angle;
s43, determining the moving direction of the mobile device according to the safety area;
s5, controlling the mobile equipment to move along the target direction by adopting an improved dynamic window method;
the improved dynamic windowing method comprises:
s51, determining a speed sampling space according to the speed constraint and the environment constraint of the mobile equipment, and determining a speed sampling space VrThe calculation formula of (2) is as follows:
Vr＝Vs∩Va∩Vd
wherein, Vs＝{(v，w)|v∈[0，vmax]∧w∈[wmin，wmax]}，vmaxIs the maximum linear velocity, w, of the mobile deviceminAnd wmaxA minimum angular velocity and a maximum angular velocity of the mobile device, respectively;
Figure FDA0003579998210000012
dist (v, w) is the stopping distance of the mobile device,
Figure FDA0003579998210000013
and
Figure FDA0003579998210000014
maximum acceleration and maximum angular acceleration of the mobile device, respectively;
Figure FDA0003579998210000021
vaand waLinear velocity and angular velocity of the current mobile equipment;
s52, predicting the track of each speed in the speed sampling space;
s53, evaluating the trajectory according to an evaluation function, and taking a speed corresponding to the trajectory with the highest evaluation as the speed of the mobile device, where a calculation formula of the evaluation function is:
G(v，w)＝α·heading(v，w)+β·obs(v，w)+γ·vel(v，w)
wherein alpha, beta and gamma are coefficients,
Figure FDA0003579998210000022
theta is an included angle between the orientation of the mobile equipment at the track end position and the destination direction;
Figure FDA0003579998210000023
Robsis the minimum distance, R, of an obstacle to the center of the mobile device trajectoryrobIs the radius of the mobile device, if RobsLess than RrobThen the speed corresponding to said track is deleted, DsIn order to be a safe distance away from the vehicle,
Figure FDA0003579998210000024
Figure FDA0003579998210000025
s6, repeating the steps S1-S5 until the mobile device reaches the destination or no feasible gap exists.
2. The local path planning algorithm according to claim 1, wherein the step S2 specifically includes:
s21, analyzing the sensor data, obtaining the clearance between obstacles in the sensor data, and obtaining a plurality of clearance data, wherein the clearance data form a clearance set;
and S22, deleting the gaps with the width smaller than the diameter of the mobile equipment in the gap set to obtain the candidate gap set.
3. The local path planning algorithm according to claim 2, wherein the step S21 specifically includes:
taking the orientation of the mobile device as a starting point, scanning sampling points at preset angles at intervals in the counterclockwise direction, and numbering the sampling points as 0, 1, …, i +1, … and N-1, wherein if the ith sampling point and the (i + 1) th sampling point meet: rhoi＜Rmax&&ρi+1＝RmaxIf said ith sample point is a first type of sample point; if the ith sampling point and the (i + 1) th sampling point meet the following conditions: rhoi＝Rmax&&ρi+1＜RmaxThen the i +1 th sampling point is the second kind of sampling point, where RmaxIs the maximum distance, p, scanned by the sensoriDistance, p, of the ith sampling point from the mobile devicei+1The distance between the (i + 1) th sampling point and the mobile equipment;
and setting gaps which are adjacent and sequentially arranged between the first type of sampling point and the second type of sampling point as a first gap, a second gap, … … and an Mth gap in sequence, wherein the first gap, the second gap, … … and the Mth gap form the gap set.
4. The local path planning algorithm according to claim 1, wherein the step S3 specifically includes:
determining a relationship of the destination to a gap in the set of candidate gaps:
if the destination is within a gap of the candidate gap set, the gap is a target gap;
and if the destination is not in any gap in the candidate gap set, setting a gap with the smallest included angle with the connection line of the mobile equipment and the destination as a target gap.
5. The local path planning algorithm according to claim 4, wherein the setting of the gap having the smallest included angle with the mobile device and the destination line as the target gap is specifically:
and judging the included angle between the boundary of the two gaps and the connecting line, and setting the gap corresponding to the boundary with the smaller included angle as a target gap.
6. The local path planning algorithm according to claim 1, wherein the determining of the safety region according to the critical deviation angle in step S42 specifically includes:
and two boundaries of the gap respectively move delta towards the gap to form two new boundaries, and the area between the two new boundaries is a safe area.
7. The local path planning algorithm according to claim 6, wherein the step S43 specifically includes:
if the connection direction of the mobile equipment and the destination is in the safe area, directly taking the connection direction as the moving direction; otherwise, acquiring an included angle between the two new boundaries and a connecting line between the mobile equipment and the destination, and taking the new boundary with the smaller included angle as the moving direction of the mobile equipment.
8. A local path planning apparatus comprising:
the judging module is used for judging whether the mobile equipment reaches the destination or not, if so, the operation is finished, otherwise, the acquiring module is executed, wherein the mobile equipment is loaded with a sensor;
the acquisition module is used for acquiring a candidate gap set according to the sensor data;
a selection module for selecting a nearest gap from the candidate gap set as a target gap;
a determining module, configured to determine a moving direction of the mobile device according to the target gap;
wherein, the determining module specifically comprises:
obtaining a critical offset angle, wherein the calculation formula of the critical offset angle δ is as follows:
Figure FDA0003579998210000041
wherein R isrobRadius of the mobile device, DsFor a safe distance, θgIs the angular width of the gap, DgThe distance width between the obstacles forming the gap and the obstacles;
determining a safety region according to the critical deviation angle;
determining a moving direction of the mobile device according to the safety area;
the control module is used for controlling the mobile equipment to move along the target direction by adopting an improved dynamic window method;
wherein the improved dynamic windowing method comprises:
determining a velocity sampling space, a velocity sampling space V, according to the velocity constraints and environmental constraints of the mobile devicerThe calculation formula of (2) is as follows:
Vr＝Vs∩Va∩Vd
wherein, Vs＝{(v，w)|v∈[0，vmax]∧w∈[wmin，wmax]}，vmaxIs the maximum linear velocity, w, of the mobile deviceminAnd wmaxRespectively a minimum angular velocity and a maximum angular velocity of the mobile device;
Figure FDA0003579998210000042
dist (v, w) is the stopping distance of the mobile device，
Figure FDA0003579998210000043
And
Figure FDA0003579998210000044
maximum acceleration and maximum angular acceleration of the mobile device, respectively;
Figure FDA0003579998210000045
vaand waLinear velocity and angular velocity of the current mobile equipment;
predicting the track of each speed in the speed sampling space;
evaluating the track according to an evaluation function, and taking the speed corresponding to the track with the highest evaluation as the speed of the mobile equipment, wherein the calculation formula of the evaluation function is as follows:
G(v，w)＝α·heading(v，w)+β·obs(v，w)+γ·vel(v，w)
wherein alpha, beta and gamma are coefficients,
Figure FDA0003579998210000051
theta is an included angle between the orientation of the mobile equipment at the track end position and the destination direction;
Figure FDA0003579998210000052
Robsis the minimum distance, R, of an obstacle to the center of the mobile device trajectoryrobIs the radius of the mobile device, if RobsLess than RrobThen the speed corresponding to said track is deleted, DsIn order to be a safe distance away from the vehicle,
Figure FDA0003579998210000053
Figure FDA0003579998210000054
and the repeating module is used for repeatedly executing the functions of the judging module, the obtaining module, the selecting module, the determining module and the controlling module until the mobile equipment reaches the destination or no feasible gap exists.
CN201910602244.XA 2019-07-04 2019-07-04 Local path planning algorithm and device Active CN111813100B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910602244.XA CN111813100B (en) 2019-07-04 2019-07-04 Local path planning algorithm and device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910602244.XA CN111813100B (en) 2019-07-04 2019-07-04 Local path planning algorithm and device

Publications (2)

Publication Number Publication Date
CN111813100A CN111813100A (en) 2020-10-23
CN111813100B true CN111813100B (en) 2022-07-15

Family

ID=72844037

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910602244.XA Active CN111813100B (en) 2019-07-04 2019-07-04 Local path planning algorithm and device

Country Status (1)

Country Link
CN (1) CN111813100B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112526991B (en) * 2020-11-25 2022-05-24 中国科学技术大学 Robot movement method, device, electronic equipment and storage medium
CN115328211B (en) * 2022-10-17 2022-12-27 复亚智能科技(太仓)有限公司 Unmanned aerial vehicle local path planning method

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108733038A (en) * 2017-04-18 2018-11-02 深圳市丰巨泰科电子有限公司 Robot local dynamic station paths planning method

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB0508217D0 (en) * 2005-04-25 2005-06-01 Renishaw Plc Method for scanning the surface of a workpiece
CN105911992B (en) * 2016-06-14 2019-02-22 广东技术师范学院 A kind of automatic path planning method and mobile robot of mobile robot
CN106484955B (en) * 2016-09-14 2019-05-14 中国科学院计算技术研究所 A kind of indoor arrangement evaluation method and system based on Voronoi diagram
US10429847B2 (en) * 2017-09-22 2019-10-01 Locus Robotics Corp. Dynamic window approach using optimal reciprocal collision avoidance cost-critic
CN107703948B (en) * 2017-11-14 2020-09-29 上海理工大学 Mobile robot local dynamic path planning method based on self-adaptive dynamic window
CN108958282B (en) * 2018-06-21 2020-08-14 浙江大学 Three-dimensional space path planning method based on dynamic spherical window

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108733038A (en) * 2017-04-18 2018-11-02 深圳市丰巨泰科电子有限公司 Robot local dynamic station paths planning method

Also Published As

Publication number Publication date
CN111813100A (en) 2020-10-23

Similar Documents

Publication Publication Date Title
CN109606263B (en) Method for monitoring blind spot of monitoring vehicle and blind spot monitor using the same
JP6207723B2 (en) Collision prevention device
JP6272566B2 (en) Route prediction device
CN109829351B (en) Method and device for detecting lane information and computer readable storage medium
JP6246609B2 (en) Self-position estimation apparatus and self-position estimation method
CN105916749B (en) Method for running vehicle
CN111161322B (en) LSTM neural network pedestrian trajectory prediction method based on human-vehicle interaction
CN111813100B (en) Local path planning algorithm and device
US20120116662A1 (en) System and Method for Tracking Objects
CN110713087B (en) Elevator door state detection method and device
CN110929796B (en) Multi-source sensor-based decision layer data fusion method and system and storage medium
US20160040979A1 (en) Apparatus and method for monitoring moving objects in sensing area
CN112526999A (en) Speed planning method, device, electronic equipment and storage medium
WO2021133745A1 (en) Resource prioritization based on travel path relevance
CN111707258B (en) External vehicle monitoring method, device, equipment and storage medium
KR20220041485A (en) Method and apparatus for tracking an object using LIDAR sensor, vehicle including the apparatus, and recording medium for recording program performing the method
WO2020164090A1 (en) Trajectory prediction for driving strategy
CN113015886A (en) Creation and updating of maps in off-street regions
EP3924901A1 (en) Trajectory prediction using deep learning multiple predictor fusion and bayesian optimization
CN110497906B (en) Vehicle control method, apparatus, device, and medium
WO2024027210A1 (en) Control method and apparatus for robot, electronic device, and storage medium
CN115309168B (en) Underground unmanned vehicle control method and device
Jia et al. Measuring the motion of vulnerable road users relative to moving HGVs
KR20230014008A (en) Method and apparatus for determining the possibility of collision of a driving vehicle using an artificial neural network
US10853644B2 (en) Method and system for determining possible geographic positions of an assumed undetected target

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