CN116643572A - Local path planning method, electronic equipment and medium for indoor complex environment - Google Patents
Local path planning method, electronic equipment and medium for indoor complex environment Download PDFInfo
- Publication number
- CN116643572A CN116643572A CN202310782027.XA CN202310782027A CN116643572A CN 116643572 A CN116643572 A CN 116643572A CN 202310782027 A CN202310782027 A CN 202310782027A CN 116643572 A CN116643572 A CN 116643572A
- Authority
- CN
- China
- Prior art keywords
- robot
- speed
- evaluation function
- obstacle
- complex environment
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000011156 evaluation Methods 0.000 claims abstract description 80
- 238000005070 sampling Methods 0.000 claims abstract description 13
- 230000008569 process Effects 0.000 claims abstract description 9
- 238000012216 screening Methods 0.000 claims abstract description 5
- 230000006870 function Effects 0.000 claims description 62
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 230000010339 dilation Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 20
- 238000004088 simulation Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 6
- 230000007613 environmental effect Effects 0.000 description 4
- 230000004888 barrier function Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000005299 abrasion Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000005251 gamma ray Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008520 organization Effects 0.000 description 1
- 238000011160 research 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a local path planning method, electronic equipment and medium for indoor complex environment, wherein the method comprises the following steps: acquiring dynamic speed weight parameters of the obstacle according to a pre-constructed indoor map containing obstacle information and environment boundary lines, and adaptively adjusting the speed weight of the robot; calculating the current speed space of the robot, and sampling the speed space to obtain a group speed instruction; according to each group of speed instructions, predicting the motion trail to obtain n groups of predicted trail, constructing an evaluation function, and screening the predicted trail with the highest score as a final trail; the evaluation function is composed of a direction angle evaluation function, an obstacle distance evaluation function, a speed evaluation function, and a potential field evaluation function. Compared with the prior art, the invention can ensure the safety of the robot in the running process, has high running efficiency, and can keep better running posture of the robot in a complex environment.
Description
Technical Field
The present invention relates to the field of local path planning technologies for indoor robots, and in particular, to a local path planning method, an electronic device, and a medium for an indoor complex environment.
Background
The path planning and navigation obstacle avoidance are the basis for realizing other intelligent tasks of the robot, and are the cores of the autonomous performance of the robot; generally, whether a path is optimal is determined mainly by means of evaluation criteria such as path length, movement time, obstacle avoidance performance, path smoothness and the like, so that path planning has decisive effects in reducing working time of a robot, reducing energy loss, reducing abrasion of the robot and the like.
Through analysis and research on basic principles and improvement strategies of an artificial potential field method, the APF algorithm is found to be difficult to meet the requirement of robot local planning, a new planning algorithm needs to be found, the APF algorithm is more suitable to be used as an optimization mode to improve the existing algorithm, and the dynamic window method is one of the more mature local path planning algorithms, and is widely applied to a mobile robot platform due to the fact that the dynamic window method fully considers the limitation of robot kinematics.
At present, the mobile robot industry develops at a high speed, the autonomous navigation of the robot is more complex, the simple basic algorithm can not meet the requirements of real-time performance and flexibility of local path planning, the fusion algorithm example combining multiple algorithms is gradually generated in recent years, the algorithm performance is greatly improved, and the following problems still exist:
(1) The prospective is insufficient, the real-time obstacle avoidance effect is poor, and the safety of the robot in the running process is difficult to be ensured sufficiently.
(2) The operation efficiency is to be improved, and the robot is difficult to maintain a good operation posture in a complex environment.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provide a local path planning method, electronic equipment and medium for an indoor complex environment.
The aim of the invention can be achieved by the following technical scheme:
according to a first aspect of the present invention, the present invention provides a local path planning method for an indoor complex environment, including the following steps:
acquiring dynamic speed weight parameters of the obstacle according to a pre-constructed indoor map containing obstacle information and environment boundary lines, and adaptively adjusting the speed weight of the robot;
calculating the current speed space of the robot, and sampling the speed space to obtain a group speed instruction;
according to each group of speed instructions, carrying out motion track prediction by adopting a dynamic window method to obtain n groups of predicted tracks, constructing an evaluation function according to the speed instructions of each group of predicted tracks and the speed weight of the robot, calculating the score of each group of predicted tracks according to the evaluation function, and screening the predicted track with the highest score as a final track; the evaluation function is composed of a direction angle evaluation function, an obstacle distance evaluation function, a speed evaluation function, and a potential field evaluation function.
Preferably, the formula describing the evaluation function is:
G mod (v i ,ω i )＝α·heading(v i ,ω i )+β·dist(v i ,ω i )+γ adapt ·velocity(v i ,w i )+λ·potential(v i ,ω i )
wherein G is mod (v i ,ω i ) To evaluate the function, gamma adapt For the adaptive velocity evaluation weight, λ is potential field evaluation weight, potential (v i ,ω i ) As a potential field evaluation function, (v) i ,ω i ) Speed command information representing the set of tracks, head (v i ,ω i ) Represents the direction angle evaluation function, α represents the direction angle evaluation weight, dist (v i ,ω i ) Representing an obstacle evaluation function, β representing an obstacle evaluation weight; velocity (v) i ,ω i ) A speed evaluation function is represented.
Preferably, the process of obtaining the potential field evaluation function is specifically: evaluating the last pose of the predicted track of the robot, calculating a resultant force direction angle according to the positions of the obstacle and the target point, and making a difference value between the resultant force direction angle and the predicted direction angle to obtain a deviation value;
the potential field evaluation function is expressed as:
potential(v i ,w i )＝180°-|θ force -θ fore |
in θ force Represents the resultant force direction angle, theta fore The pointing angle is the final pose of the robot.
Preferably, the calculation formula describing the speed weight of the robot is:
wherein, gamma min And gamma max Respectively representing the minimum value and the maximum value of the speed weight, wherein K represents the setting of a proportion value, alpha is an index value and is set as a rational number; d (D) min Represents the shortest distance between the centroid of the robot and the nearby obstacle in the current state, D s Is threshold value, D s The calculation formula of (2) is as follows:
where f is the scaling factor.
Preferably, the calculation formula of the number of predicted trajectories is:
wherein omega is max For the maximum angular velocity of the robot during rotation, Δt is the sampling period interval, [ delta ] f -ω max Δt,δ f +ω max Δt]For the dynamic limit position after the Δt sampling period interval, [ max (δ f ,δ f -ω max Δt),min(δ f ,δ f +ω max Δt)]The range of the rotation angle command set selectable for the dynamic window, Δδ is the resolution of the rotation angle variation.
Preferably, the velocity space is expressed as:
V＝V s ∩V d ∩V a
V s ＝{(v,ω)|v min ≤v≤v max ,ω min ≤ω≤ω max }
V d ＝{(v,ω)|v 0 -v a Δt≤v≤v 0 +v a Δt,ω 0 -ω a Δt≤ω≤ω 0 +ω a Δt}
wherein V is s To limit the resulting speed space by the maximum speed of the robot, V d To limit the resulting velocity space by acceleration, V a In order to speed space of robot under maximum deceleration condition, v a 、ω a Indicating the maximum acceleration of the robot, (v) 0 ,ω 0 ) For the current point speed, (v) a ,ω a ) Is the maximum acceleration.
Preferably, the direction angle evaluation function is expressed as:
heading(v i ,ω i )＝180°-|θ target -θ fore |
in θ target For the included angle theta between the connection line of the predicted position of the robot and the target point and the x-axis direction of the world coordinate system fore The pointing angle (v) is the final pose of the robot i ,ω i ) Speed command information representing the set of trajectories.
Preferably, the obstacle evaluation function is expressed as:
wherein d 0 Maximum scoring of obstacles, i.e. if the distance between the predicted trajectory and the obstacle exceeds d 0 The trajectory is considered safe; d, d min Represents the nearest distance between the predicted track and the obstacle, r robot And r inf Representing the robot radius and the obstacle dilation radius.
According to a second aspect provided by the present invention, the present invention provides an electronic device, comprising:
one or more processors; a memory; and one or more programs stored in the memory, the one or more programs including instructions for performing the local path planning method for an indoor complex environment as described in any of the above.
According to a third aspect of the present invention, there is provided a computer readable storage medium comprising one or more programs for execution by one or more processors of an electronic device, the one or more programs comprising instructions for performing a local path planning method for an indoor complex environment as described in any of the above.
Compared with the prior art, the invention has the following beneficial effects:
(1) According to the invention, the forward direction of the robot is guided by the artificial potential field, so that the problems of poor prospective, poor real-time obstacle avoidance effect and the like of the traditional DWA algorithm can be effectively solved; the potential field evaluation function also considers the obstacle information, and the consideration range is larger than that of the obstacle avoidance evaluation function, so that the obstacle avoidance capability of the algorithm is improved to a certain extent, and the safety of the robot in the operation process is improved.
(3) The invention provides a dynamic speed weight parameter adjustment mode according to obstacle information, which can adaptively improve speed evaluation weight according to obstacle density, wherein the basic mode is that when the speed evaluation weight is close to an obstacle, the speed weight is reduced to ensure safety; and in the open area, the speed weight is increased to improve the running efficiency. The method aims at purposefully adjusting the running instruction of the robot according to the environment information, and seeking balance between safety and rapidity, so that the robot can still keep a better running posture in a complex environment.
Drawings
Fig. 1 is a flow chart of a local path planning method for an indoor complex environment according to the present embodiment.
Figure 2 is a schematic diagram of a Gazebo build 3D environment.
Fig. 3 is an environmental plan view.
Fig. 4 is a global map.
Fig. 5 is a grid map.
FIG. 6 is an rviz launch map using a complex library indoor environment built on Gazebo.
Fig. 7 is a simulation result of MATLAB path planning using the embodiment of fig. 1.
FIG. 8 is simulation results of an Rviz path planning using the embodiment of FIG. 1.
Detailed Description
The invention will now be described in detail with reference to the drawings and specific examples. The present embodiment is implemented on the premise of the technical scheme of the present invention, and a detailed implementation manner and a specific operation process are given, but the protection scope of the present invention is not limited to the following examples.
According to a first aspect of the present invention, referring to fig. 1, this embodiment provides a local path planning method for an indoor complex environment, including the following steps:
s1: and (3) scanning environment information, and constructing a static grid digital map of the indoor environment according to the environment information, wherein the environment information comprises barriers, environment boundary lines and the like.
As an alternative implementation, a complex indoor environment is built by using Gazebo software, as shown in fig. 2 and 3, a robot scans environment information by using its own 2D laser radar, and a two-dimensional grid map is built by using a Gmapping algorithm, so as to generate a global map and a static grid digital map of the indoor environment scaled in equal proportion, as shown in fig. 4.
S2: and self-adaptively adjusting the speed weight of the robot according to the dynamic speed weight parameters of the obstacle.
Specifically, when the obstacle is closer, the speed weight is reduced to ensure the safety; and in the open area, the speed weight is increased to improve the running efficiency.
The method aims at purposefully adjusting the running instruction of the robot according to the environment information, and seeking balance between safety and rapidity, so that the robot can still keep a better running posture in a complex environment.
The calculation formula of the speed weight of the robot is as follows:
wherein, gamma min And gamma max Respectively representing the minimum and maximum values of the velocity weights, generally, gamma min The corresponding value with the best safety performance in the dense barrier area is set, and is not suitable to be too small; gamma ray max Setting a corresponding value capable of rapidly passing through the dense area of the obstacle, and also not suitable for overlarge, and setting a speed weight threshold can avoid the problem of overlarge or underspeed speed score; k represents the setting of a proportion value, generally set to 1, otherwise, the change of the speed weight is discontinuous and abrupt change occurs; alpha is an index value and is set as a rational number; d (D) min Represents the shortest distance between the centroid of the robot and the nearby obstacle in the current state, D s If the closest distance between the centroid of the robot and the obstacle is larger than the threshold, the robot is indicated to have little influence on the safety of the robot in operation under the current environmental conditions, and the robot can keep high-speed movement, and the calculation mode is as follows:
where f is the scaling factor.
S3: and calculating the current speed space of the robot, and sampling the speed space to obtain a group speed instruction.
In this embodiment, a non-omnidirectional robot model is employed for speed space computation and sampling.
The current speed space of the robot can be expressed as:
V＝{(v,ω)|v 1 ≤v≤v 2 ,ω 1 ≤ω≤ω 2 }
in the formula, mostThe important point is the setting of the speed limit, i.e. v 1 、v 2 、ω 1 、ω 2 In general, the setting of the speed space considers three limits, namely a maximum speed limit, an acceleration limit and an obstacle limit, respectively.
The maximum speed limit refers to the fact that the robot has its fixed speed and angular speed limits without being subject to other external effects. Let the speed space obtained by the maximum speed limitation of the robot be V s ，V s Can be expressed as:
V s ＝{(v,ω)|v min ≤v≤v max ,ω min ≤ω≤ω max }
in the formula, v min 、v max 、ω min 、ω max Representing the maximum minimum speed and angular speed limits of the robot, respectively.
Acceleration limit means that the robot can only move within a certain speed range due to the maximum acceleration limit at the current speed of the robot. Assuming that the robot moves for one step for Δt, the current point speed is (v 0 ,ω 0 ) Maximum acceleration is (v) a ,ω a ) The velocity space V obtained by acceleration limitation d Can be expressed as:
V d ＝{(v,ω)|v 0 -v a Δt≤v≤v 0 +v a Δt,ω 0 -ω a Δt≤ω≤ω 0 +ω a Δt}
the obstacle limit indicates that when the robot detects an obstacle, a braking distance should be reserved for decelerating, so that safe operation of the robot is ensured. In order to ensure that the robot can stop in time before collision with an obstacle, the speed space V of the robot is reduced at maximum a Can be expressed as:
wherein dist (v, ω) represents the closest distance from the obstacle on the trajectory corresponding to the velocity (v, ω), v a 、ω a Indicating machineMaximum acceleration of the robot.
According to the above speed limit, the final speed space is obtained as follows:
V＝V s ∩V d ∩V a
s4: according to each group of speed instructions, carrying out motion track prediction by adopting a dynamic window method to obtain n groups of predicted tracks, constructing an evaluation function according to the speed instructions of each group of predicted tracks and the speed weight of the robot, calculating the score of each group of predicted tracks according to the evaluation function, and screening the predicted track with the highest score as a final track; the evaluation function is composed of a direction angle evaluation function, an obstacle distance evaluation function, a speed evaluation function, and a potential field evaluation function.
According to the speed instruction obtained by sampling, the motion track prediction in a period of time can be performed, and the track simulation method in the dynamic window method is to guide the robot to move in a period of time by utilizing a group of speed instructions, so that the speed of the robot is kept unchanged in the motion process.
Assuming the simulation time length t and the time resolution is deltat, the simulation time t can be divided into n sections according to the resolution deltat, and the robot pose is continuously updated according to the robot model in each time increment, so that the robot simulation track in the t time is finally obtained. And simulating each group of speed instructions to finally obtain m groups of tracks.
In the dynamic window method, the prediction area and the angle limitation of the dynamic window are improved.
Assuming that the speed of the mobile robot is constant, assuming that the maximum angular speed when the robot rotates is ω max The dynamic limit position after the delta t sampling period interval is [ delta ] f -ω max Δt,δ f +ω max Δt]The method comprises the steps of carrying out a first treatment on the surface of the At the same time, the limitation of the robot is also considered, and the rotation angle range of the robot in the sampling period is [ delta ] fmin ,δ fmax ]The method comprises the steps of carrying out a first treatment on the surface of the Combining the dynamic model of the robot and the rotation angle limit of the robot, the selectable rotation angle command set range of the dynamic window is [ max (delta) f ,δ f -ω max Δt),min(δ f ,δ f +ω max Δt)]Resolution of rotation angle changeFor delta, the number of sampling actions or trajectories for the current window is:
the formula describing the evaluation function is:
G mod (v i ,ω i )＝α·heading(v i ,ω i )+β·dist(v i ,ω i )+γ adapt ·velocity(v i ,w i )+λ·potential(v i ,ω i )
wherein G is mod (v i ,ω i ) To evaluate the function, gamma adapt For the adaptive velocity evaluation weight, λ is potential field evaluation weight, potential (v i ,ω i ) As a potential field evaluation function, (v) i ,ω i ) Speed command information representing the set of tracks, head (v i ,ω i ) Represents the direction angle evaluation function, α represents the direction angle evaluation weight, dist (v i ,ω i ) Representing an obstacle evaluation function, β representing an obstacle evaluation weight; velocity (v) i ,ω i ) A speed evaluation function is represented.
The method adopts a potential field evaluation function similar to a direction angle evaluation function, mainly evaluates the last pose of a predicted track of a robot, firstly detects surrounding environment information, then calculates a resultant force direction angle according to the positions of an obstacle and a target point, and finally makes a difference with the predicted direction angle to obtain a deviation value as a basis of track judgment.
The potential field evaluation function can be expressed as:
potential(v i ,w i )＝180°-|θ force -θ fore |
wherein θ force Representing the resultant force direction angle.
The basic idea of this is to hope that the robot forward direction is consistent with the potential field force direction; the forward direction of the robot is guided by the artificial potential field, so that the problems of poor prospective, poor real-time obstacle avoidance effect and the like of the traditional DWA algorithm can be effectively solved; the potential field evaluation function also considers the obstacle information, and the consideration range is larger than that of the obstacle avoidance evaluation function, so that the obstacle avoidance capability of the algorithm is improved to a certain extent, and the safety of the robot in the operation process is improved.
Establishing an environment model of the robot by using an artificial potential field method, and if the repulsive potential field is U dist The environmental boundary potential field is U bound The potential field of the obstacle is U obs The repulsive potential field is of the form:
U dist ＝U bound +U obs
if the gravitational potential field towards the target point is U goal The total potential field is then:
U＝U dist +U goal
the direction angle evaluation function is expressed as:
heading(v i ,ω i )＝180°-|θ target -θ fore |
in θ target Is the included angle theta between the connection line of the predicted position of the robot and the target point and the x-axis direction of the world coordinate system fore Pointing angle of final pose of robot; the evaluation function aims at selecting a track with a small difference between the target angle and the pointing angle, and the basic idea is to aim the advancing direction of the robot at the end point.
The obstacle evaluation function is to evaluate the whole track, and takes the following form:
wherein d is 0 Maximum scoring of obstacles, i.e. if the distance between the predicted trajectory and the obstacle exceeds d 0 The trajectory is considered safe; d, d min Represents the nearest distance between the predicted track and the obstacle, r robot And r inf Representing the robot radius and the obstacle dilation radius. The evaluation function aims at selecting a trajectory that is further away from the obstacle, the basic idea of which is to hope that the robot will not collide during operation.
The speed evaluation function is used for evaluating a speed instruction of a predicted track, and mainly aims at the linear speed in a robot speed space, and takes the following form:
velocity(v i ,ω i )＝|v i |
the evaluation function aims at selecting a trajectory with a larger linear velocity, the basic idea of which is to expect the movement speed of the robot to be as fast as possible.
The local path planning method for the indoor complex environment provided by the embodiment is applied to the Rviz starting map of the indoor environment of the complex library constructed on the Gazebo shown in fig. 6, and simulation is performed by using MATLAB and Rviz respectively, and the simulation results are shown with reference to fig. 7 and 8, and the planning routes of the algorithm on the two simulation platforms are approximately the same.
In summary, the invention combines the traditional APF algorithm to provide a fusion optimization mode, and the track planning solves the problem that 'I are now located somewhere and how to reach the target' can be regarded as the core of the realization of the local motion of the robot, and is the upper tissue organization part of the frame of the realization of the local motion; different from global path planning, the local track planning considers the influence of an actual moving obstacle and the constraint of robot dynamics, a section of feasible track from the current position to a target point is planned as far as possible in a specified time domain according to the direction indicated by the global path, and the time cost of the process is relatively low. Because the planned track is regarded as the reference of the state control of the lower robot, the rationality of the upper planning is closely related to the tracking control effect of the lower layer, and is the basis and the guarantee of the realization of the local motion of the vehicle.
The basic idea is to apply an improved APF algorithm model, construct a new potential field evaluation mechanism, and adaptively adjust the weight of each evaluation function according to surrounding environment information so as to improve the algorithm flexibility while enhancing the algorithm prospective, and the studied local track planning algorithm framework is composed of two parts of dynamic window searching and local optimal screening, wherein the two parts are assumed to be known surrounding environment information such as barriers, environment boundary lines and the like, and the specific expression is as follows: the dynamic track clusters under the current and prediction windows are sampled by a dynamic window method, the better sub-tracks are screened out by the environmental potential field set by an analog artificial potential field method, and the sub-tracks at each discrete point moment are connected, so that a reasonable local track is output.
According to a second aspect provided by the present invention, the present invention provides an electronic device, comprising:
one or more processors; a memory; and one or more programs stored in the memory, the one or more programs including instructions for performing the local path planning method for an indoor complex environment as described in any of the above.
According to a third aspect of the present invention, there is provided a computer readable storage medium comprising one or more programs for execution by one or more processors of an electronic device, the one or more programs comprising instructions for performing a local path planning method for an indoor complex environment as described in any of the above.
The functions, if implemented in the form of software functional units and sold or used as a stand-alone product, may be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention may be embodied essentially or in a part contributing to the prior art or in a part of the technical solution, in the form of a software product stored in a storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server, a network device, etc.) to perform all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
The foregoing describes in detail preferred embodiments of the present invention. It should be understood that numerous modifications and variations can be made in accordance with the concepts of the invention by one of ordinary skill in the art without undue burden. Therefore, all technical solutions which can be obtained by logic analysis, reasoning or limited experiments based on the prior art by the person skilled in the art according to the inventive concept shall be within the scope of protection defined by the claims.
Claims (10)
1. The local path planning method for the indoor complex environment is characterized by comprising the following steps of:
acquiring dynamic speed weight parameters of the obstacle according to a pre-constructed indoor map containing obstacle information and environment boundary lines, and adaptively adjusting the speed weight of the robot;
calculating the current speed space of the robot, and sampling the speed space to obtain a group speed instruction;
obtaining the current position and the target position of the robot, further predicting the motion track by adopting a dynamic window method according to each group of speed instructions to obtain n groups of predicted tracks, constructing an evaluation function according to the speed instructions of each group of predicted tracks and the speed weight of the robot, calculating the score of each group of predicted tracks according to the evaluation function, and screening the predicted track with the highest score as a final track; the evaluation function is composed of a direction angle evaluation function, an obstacle distance evaluation function, a speed evaluation function, and a potential field evaluation function.
2. The local path planning method for an indoor complex environment according to claim 1, wherein the formula describing the evaluation function is:
G mod (v i ,ω i )＝α·heading(v i ,ω i )+β·dist(v i ,ω i )+γ adapt ·velocity(v i ,w i )+λ·potential(v i ,ω i )
wherein G is mod (v i ,ω i ) To evaluate the function, gamma adapt For the adaptive velocity evaluation weight, λ is potential field evaluation weight, potential (v i ,ω i ) As a potential field evaluation function, (v) i ,ω i ) Speed command information representing the set of tracks, head (v i ,ω i ) Represents the direction angle evaluation function, α represents the direction angle evaluation weight, dist (v i ,ω i ) Representing an obstacle evaluation function, β representing an obstacle evaluation weight; velocity (v) i ,ω i ) A speed evaluation function is represented.
3. The method for planning a local path for an indoor complex environment according to claim 2, wherein the process of obtaining the potential field evaluation function is specifically: evaluating the last pose of the predicted track of the robot, calculating a resultant force direction angle according to the positions of the obstacle and the target point, and making a difference value between the resultant force direction angle and the predicted direction angle to obtain a deviation value;
the potential field evaluation function is expressed as:
potential(v i ,w i )＝180°-|θ force -θ fore |
in θ force Represents the resultant force direction angle, theta fore The pointing angle is the final pose of the robot.
4. The local path planning method for an indoor complex environment according to claim 1, wherein a calculation formula describing a speed weight of the robot is:
wherein, gamma min And gamma max Respectively representing the minimum value and the maximum value of the speed weight, wherein K represents the setting of a proportion value, alpha is an index value and is set as a rational number; d (D) min Represents the shortest distance between the centroid of the robot and the nearby obstacle in the current state, D s Is threshold value, D s The calculation formula of (2) is as follows:
where f is the scaling factor.
5. The local path planning method for an indoor complex environment according to claim 1, wherein the calculation formula of the number of predicted trajectories is:
wherein omega is max For the maximum angular velocity of the robot during rotation, Δt is the sampling period interval, [ delta ] f -ω max Δt,δ f +ω max Δt]For the dynamic limit position after the Δt sampling period interval, [ max (δ f ,δ f -ω max Δt),min(δ f ,δ f +ω max Δt)]The range of the rotation angle command set selectable for the dynamic window, Δδ is the resolution of the rotation angle variation.
6. A local path planning method for an indoor complex environment according to claim 1, wherein the velocity space is expressed as:
V＝V s ∩V d ∩V a
V s ＝{(v,ω)|v min ≤v≤v max ,ω min ≤ω≤ω max }
V d ＝{(v,ω)|v 0 -v a Δt≤v≤v 0 +v a Δt,ω 0 -ω a Δt≤ω≤ω 0 +ω a Δt}
wherein V is s To limit the resulting speed space by the maximum speed of the robot, V d To limit the resulting velocity space by acceleration, V a In order to speed space of robot under maximum deceleration condition, v a 、ω a Indicating the maximum acceleration of the robot, (v) 0 ,ω 0 ) For the current point speed, (v) a ,ω a ) Is the maximum acceleration。
7. The local path planning method for an indoor complex environment according to claim 1, wherein the direction angle evaluation function is expressed as:
heading(v i ,ω i )＝180°-|θ target -θ fore |
in θ target For the included angle theta between the connection line of the predicted position of the robot and the target point and the x-axis direction of the world coordinate system fore The pointing angle (v) is the final pose of the robot i ,ω i ) Speed command information representing the set of trajectories.
8. The local path planning method for an indoor complex environment according to claim 1, wherein the obstacle evaluation function is expressed as:
wherein d 0 Maximum scoring of obstacles, i.e. if the distance between the predicted trajectory and the obstacle exceeds d 0 The trajectory is considered safe; d, d min Represents the nearest distance between the predicted track and the obstacle, r robot And r inf Representing the robot radius and the obstacle dilation radius.
9. An electronic device, comprising:
one or more processors; a memory; and one or more programs stored in the memory, the one or more programs including instructions for performing the local path planning method for an indoor complex environment according to any one of claims 1-8.
10. A computer-readable storage medium comprising one or more programs for execution by one or more processors of an electronic device, the one or more programs including instructions for performing the local path planning method for an indoor complex environment of any of claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310782027.XA CN116643572A (en) | 2023-06-29 | 2023-06-29 | Local path planning method, electronic equipment and medium for indoor complex environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310782027.XA CN116643572A (en) | 2023-06-29 | 2023-06-29 | Local path planning method, electronic equipment and medium for indoor complex environment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116643572A true CN116643572A (en) | 2023-08-25 |
Family
ID=87624911
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310782027.XA Pending CN116643572A (en) | 2023-06-29 | 2023-06-29 | Local path planning method, electronic equipment and medium for indoor complex environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116643572A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117111617A (en) * | 2023-10-23 | 2023-11-24 | 山东优宝特智能机器人有限公司 | Robot path planning method and system considering collision uncertainty of perception dead zone |
| CN117176011A (en) * | 2023-11-02 | 2023-12-05 | 南通威尔电机有限公司 | Parameter intelligent adjusting method and system for permanent magnet synchronous submersible motor |
| CN117215317A (en) * | 2023-11-09 | 2023-12-12 | 烟台哈尔滨工程大学研究院 | Unmanned ship local path planning method, equipment and storage medium |
-
2023
- 2023-06-29 CN CN202310782027.XA patent/CN116643572A/en active Pending
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117111617A (en) * | 2023-10-23 | 2023-11-24 | 山东优宝特智能机器人有限公司 | Robot path planning method and system considering collision uncertainty of perception dead zone |
| CN117176011A (en) * | 2023-11-02 | 2023-12-05 | 南通威尔电机有限公司 | Parameter intelligent adjusting method and system for permanent magnet synchronous submersible motor |
| CN117176011B (en) * | 2023-11-02 | 2024-02-13 | 南通威尔电机有限公司 | Parameter intelligent adjusting method and system for permanent magnet synchronous submersible motor |
| CN117215317A (en) * | 2023-11-09 | 2023-12-12 | 烟台哈尔滨工程大学研究院 | Unmanned ship local path planning method, equipment and storage medium |
| CN117215317B (en) * | 2023-11-09 | 2024-02-09 | 烟台哈尔滨工程大学研究院 | Unmanned ship local path planning method, equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116643572A (en) | Local path planning method, electronic equipment and medium for indoor complex environment | |
| CN109375632B (en) | Real-time trajectory planning method for automatic driving vehicle | |
| CN111338340B (en) | Model prediction-based local path planning method for unmanned vehicle | |
| Cai | ROBOTICS: From Manipulator to Mobilebot | |
| CN109976340B (en) | Man-machine cooperation dynamic obstacle avoidance method and system based on deep reinforcement learning | |
| Bouton et al. | Reinforcement learning with iterative reasoning for merging in dense traffic | |
| CN112558612B (en) | Heterogeneous intelligent agent formation control method based on cloud model quantum genetic algorithm | |
| CN114442637B (en) | Unmanned vehicle local dynamic obstacle avoidance path planning method | |
| CN115683145A (en) | Automatic driving safety obstacle avoidance method based on track prediction | |
| CN114894212A (en) | Improved path planning method based on adaptive parameters | |
| CN115047878A (en) | DM-DQN-based mobile robot path planning method | |
| Zhang et al. | A hybrid path planning algorithm considering AUV dynamic constraints based on improved A* algorithm and APF algorithm | |
| CN113093804A (en) | Unmanned ship formation control method and control system based on inversion sliding mode control | |
| Sun et al. | Cooperative strategy for pursuit-evasion problem in the presence of static and dynamic obstacles | |
| CN115454083A (en) | Double-layer path planning method based on fast expansion random tree and dynamic window method | |
| CN116009530A (en) | Path planning method and system for self-adaptive tangential obstacle avoidance | |
| CN115373406B (en) | Obstacle crossing method and system for crawling robot | |
| CN114609925B (en) | Training method of underwater exploration strategy model and underwater exploration method of bionic machine fish | |
| Zhang et al. | Target Tracking and Path Planning of Mobile Sensor Based on Deep Reinforcement Learning | |
| CN114167856A (en) | Service robot local path planning method based on artificial emotion | |
| Luo et al. | UAV path planning based on the average TD3 algorithm with prioritized experience replay | |
| CN113985927A (en) | Method for optimizing perching and stopping moving track of quad-rotor unmanned aerial vehicle | |
| Fan et al. | An Improved Path Planning Algorithm With Adaptive Parameters and Predictions | |
| CN116520832A (en) | Speed and density planning method suitable for cluster crossing virtual pipeline | |
| Zhai et al. | Local Trajectory Planning for Obstacle Avoidance of Unmanned Tracked Vehicles Based on Artificial Potential Field Method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication |