CN116382310B - Artificial potential field path planning method and system - Google Patents
Artificial potential field path planning method and system Download PDFInfo
- Publication number
- CN116382310B CN116382310B CN202310661875.5A CN202310661875A CN116382310B CN 116382310 B CN116382310 B CN 116382310B CN 202310661875 A CN202310661875 A CN 202310661875A CN 116382310 B CN116382310 B CN 116382310B
- Authority
- CN
- China
- Prior art keywords
- robot
- point
- potential field
- path
- coordinates
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 126
- 238000001514 detection method Methods 0.000 claims abstract description 32
- 230000006870 function Effects 0.000 claims description 68
- 238000004364 calculation method Methods 0.000 claims description 28
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000001914 filtration Methods 0.000 claims description 15
- 230000007704 transition Effects 0.000 claims description 9
- 238000011156 evaluation Methods 0.000 claims description 4
- 230000006835 compression Effects 0.000 claims description 3
- 238000007906 compression Methods 0.000 claims description 3
- 238000004590 computer program Methods 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000001052 transient effect Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 description 10
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000004088 simulation Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 4
- 238000009499 grossing Methods 0.000 description 4
- 241000282414 Homo sapiens Species 0.000 description 3
- 238000007796 conventional method Methods 0.000 description 3
- 238000011478 gradient descent method Methods 0.000 description 3
- 230000000052 comparative effect Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 239000005871 repellent Substances 0.000 description 1
- 239000000523 sample Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000003860 storage 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0255—Control of position or course in two dimensions specially adapted to land vehicles using acoustic signals, e.g. ultra-sonic singals
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Acoustics & Sound (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a method and a system for planning an artificial potential field path, wherein the method comprises the following steps: establishing a map for path planning, determining a starting point and an ending point, and establishing a potential field function; ending when the robot reaches the target point, otherwise, establishing two detection coordinate points by taking the current position coordinates of the robot as the center; calculating the value of a potential field function at the detection coordinate point, and determining the position coordinate of a path point planned by the robot in the next step; and adopting a four-dimensional alpha-beta filter, performing kinematic constraint on the position coordinates of the planned path points based on a kinematic equation of the speed position relation, obtaining new path point coordinates, controlling the robot to move to the new coordinate position, and repeating the movement until reaching the target point. The system comprises a map building module, a potential field function building module, a robot arrival judging module, a detection coordinate point building module, a planning path point position coordinate calculating module, a new path point coordinate determining module and a robot moving module. The method has the advantages of small calculated amount, smooth track and good instantaneity.
Description
Technical Field
The invention relates to the technical field of robot path planning, in particular to a method and a system for planning an artificial potential field path.
Background
With the development of modern manufacturing industry, robots have begun to be put into practical production and application in some fields. The path planning technology is one of key technologies of a mobile robot, and aims to plan a path connecting a starting point and an ending point, and meanwhile, the robot avoids obstacles along the path, so that the robot can autonomously complete various tasks. Representative solutions to the robot path planning problem include a-algorithm, D-algorithm, artificial potential field method, and the like.
The artificial potential field method is used as a classical method, and has the advantages that the planned track is smooth and accords with the robot kinematics rule, the overall map building is not needed, and the like. However, the classical artificial potential field method needs to calculate the gradient value of the potential field function, so that the calculation amount is increased, the calculation efficiency is reduced, and the problem of local optimum easily occurs in an environment with dense obstacles, thereby causing planning failure. Many scholars at home and abroad improve the classical artificial potential field method, and summarize the following improvement methods:
(1) The potential field function model is improved, a potential field function at a local optimum is optimized, for example, patent 202211077482.1 discloses an unmanned vehicle local real-time obstacle avoidance path planning method based on an improved artificial potential field method, and the planning effect of a traditional artificial potential field is improved through the improved potential field function.
(2) In the potential field model, the robot is guided to deviate from a local optimal position by establishing a virtual target point, on the premise of attractive force generated by the original target point of the traditional artificial potential field, the robot obstacle avoidance and path planning of an improved artificial potential field method are provided, the method of establishing a virtual traction point is used for solving the problem of local minimum, increasing a rapid function and improving the movement rate of the robot so as to overcome the repeated oscillation or stopping of the robot near the obstacle (the robot obstacle avoidance and path planning [ J ] of the improved artificial potential field method is realized by computer simulation, 2020,37 (2): 360-364).
(3) The potential field function is dynamically adjusted by setting a threshold value or using a self-adaptive method and the like, so that the problem of local optimum is eliminated (an artificial potential field method is improved to independently move the robot path planning [ J ]. A control project, 2019,26 (6): 1091-1098).
(4) By combining with other methods, such as an A algorithm, when the robot falls into the local optimum, a planning strategy is changed, so that the local optimum position is jumped out (a patrol robot path planning [ J ] of a computer age, 2022 (11): 29-33+37 by fusing a safety A algorithm and improving an artificial potential field method).
However, the above-mentioned various improved methods are still path planning methods based on gradient descent, which require calculating the gradient magnitude and gradient direction of the potential field function, and if the parameters of the potential field function are adjusted by combining with other path planning methods or by adopting an adaptive method on the premise, the computational power requirement on the equipment is further increased; and the mathematical level proves that the local optimal solution is a problem which cannot be avoided by a gradient descent method, and the situations of planning failure and serious oscillation of a planning track still exist when the various methods face a more complex map environment.
Other path planning methods, such as a, RRT, etc., are not prone to problems of local optimal solutions, but need to know global static maps, and the planned trajectory does not conform to the robot kinematic constraint. The common solution is to adopt methods such as Bayesian curve interpolation to carry out kinematic constraint on the track, the method has poor path planning effect and instantaneity in the environment with dynamic obstacles, and the method needs to store an established environment map and has high requirements on the storage capacity and the calculation capacity of equipment.
Disclosure of Invention
The invention aims to provide an artificial potential field path planning method and system with small calculated amount, good real-time performance and smooth track.
The technical solution for realizing the purpose of the invention is as follows: an artificial potential field path planning method comprising the steps of:
step 1, establishing a map for path planning according to current position information of a robot and surrounding obstacle environment map information of the robot, which are acquired by a sensor, and determining a starting point and an ending point;
step 2, establishing a potential field function according to a calculation formula of the artificial potential field;
step 3, judging whether the robot reaches the target point or not: if yes, ending; otherwise, entering a step 4;
step 4, two detection coordinate points are established by taking the current position coordinates of the robot as the center;
step 5, calculating the value of a potential field function at the detected coordinate point, and obtaining the position coordinates of the next planning path point of the robot;
step 6, adopting four dimensions according to the position coordinates of the planned path points obtained in the step 5 and the current moving speed of the robotThe filter is used for performing kinematic constraint on the position coordinates of the planned path points based on a kinematic equation of the speed position relationship to obtain new path point coordinates conforming to the robot kinematics;
and 7, transmitting the new path point coordinates subjected to the constraint of the filter in the step 6 to the robot, controlling the robot to move to the new coordinate position, and returning to the step 3.
An artificial potential field path planning system comprising:
the map building module is used for building a map for path planning according to the current position information of the robot and the surrounding obstacle environment map information of the robot, which are acquired by the sensor, and determining a starting point and an ending point;
the potential field function building module is used for building a potential field function according to a calculation formula of the artificial potential field;
the robot arrival judging module is used for judging whether the robot arrives at the target point or not: if yes, ending; otherwise, entering a detection coordinate point establishing module;
the detection coordinate point establishing module is used for establishing two detection coordinate points by taking the current position coordinates of the robot as the center;
the position coordinate calculation module is used for calculating the value of the potential field function at the position of the detection coordinate point and obtaining the position coordinate of the next planning path point of the robot;
the new path point coordinate determining module is used for adopting four dimensions according to the position coordinates of the obtained planning path points and the current moving speed of the robotThe filter is used for performing kinematic constraint on the position coordinates of the planned path points based on a kinematic equation of the speed position relationship to obtain new path point coordinates conforming to the robot kinematics;
and the robot moving module is used for sending the new path point coordinates constrained by the filter to the robot, controlling the robot to move to the new coordinate position, and returning to the robot arrival judging module.
A mobile terminal comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the artificial potential field path planning method when executing the program.
Compared with the prior art, the invention has the remarkable advantages that: (1) The path planning is not performed by adopting gradient descent, so that the problem of local optimum in the planning process is avoided; (2) Virtual target guide points are not required to be set, gradients of the artificial potential field function are not required to be calculated, iteration times are reduced, and the requirement on equipment computing force is lowered; (3) The design filtering link carries out kinematic constraint on the planning result, so that the track is smoother; (4) The designed four-dimensional filter is smooth in path while planning, has real-time performance, and reduces the performance requirement on equipment.
Drawings
Fig. 1 is a flow chart of a procedure for the gradient-free APF method.
Fig. 2 is a trace diagram without using kinematic constraints for the gradient-free APF method.
Fig. 3 is a graph of the effect of the gradient-free APF method using a kinematic constraint filter.
Fig. 4 is a graph comparing the gradient-free APF process with the conventional APF process.
Fig. 5 is a convergence graph of the robot position function in a comparative experiment.
Fig. 6 is a graph of the result of failure of the conventional APF regulation.
Fig. 7 is a graph comparing the gradient-free APF process with the conventional APF process.
Fig. 8 is a convergence diagram of the robot position function in the second comparative experiment.
Detailed Description
The invention provides a method for planning an artificial potential field path, which comprises the following steps:
step 1, establishing a map for path planning according to current position information of a robot and surrounding obstacle environment map information of the robot, which are acquired by a sensor, and determining a starting point and an ending point;
step 2, establishing a potential field function according to a calculation formula of the artificial potential field;
step 3, judging whether the robot reaches the target point or not: if yes, ending; otherwise, entering a step 4;
step 4, two detection coordinate points are established by taking the current position coordinates of the robot as the center;
step 5, calculating the value of a potential field function at the detected coordinate point, and obtaining the position coordinates of the next planning path point of the robot;
step 6, adopting four dimensions according to the position coordinates of the planned path points obtained in the step 5 and the current moving speed of the robotThe filter is used for performing kinematic constraint on the position coordinates of the planned path points based on a kinematic equation of the speed position relation to obtain the robot motionLearning new path point coordinates;
and 7, transmitting the new path point coordinates subjected to the constraint of the filter in the step 6 to the robot, controlling the robot to move to the new coordinate position, and returning to the step 3.
As a specific example, in step 1:
the sensor comprises a laser radar, a camera, inertial navigation equipment and ultrasonic equipment;
the map information of the surrounding obstacle environment of the robot includes the distance between the robot and the surrounding obstacle, the distance between the robot and the end point, and the current moving speed of the robot.
As a specific example, the step 2 of establishing a potential field function according to the calculation formula of the artificial potential field refers to establishing a potential field function according to the calculation formula of the traditional artificial potential field by using the map information obtained in the step 1, including the distance between the robot and the surrounding obstacle, the distance between the robot and the end point, the current moving speed of the robot, and the like; the method comprises the following steps:
constructing a gravitational field function by measuring the distance from the robot to the target pointThe method comprises the following steps:
(1)
wherein ,representing the current position of the robot with coordinates +.>；/>Representing the position of the target point, the coordinates areIs used for controlling the gravitational field to act on the robotUsing parameters of intensity; />Representing a distance between the target point and the current position of the robot; />Representing the gravitational field coefficient, determining the gravitational field magnitude, < ->Setting a numerical value for a person, wherein the numerical value is 0.1-1.0, and adjusting according to the size of a map, the density of obstacles in the map and the distance between the robot and a destination;
virtual repulsive force is calculated by distance from robot to nearest obstacleBased on->Representing the position of the obstacle point with the coordinates +.>Repulsive force field->Expressed as:
(2)
wherein ,representing the repulsive force field coefficient, determining the repulsive force field magnitude, < ->For the artificial setting of the values, the values are obtained by using the reciprocal of the gravitational field coefficient or a larger value, if +.>Is lower than gravitational field coefficient->May result in too small a repulsive force field generated by an obstacle further from the end point, resulting in a repulsive force field covered by the gravitational field, such that the obstacle is erroneously calculated by the robot as a passable area.
The application range of the repulsive force field is expressed, and the repulsive force application range generated by the obstacle is determined, which can be also understood as the minimum safety distance between the robot and the obstacle, < + >>Manually setting a numerical value which is 1-2;
、/>and the distance between the robot and the end point is adjusted according to the size of the map, the density of the obstacle in the map and the size of the robot. The larger the repulsive force field is, and the larger the safety distance between the robot and the obstacle is.
After the mathematical model of the gravitational field and the repulsive field is obtained, the two potential fields are overlapped to obtain the total potential field function：
(3)
The numerical value of the potential field function at the detected coordinate point is calculated by using the calculation method provided by the invention, and the position coordinates of the path point to be moved by the robot in the next step are obtained. The traditional artificial potential field method also needs to calculate the gradient size and direction of the potential field after calculating the potential field function, and has higher requirement on the calculation force of equipment. The calculation method provided by the invention does not need to calculate the gradient size and direction of the potential field function, greatly reduces the requirement on the calculation force of equipment, and does not have the problem that the conventional method fails to plan due to the common local optimal solution in the gradient descent method.
As a specific example, in step 4, two detection coordinate points are established with the current position coordinates of the robot as the center, specifically as follows:
two detection points are established by taking the current position coordinates of the robot as the center、/>The expression is as follows:
(4)
(5)
wherein ,the current position of the robot; />As a random function +.>For the dimension of the map, if a two-dimensional map is used +.>If three-dimensional map navigation is used +.>；/>The parameters are set manually, the value is 5-10, and the parameters are determined according to the size of the map and the obstacleThe density of the obstacle is adjusted; the larger the map, the smaller the obstacle density, and the larger the value can be set.
As a specific example, in step 5, the magnitude of the potential field function value at the detected coordinate point is calculated, and the position coordinates of the next planned path point of the robot are obtained, which is specifically as follows:
calculating the function value of the detection point position by using the established potential field function, comparing, and calculating the position at the next iteration through the following formula:
(6)
wherein ,the value of the parameter is 5-10, and the size is adjusted according to the map size and the obstacle density; />The current position of the robot; />For the coordinate position of the next movement of the robot, < >> and />Calculated for formula (4)>、/>The potential field function size at the coordinates.
As a specific example, in step 6, the new path point coordinates conforming to the robot kinematics are obtained by the following processing procedure:
constant gainThe filter equation of the filter is:
(7)
wherein ,、/>is->The coefficient of the filter is set by various setting methods according to the application field of the filter, and the setting method adopted by the invention will be given later; />Representing the time interval of the sensor for acquiring data as a time coefficient; />Is true value +.>Is->Predicted estimate of->For the result after filtering, +.>Is->Derivative of>Is->Is a derivative of (2);
the observation equation is:
(8)
wherein ,coordinates for the measured values, i.e. the planned path;
according to two dimensionsThe filter is designed into a four-dimensional track optimizing filter, and the robot is recorded in +.>The position on the shaft is +.>Robot is +.>The position on the shaft is +.>At the same time the speed during the movement of the robot is +.>Shaft and->The axes are orthogonally decomposed, and the robot phase is recorded as +.>The speed projection on the axis is +.>In->The speed projection on the axis is +.>；
Writing out a state transition matrix according to the position and speed relation：
(9)
The state transition relationship is:
(10)
wherein The robot is at the previous moment +>Position on the shaft, ">The robot is at the previous moment +>Speed on shaft, +.>The robot is at the previous moment +>Position on the shaft, ">The robot is at the previous moment +>Speed on shaft, +.>For robot +.>Position estimation on the shaft,/>For robot +.>On-axis speed estimation, < >>For robot +.>Position estimation on the shaft,/>For robot +.>A speed estimate on the shaft;
filtering the estimate vector according to the state transition matrix:
(11)
wherein ,for filtering the evaluation +.>For robot +.>Position estimation on axis->For robot +.>Speed estimation on shaft,/->For robot +.>Position estimation on axis->For robot +.>An on-axis velocity estimate;
predicting an estimated value vector:
(12)
wherein ,for predictive evaluation +.>For robot +.>On-axis position prediction estimation, +.>For robot +.>On-axis velocity prediction estimation, +.>For robot +.>On-axis position prediction estimation, +.>Is in the robotA speed predictive estimate on the shaft;
gain matrix：
(13)
wherein ,、/>is->Filter parameters in the axial direction;
measuring matrix：
(14)
The expression of the filtering and prediction equation in matrix form is:
(15)
derived, in matrix formThe filter equation is written as:
(16)
filtering estimation value for the last moment;
wherein Is a 4 x 4 matrix, the matrix is as follows:
(17)
is a 4 x 1 column vector, as shown below,
(18)
in the formula 、/>、/> and />Adjusting according to the moving speed of the robot;
the four-dimensional filter adopts a criterion of minimizing a speed estimation steady state variance compression coefficient under the condition of a given speed estimation transient square difference to determine +.>And->Relationship +.>And->The relationship is as follows>，/>The parameters are set to be 0.1-1, < >>Andaccording to->，/>The parameters are calculated from the following equation:
(19)
in the aspect of path planning, most of existing methods for performing kinematic constraint on tracks basically adopt interpolation processing on the tracks after global path planning is finished, and have poor instantaneity. The invention provides a real-time kinematic constraint on the planned track by using a filter, and has lower calculation force requirement on equipment.
The invention also provides a system for planning the path of the artificial potential field, which comprises:
the map building module is used for building a map for path planning according to the current position information of the robot and the surrounding obstacle environment map information of the robot, which are acquired by the sensor, and determining a starting point and an ending point;
the potential field function building module is used for building a potential field function according to a calculation formula of the artificial potential field;
the robot arrival judging module is used for judging whether the robot arrives at the target point or not: if yes, ending; otherwise, entering a detection coordinate point establishing module;
the detection coordinate point establishing module is used for establishing two detection coordinate points by taking the current position coordinates of the robot as the center;
the position coordinate calculation module is used for calculating the value of the potential field function at the position of the detection coordinate point and obtaining the position coordinate of the next planning path point of the robot;
the new path point coordinate determining module is used for adopting four dimensions according to the position coordinates of the obtained planning path points and the current moving speed of the robotThe filter is used for performing kinematic constraint on the position coordinates of the planned path points based on a kinematic equation of the speed position relationship to obtain new path point coordinates conforming to the robot kinematics;
and the robot moving module is used for sending the new path point coordinates constrained by the filter to the robot, controlling the robot to move to the new coordinate position, and returning to the robot arrival judging module.
The invention also provides a mobile terminal comprising a memory, a processor and a computer program stored on the memory and running on the processor, wherein the processor realizes the artificial potential field path planning method when executing the program.
The invention will be described in further detail with reference to the accompanying drawings and specific examples.
Examples
The invention provides an improved artificial potential field path planning method, which is called a gradient-free APF method.
The gradient-free APF method provided by the invention solves the problem of planning failure caused by the fact that the traditional manual potential field path planning method (traditional APF method) falls into local optimum under the condition of dense barriers, the new method does not need to calculate function gradients, and the iteration times can be greatly reduced while the calculated amount is reduced. In the new method, the planned track is smoothed by adopting a filtering means, so that the track can be smoothed while being planned, and compared with the traditional method for smoothing the path by using fitting and interpolation modes after the planning of all path points is completed, the method has more real-time performance.
With reference to fig. 1, the gradient-free APF method proposed by the present invention comprises the steps of:
step 1, acquiring current position information of a robot and surrounding obstacle environment map information of the robot according to sensors such as a laser radar, a camera, inertial navigation equipment and ultrasonic waves, wherein the current position information comprises the distance between the robot and surrounding obstacles, the distance between the robot and a terminal point, the current moving speed of the robot and the like. Establishing a map for path planning, and determining a starting point and an ending point;
step 2, establishing a potential field function according to a calculation formula of a traditional artificial potential field by utilizing the map information obtained in the step 1, including the distance between the robot and surrounding obstacles, the distance between the robot and an end point, the current moving speed of the robot and the like;
and 3, establishing two detection coordinate points by taking the current coordinates of the robot as the center according to the artificial potential field function established in the step 2, calculating the value of the potential field function at the detection coordinate points by using the calculation method provided by the invention, and obtaining the position coordinates of the path points to which the robot needs to move next. The traditional artificial potential field method also needs to calculate the gradient size and direction of the potential field after calculating the potential field function, and has higher requirement on the calculation force of equipment. The calculation method provided by the invention does not need to calculate the gradient size and direction of the potential field function, greatly reduces the requirement on the calculation force of equipment, and does not have the problem that the conventional method fails to plan due to the common local optimal solution in the gradient descent method;
step 4, adopting four dimensions of design according to the path points obtained in the step 3 and the current moving speed of the robot obtained in the step 2And (3) the filter performs kinematic constraint on the path point coordinates planned in the step (3) by using a kinematic equation of the speed position relationship to obtain final new path point coordinates conforming to the robot kinematics. In the aspect of path planning, most existing methods for performing kinematic constraint on tracks basically adopt the following steps ofAnd after the global path planning is finished, the track is interpolated, so that the real-time performance is poor. The invention provides a real-time kinematic constraint on the planned track by using a filter, and has lower calculation force requirement on equipment.
Step 5, the new path point coordinates after being constrained by the filter in the step 4 are sent to the robot, and the robot is controlled to move to a new coordinate position; and (5) repeating the path planning process of the steps 1-5 until the robot moves to the end point.
Step 1, 2, acquiring current position information of a robot and surrounding obstacle environment map information of the robot according to sensors such as a laser radar, a camera, an IMU and ultrasonic waves, establishing a map for path planning, determining a starting point and an ending point, and establishing an artificial potential field function, wherein the steps are as follows:
constructing a gravitational field function by measuring the distance from the robot to the target pointThe method comprises the following steps:
(1)
wherein Representing the current position of the robot with coordinates +.>; />Representing the position of the target point, the coordinates areIs a parameter for controlling the strength of the gravitational field acting on the robot. />Representing the distance between the target point and the current position of the robot.
Virtual repulsive forces are generallyWith distance of robot to nearest obstacleBased on->Representing the position of the obstacle point with the coordinates +.>. Force-repellent field->Can be expressed as:
(2)
after the mathematical model of the gravitational field and the repulsive field is obtained, the two potential fields are overlapped to obtain the total potential field function：
(3)
Further, step 3, with the robot as the center, two detection points are established, and the numerical value of the artificial potential field function at the detection points is calculated by using the calculation formula provided by the invention, and the method specifically comprises the following steps:
with the robot as the center, two detection points are established，/>The mathematical expression of the probe points is as follows,
(4)
(5)
the current position of the robot; />As a random function +.>Is the dimension of the map. For example, if a two-dimensional map is used, +.>The method comprises the steps of carrying out a first treatment on the surface of the If three-dimensional map navigation is used, +.>；/>Is a set parameter for human. And calculating the function value of the position of the detection point by using the potential field function established before, comparing, and calculating the position at the next iteration through the following mathematical formula.
(6)
wherein Parameters set for human beings; />The current position of the robot; obtained->The coordinate position of the robot for the next movement.
Further, after the new path point is obtained in the step 4, a designed four-dimensional filter is adopted, and the new path point is smoothed by using a kinematic equation of a speed and position relation, which is specifically as follows:
the following is the constant gainThe filter is taken as an example, and the path planned by the improved algorithm is smoothly optimized in real time. Constant gain->The filter equation of the filter is:
(7)
wherein ，/>Coefficients set for human beings, ++>For the time coefficient>Is true value +.>Is->Predicted estimate of->For the result after filtering, +.>Is->Derivative of>Is->Is a derivative of (a).
The observation equation is:
(8)
path coordinates planned for the measured values, i.e. the improved method, < >>As a time coefficient, a time interval for acquiring data for the sensor in engineering application;
in the invention imitate two dimensionsAnd the filter is designed into a four-dimensional track optimization filter. Record robot in->The position on the shaft is +.>Robot is +.>The position on the shaft is +.>At the same time the speed during the movement of the robot is +.>Shaft and->The axes are orthogonally decomposed, and the robot phase is recorded as +.>The speed projection on the axis is +.>In->The speed projection on the axis is +.>. The state transition matrix can be written according to the position and speed relation:
(9)
the state transition relationship is:
(10)
according to the state transition matrix, the estimation vector is filtered:
(11)
for robot +.>Position estimation on axis->For robot +.>Speed estimation on shaft,/->For robot +.>Position estimation on axis->For robot +.>An on-axis velocity estimate.
Predicting an estimated value vector:
(12)/>
for robot +.>On-axis position prediction estimation, +.>For robot +.>On-axis velocity prediction estimation, +.>For robot +.>On-axis position prediction estimation, +.>For robot +.>A predictive estimate of speed on the shaft.
Gain matrix:
(13)
measurement matrix:
(14)
the expression of the filtering and prediction equation in matrix form is:
(15)
derived, in matrix formThe filter equation can be written as:
(16)
wherein Is a 4 x 4 matrix, the matrix is as follows:
(17)
is a 4 x 1 column vector, as shown below,
(18)
in the formula ，/>，/> and />Is artificialAnd (5) setting parameters.
The key of the filter is to determine +.>、/>There are several selection criteria and optimization methods, taking into account the kinematic characteristics of the robot, the double ++designed by the invention>The four-dimensional filter adopts a criterion of enabling a speed estimation steady state variance compression coefficient to be minimum under the condition of a given speed estimation transient square difference to determine +.>And->Relationship +.>And->The relationship is as follows->
(19)
The gradient-free APF method provided by the invention is simulated by utilizing MATLAB, and the correctness and the effectiveness are verified.
In order to compare the planning effects of the gradient-free APF method and the original artificial potential field method, two path planning methods are adopted in the same obstacle map for simulation comparison.
The simulation specific process using the gradient-free APF method provided by the invention comprises the following steps: firstly, establishing a simulated obstacle environment, setting a starting point and an ending point of a robot, calculating the distance from the current coordinates of the robot to surrounding obstacles and the distance from the current coordinates of the robot to the ending point, and generating an artificial potential field function according to the current coordinates; then, taking the coordinate position of the robot as the center, generating a detection point according to the method of the gradient-free APF method provided by the invention, calculating the artificial potential field function size at the detection point, planning a new path point by using the iteration formula provided by the invention, smoothing the path by using a four-dimensional filter to obtain a new coordinate point conforming to the kinematics of the robot, moving the robot to the new coordinate position, and repeating the process until the robot moves to the end point.
The map size in the simulation environment is 600m multiplied by 600m, the starting point coordinates of the path planning are (420 ), the end point coordinates are (50, 50), and the end point is considered to be reached when the distance from the end point is smaller than 1 m.
Table 1 comparison of iteration times of gradient-free APF method and conventional APF method
Table 2 comparison of planned path length for the gradient-free APF method with the conventional APF method
Tables 1-2 show the comparison of the iteration times and the path length of the gradient-free APF method and the original method in the same experiment. Table 1 is a table of comparison between the iteration number of the gradient-free APF method proposed by the present invention and the iteration number of the original method. Table 2 is a comparison table of the planned path length of the non-gradient APF method according to the present invention and the original method.
Fig. 2 to fig. 5 are diagrams showing a path planning image using the non-gradient APF method proposed by the present invention, an image comparing the method of the present invention with the conventional APF method, and a comparison image of the iteration number of the two methods in a simulation environment, wherein fig. 2 and fig. 3 are diagrams showing a path (a path without filtering smoothing) planned in the first step of the non-gradient APF method proposed by the present invention and a final path planned after adopting a kinematic constraint filter.
Fig. 4-5 are graphs for comparing a path planned by a gradient-free APF method and a conventional APF method in the simulation environment, wherein fig. 4 shows the difference between the path planned by the conventional artificial potential field method and the path planned by the gradient-free artificial potential field method proposed by the present invention, fig. 5 is the iteration times of the two methods in the same environment and the potential field function value of the position of the robot after each iteration in the iteration process, and it can be seen from the graph that the method of the present invention has significant advantages in iteration times compared with the original method. Compared with the traditional APF method, the method provided by the invention has the advantages that the convergence to the end point is faster, namely, the coordinate position with the potential field function value of 0 is realized, and the calculation force requirement on equipment is greatly reduced.
Fig. 6 illustrates an example of the failure of the conventional artificial potential field method to plan in another simulated map environment, where it can be seen that the conventional APF method fails to converge to the endpoint. Fig. 7 shows the path planned by the method of the present invention in the same map environment, and it can be seen that the method not only requires a small number of iterations, but also can successfully complete path planning in some special environments. Fig. 8 shows the iteration times of the two methods in the experiment and the potential field function value of the position of the robot after each iteration in the iteration process, and it can be seen from the figure that the conventional method fails to converge to the coordinate with the potential field function of 0, that is, fails to plan to the end point coordinate, and the method of the present invention quickly converges to the coordinate with the potential field function of 0, that is, successfully completes the path planning.
Therefore, the gradient-free APF method provided by the invention can solve the problem that the traditional APF method falls into a local optimal solution, and the calculated amount required in planning is less, and the iteration times are far lower than those of the traditional APF method. The track is smoothed in real time by using the four-dimensional filter, so that compared with a common method for smoothing by using an interpolation means after planning, the method has more real-time performance and is convenient to use in a real-time scene.
In summary, the non-gradient APF method provided by the invention adopts a new path planning method based on the potential field function of the original artificial potential field method, so that the iteration times are greatly reduced, and the problem that the original method falls into a local optimal solution is solved; the four-dimensional filter is added to optimize the path, the path is smooth, the length is shortened to some extent, and the practicability of the algorithm is greatly improved.
Claims (3)
1. An artificial potential field path planning method, characterized by comprising the following steps:
step 1, establishing a map for path planning according to current position information of a robot and surrounding obstacle environment map information of the robot, which are acquired by a sensor, and determining a starting point and an ending point;
step 2, establishing a potential field function according to a calculation formula of the artificial potential field;
step 3, judging whether the robot reaches the target point or not: if yes, ending; otherwise, entering a step 4;
step 4, two detection coordinate points are established by taking the current position coordinates of the robot as the center;
step 5, calculating the value of a potential field function at the detected coordinate point, and obtaining the position coordinates of the next planning path point of the robot;
step 6, adopting four dimensions according to the position coordinates of the planned path points obtained in the step 5 and the current moving speed of the robotThe filter is used for performing kinematic constraint on the position coordinates of the planned path points based on a kinematic equation of the speed position relationship to obtain new path point coordinates conforming to the robot kinematics;
step 7, the new path point coordinates after being constrained by the filter in the step 6 are sent to the robot, the robot is controlled to move to the new coordinate position, and then the step 3 is returned;
in step 1:
the sensor comprises a laser radar, a camera, inertial navigation equipment and ultrasonic equipment;
the map information of the surrounding obstacle environment of the robot comprises the distance between the robot and the surrounding obstacle, the distance between the robot and the end point, and the current moving speed of the robot;
step 2, establishing a potential field function according to a calculation formula of the artificial potential field, wherein the potential field function is specifically as follows:
constructing a gravitational field function by measuring the distance from the robot to the target pointThe method comprises the following steps:
(1)
wherein ,representing the current position of the robot with coordinates +.>；/>Representing the position of the target point, coordinates +.>Is a parameter for controlling the strength of the gravitational field acting on the robot; />Representing a distance between the target point and the current position of the robot; />Representing the gravitational field coefficient, determining the gravitational field magnitude, < ->Setting a numerical value for a person, wherein the numerical value is 0.1-1.0, and adjusting according to the size of a map, the density of obstacles in the map and the distance between the robot and a destination;
virtual repulsive force is calculated by distance from robot to nearest obstacleBased on->Representing the position of the obstacle point with the coordinates +.>Repulsive force field->Expressed as:
(2)
wherein ,representing the repulsive force field coefficient, determining the repulsive force field magnitude, < ->The value is the reciprocal of gravitational field coefficient or larger; />Indicating the repulsive force field action range, < >>The value range is 1-2;
after the mathematical model of the gravitational field and the repulsive field is obtained, the two potential fields are overlapped to obtain the total potential field function：
(3)
In step 4, two detection coordinate points are established by taking the current position coordinates of the robot as the center, and the method specifically comprises the following steps:
two detection points are established by taking the current position coordinates of the robot as the center、/>The expression is as follows:
(4)
(5)
wherein ,the current position of the robot; />As a random function +.>For the dimension of the map, if a two-dimensional map is used +.>If three-dimensional map navigation is used +.>；/>Manually setting parameters, wherein the value is 5-10, and adjusting the size according to the size of the map and the density of the obstacle;
in step 5, calculating the value of the potential field function at the detected coordinate point, and obtaining the position coordinates of the next planning path point of the robot, wherein the method specifically comprises the following steps:
calculating the function value of the detection point position by using the established potential field function, comparing, and calculating the position at the next iteration through the following formula:
(6)
wherein ,the value of the parameter is 5-10, and the size is adjusted according to the map size and the obstacle density; />The current position of the robot; />For the coordinate position of the next movement of the robot, < >> and />Calculated for formula (4)>、/>The magnitude of the potential field function at the coordinates;
in the step 6, the new path point coordinates conforming to the robot kinematics are obtained through the following processing procedures:
constant gainThe filter equation of the filter is:
(7)
wherein ,、/>is->Coefficients of the filter; />Representing the time interval of the sensor for acquiring data as a time coefficient;is true value +.>Is->Predicted estimate of->For the result after filtering, +.>Is->Derivative of>Is thatIs a derivative of (2);
the observation equation is:
(8)
wherein ,coordinates for the measured values, i.e. the planned path;
according to two dimensionsThe filter is designed into a four-dimensional track optimizing filter, and the robot is recorded in +.>The position on the shaft is +.>Robot is +.>The position on the shaft is +.>At the same time the speed during the movement of the robot is +.>Shaft and->The axes are orthogonally decomposed, and the robot phase is recorded as +.>The speed projection on the axis is +.>In->The speed projection on the axis is +.>；
Writing out a state transition matrix according to the position and speed relation：
(9)
The state transition relationship is:
(10)
wherein The robot is at the previous moment +>Position on the shaft, ">The robot is at the previous moment +>The speed of the shaft is such that,the robot is at the previous moment +>Position on the shaft, ">The robot is at the previous moment +>Speed on shaft, +.>For robot +.>Position estimation on the shaft,/>For robot +.>On-axis speed estimation, < >>Is in the robotPosition estimation on the shaft,/>For robot +.>A speed estimate on the shaft;
filtering the estimate vector according to the state transition matrix:
(11)
wherein ,for filtering the evaluation +.>For robot +.>Position estimation on axis->For robot +.>Speed estimation on shaft,/->For robot +.>Position estimation on axis->For robot +.>An on-axis velocity estimate;
predicting an estimated value vector:
(12)
wherein ,for predictive evaluation +.>For robot +.>On-axis position prediction estimation, +.>For robot +.>On-axis velocity prediction estimation, +.>For robot +.>On-axis position prediction estimation, +.>For robot +.>A speed predictive estimate on the shaft;
gain matrix：
(13)
wherein ,、/>is->Filter parameters in the axial direction;
measuring matrix：
(14)
The expression of the filtering and prediction equation in matrix form is:
(15)
derived, in matrix formThe filter equation is written as:
(16)
filtering estimation value for the last moment;
wherein Is a 4 x 4 matrix, the matrix is as follows:
(17)
is a 4 x 1 column vector, as shown below,
(18)
in the formula 、/>、/> and />Adjusting according to the moving speed of the robot;
the four-dimensional filter adopts a criterion of minimizing a speed estimation steady state variance compression coefficient under the condition of a given speed estimation transient square difference to determine +.>And->Relationship +.>And->The relationship is as follows>，/>The parameters are set to be 0.1-1, < >> and />According to->，/>The parameters are calculated from the following equation:
(19)。
2. an artificial potential field path planning system for implementing the artificial potential field path planning method of claim 1, the system comprising:
the map building module is used for building a map for path planning according to the current position information of the robot and the surrounding obstacle environment map information of the robot, which are acquired by the sensor, and determining a starting point and an ending point;
the potential field function building module is used for building a potential field function according to a calculation formula of the artificial potential field;
the robot arrival judging module is used for judging whether the robot arrives at the target point or not: if yes, ending; otherwise, entering a detection coordinate point establishing module;
the detection coordinate point establishing module is used for establishing two detection coordinate points by taking the current position coordinates of the robot as the center;
the position coordinate calculation module is used for calculating the value of the potential field function at the position of the detection coordinate point and obtaining the position coordinate of the next planning path point of the robot;
the new path point coordinate determining module is used for adopting four dimensions according to the position coordinates of the obtained planning path points and the current moving speed of the robotThe filter is used for performing kinematic constraint on the position coordinates of the planned path points based on a kinematic equation of the speed position relationship to obtain new path point coordinates conforming to the robot kinematics;
and the robot moving module is used for sending the new path point coordinates constrained by the filter to the robot, controlling the robot to move to the new coordinate position, and returning to the robot arrival judging module.
3. A mobile terminal comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor implements the artificial potential field path planning method as claimed in claim 1 when executing the program.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310661875.5A CN116382310B (en) | 2023-06-06 | 2023-06-06 | Artificial potential field path planning method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310661875.5A CN116382310B (en) | 2023-06-06 | 2023-06-06 | Artificial potential field path planning method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116382310A CN116382310A (en) | 2023-07-04 |
| CN116382310B true CN116382310B (en) | 2023-08-18 |
Family
ID=86969792
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310661875.5A Active CN116382310B (en) | 2023-06-06 | 2023-06-06 | Artificial potential field path planning method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116382310B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5758298A (en) * | 1994-03-16 | 1998-05-26 | Deutsche Forschungsanstalt Fur Luft-Und Raumfahrt E.V. | Autonomous navigation system for a mobile robot or manipulator |
| CN108646761A (en) * | 2018-07-12 | 2018-10-12 | 郑州大学 | Robot indoor environment exploration, avoidance and method for tracking target based on ROS |
| CN109828588A (en) * | 2019-03-11 | 2019-05-31 | 浙江工业大学 | Paths planning method in a kind of robot chamber based on Multi-sensor Fusion |
| CN113741503A (en) * | 2021-09-16 | 2021-12-03 | 吉林工程技术师范学院 | Autonomous positioning type unmanned aerial vehicle and indoor path autonomous planning method thereof |
| CN115840448A (en) * | 2022-12-06 | 2023-03-24 | 清华大学 | Virtual potential field-based swarm intelligent robot path planning method and device |
-
2023
- 2023-06-06 CN CN202310661875.5A patent/CN116382310B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5758298A (en) * | 1994-03-16 | 1998-05-26 | Deutsche Forschungsanstalt Fur Luft-Und Raumfahrt E.V. | Autonomous navigation system for a mobile robot or manipulator |
| CN108646761A (en) * | 2018-07-12 | 2018-10-12 | 郑州大学 | Robot indoor environment exploration, avoidance and method for tracking target based on ROS |
| CN109828588A (en) * | 2019-03-11 | 2019-05-31 | 浙江工业大学 | Paths planning method in a kind of robot chamber based on Multi-sensor Fusion |
| CN113741503A (en) * | 2021-09-16 | 2021-12-03 | 吉林工程技术师范学院 | Autonomous positioning type unmanned aerial vehicle and indoor path autonomous planning method thereof |
| CN115840448A (en) * | 2022-12-06 | 2023-03-24 | 清华大学 | Virtual potential field-based swarm intelligent robot path planning method and device |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116382310A (en) | 2023-07-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109241552B (en) | Underwater robot motion planning method based on multiple constraint targets | |
| CN109782779B (en) | AUV path planning method in ocean current environment based on population hyperheuristic algorithm | |
| WO2021103391A1 (en) | Method and system for controlling gliding depth of biomimetic robotic gliding dolphin, and apparatuses | |
| JP2003241836A (en) | Control method and apparatus for free-running mobile unit | |
| CN114625151A (en) | Underwater robot obstacle avoidance path planning method based on reinforcement learning | |
| CN110561420B (en) | Arm profile constraint flexible robot track planning method and device | |
| Li et al. | Three-dimensional path planning for AUVs in ocean currents environment based on an improved compression factor particle swarm optimization algorithm | |
| CN113442140B (en) | Cartesian space obstacle avoidance planning method based on Bezier optimization | |
| CN112612290A (en) | Underwater vehicle three-dimensional multi-task path planning method considering ocean currents | |
| CN110561419A (en) | arm-shaped line constraint flexible robot track planning method and device | |
| Zhang et al. | Intelligent vector field histogram based collision avoidance method for auv | |
| CN114326810A (en) | Obstacle avoidance method of unmanned aerial vehicle in complex dynamic environment | |
| Hu et al. | Optimal path planning for mobile manipulator based on manipulability and localizability | |
| CN116382310B (en) | Artificial potential field path planning method and system | |
| KR101107735B1 (en) | Camera pose decision method | |
| CN116604557A (en) | Double-arm robot cooperative collision avoidance planning optimization method in unstructured environment | |
| CN116466701A (en) | Unmanned surface vessel energy consumption minimum track planning method | |
| CN114815899A (en) | Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor | |
| Thompson et al. | Localisation for autonomous humanoid navigation | |
| Wang et al. | An Improved A* Path Planning Algorithm of Autonomous Underwater Vehicles | |
| Xie et al. | Path Planning for Robotic Arm Based on Reinforcement Learning under the Train | |
| Gao et al. | Near-ground trajectory planning for UAVs via multi-resolution hybrid voxel-surfel map | |
| CN115291616B (en) | AUV dynamic obstacle avoidance method based on near-end strategy optimization algorithm | |
| Cai et al. | Autonomous navigation system for exhibition hall service robots via laser SLAM | |
| CN118011832A (en) | Moving body robust safety control method based on laser point cloud and obstacle function |
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 |