CN103092204B - A kind of Robotic Dynamic paths planning method of mixing - Google Patents

Abstract

The invention discloses a kind of Robotic Dynamic paths planning method of mixing, the method can be applied in environmental information part known and there is unknown dynamic and static state barrier simultaneously when.First obtain global path by a kind of genetic algorithm as Global Planning for above-mentioned situation, then carry out sector planning with the Artificial Potential Field Method improved.In sector planning, invention also contemplates that the Velocity-acceleration information of robot and barrier, so have good effect to process active path planning, in addition also contemplate the effect of Global motion planning path to sector planning.The present invention realizes simply, and the robot path obtained comparatively is optimized, and shows good dirigibility in the application simultaneously.

Description

A Hybrid Dynamic Path Planning Method for Robots

technical field

The invention relates to a path planning method, in particular to a path planning method for a mobile robot in an environment with dynamic obstacles and static obstacles.

Background technique

Real-time path planning and navigation of mobile robots is one of the key elements reflecting the robot's autonomous ability, and it is also one of the more difficult problems to solve. Robot path planning is mainly divided into planning with known environmental information and planning with unknown environmental information. For the former, offline planning is mostly used, and the obtained path is better, while the latter mostly uses online planning, which reflects the real-time nature of path planning.

In recent years, many path planning methods have been studied by people. The main path planning methods can be divided into two categories - artificial intelligence methods (AI) and artificial potential field methods (APF). The former mainly uses genetic algorithm (GA), fuzzy logic control (FLC) and artificial neural network (ANN). The latter is widely used in robot path planning due to its simplicity and rapidity. Its basic idea is that the attraction of the target point in the environment to it and the repulsion of obstacles to it constitute a potential field environment. In a dynamic environment, there are two main ideas for using the artificial potential field method to solve planning problems. One solution is proposed by Fujimura et al. The main idea is to use time information as a dimension, and convert dynamic obstacle planning into static planning, but the limitations The reason is that the trajectories of dynamic obstacles need to be known in advance. Another solution was proposed by Ko and Lee. The main idea is to introduce the velocity information of obstacles into the repulsive force function. Ge and Cui made further improvements on this basis. The advantage of this method is that it does not need to know in advance The trajectory of obstacles, so it has better real-time performance.

The situation in real life is often the situation where part of the environmental information is known, such as some working conditions in the factory environment are known, and some working conditions are unknown. The path obtained by using only the above-mentioned online path planning method is less optimized, and only using the above-mentioned offline planning method is relatively slow in calculation speed, and it is difficult to deal with dynamic obstacles.

Contents of the invention

The purpose of the present invention is to overcome the deficiencies of the prior art and provide a hybrid robot dynamic path planning method.

The object of the present invention is achieved through the following technical solutions: a hybrid robot dynamic path planning method comprises the steps:

Step 1: Use the visual sensor to obtain environmental information, including known static obstacle information in the environment, target point information, and the position information of the robot itself;

Step 2: The environmental information obtained in step 1 is represented by a grid method to obtain a grid map, and the size of the grid depends on the planning accuracy;

Step 3: Carry out global path planning with the genetic algorithm for the raster map obtained in step 2, and obtain a global path, which is a polyline;

Step 4: Extract vertices, global target points, and starting points from the polylines obtained in step 3 as key points required for local planning, and these key points are local target points for local planning;

Step 5: Using the dynamic and static obstacles around the mobile robot and the key points mentioned in step 4 as local target points, the artificial potential field method (APF) is used to construct the potential field environment, and the potential field environment is also added by step 4 The attractive potential field of the line segment connected by the obtained adjacent key points to the robot;

Step 6: The robot is subjected to attractive and repulsive forces in the potential field environment constructed in step 5, moves under the action of the resultant force, and performs local target planning;

Step 7: Determine whether the current position of the robot has reached the local target point described in step 4. If it reaches the local target point, update the target point to the next key point as the local target point and turn to step 5 to reconstruct the local potential field environment;

Step 8: If the current local target point is the global target point of the environment, the method ends when the robot reaches the global target point;

The beneficial effect of the present invention is that offline planning is carried out for known environments and online planning is carried out for unknown environments, combining the advantages of the two, the planned path is more optimized, and the processing of unknown obstacles such as dynamic obstacles is also more efficient. Strong flexibility. The method proposed by the invention is more suitable for the path planning of the actual automatic factory environment.

Description of drawings

Fig. 1 is the flowchart of the hybrid robot dynamic path planning method;

Figure 2 is a vector diagram of the robot being repulsed.

detailed description

The purpose and effects of the present invention will become more apparent by describing the present invention in detail below in conjunction with the accompanying drawings.

As shown in Figure 1, the hybrid robot dynamic path planning method of the present invention includes the following steps:

Step 1: Use the visual sensor to obtain environmental information, including known static obstacle information in the environment, target point information, and the position information of the robot itself.

This step can use the method in Chapter 3 of Liu Mingshuo. Flexible Manipulator Trajectory Tracking Based on Binocular Vision Odometry. Dissertation of Zhejiang University. 2011.04 to obtain the position information of the obstacle and the position of the robot itself.

Step 2: The environmental information obtained in step 1 is represented by the grid method to obtain a grid map, and the size of the grid depends on the planning accuracy.

This step can use Zhang Handong. Application of new raster coding method in robot path planning. Journal of Huazhong University of Science and Technology: Natural Science Edition, 2007, 35(1): 50-53. The method expresses the environmental information with the grid method.

Step 3: For the raster map obtained in step 2, use the genetic algorithm to perform global path planning to obtain a global path, which is a polyline.

This step can be adopted using Zhang Handong. Application of new raster coding method in robot path planning. Journal of Huazhong University of Science and Technology: Natural Science Edition, 2007, 35(1): 50-53. The method uses the genetic algorithm to finally get a polyline segment.

Step 4: Extract vertices, global target points, and starting points from the polylines obtained in step 3 as key points required for local planning, and these key points are local target points for local planning.

Get the vertices of the polyline, the starting point and the target point, and number them 1, 2, 3...N in sequence, where 1 is the starting point and N is the target point, and these points are taken as the key points of path planning.

Step 5: Using the dynamic and static obstacles around the mobile robot and the key points mentioned in step 4 as local target points, the artificial potential field method (APF) is used to construct the potential field environment, and the potential field environment is also added by step 4 The attractive potential field of the line segment connected by the adjacent key points to the robot is obtained.

The constructed potential field environment includes a gravitational field function and a repulsive field function. The function of the potential field environment is as follows: U(q)=U _att (q)+U _rep (q), where U _att (q) is the gravitational field function U _rep (q) is the repulsion field function, and q is the robot position vector.

Regarding the gravitational field function, the present invention introduces the speed information and acceleration information of the robot relative to the target point on the basis of the traditional gravitational field function, so that the force of the robot in the force field environment can be adjusted according to the position, speed, and acceleration. In addition, in order to track a better route, the robot is always attracted by an existing path in addition to being attracted by the target point, which is the so-called "line potential field". The improved gravitational field function is: U _att (q,v,a)=α _q ||qq _g || ^m +α _v ||vv _g || ⁿ +α _a ||aa _g || ^p +α _l (||qq _line || ^l +||vv _line || ^l +||aa ^line || ^l ) where q, v, a are the vectors of the position, velocity and acceleration of the robot, α _q , α _v , α _a , m, n, and p are proportional coefficients. Different values represent the weights of the relative position information, relative velocity information, and relative acceleration information of the robot and the target point in the gravitational function. α _l and l represent the weight of the line potential field. When α _v , α _a and α _l are zero, the improved gravitational field function is the same as the traditional gravitational field function. q _g , v _g , a _g are the position, velocity and acceleration vectors of the target point. q _line =(x ₀ ,r(x ₀ )) ^T where r(x) is the equation of the curve (x ₀ ,r(x ₀ )) represents the coordinates of the closest point on the curve to the current position of the mobile robot. The coordinates will change accordingly. v _line and a _line are the velocity and acceleration vectors of the points on the path, usually v _line =(0,0) ^T , a _line =(0,0) ^T represents the acceleration and speed is 0.

Similarly, the repulsive force field also includes the position information, relative velocity information and relative acceleration information of the robot and the obstacle, so that the robot can synthesize the above information to obtain the obstacle avoidance force. The specific formula is as follows: U _rep (q,v,a)=α _q (1/ρ _obs -1/ρ ₀ )+α _v v _ro +α _a a _ro where they are the vectors of the position, velocity and acceleration of the robot, _ρ0 represents the safe distance from the obstacle to the robot, and the robot is repelled by the obstacle only within the range of _ρ0 . ρ _obs =||qq _obs || is the distance from the center of the robot to the center of the obstacle, α _v , α _a and α _l are proportional coefficients, v _ro and a _ro represent the speed vector of the obstacle and the robot respectively Difference, the vector difference between the acceleration velocity of the obstacle and the acceleration of the robot.

Step 6: The robot is subjected to attractive and repulsive forces in the potential field environment constructed in step 5, moves under the action of the resultant force, and performs local goal planning.

The force on the robot in the potential field environment is as follows: F=F _att +F _rep where F _att is the attractive force on the robot, and F _rep is the repulsive force on the robot.

It can be deduced from the gravitational function in step 5 that the attractive force of the robot is $f_{\mathrm{att}}(q,v,a)=-\▿u_{\mathrm{att}}(q,v,a)=-{\▿}_q{u}_{\mathrm{att}}(q,v,a)-{\▿}_v{u}_{\mathrm{att}}(q,v,a)-{\▿}_a{u}_{\mathrm{att}}(q,v,a)$ The robot is subjected to gravitational force F _attq (q) caused by relative position, force F _attv (v) caused by relative velocity, and force F _atta (a) caused by relative velocity $f_{\mathrm{attq}}\left(q\right)=-{\∂u}_{\mathrm{att}}(q,v,a)/\∂q={\mathrm{m\α}}_q{\left|\right|q-q_g\left|\right|}^{m-1}{e}_{\mathrm{qrg}}+l{\mathrm{\α}}_l{\left|\right|q-q_{\mathrm{line}}\left|\right|}^{l-1}{e}_{\mathrm{qrline}},$ $f_{\mathrm{attv}}\left(v\right)=-{\∂u}_{\mathrm{att}}(q,v,a)/\∂v$ $={\mathrm{n\α}}_v{\left|\right|v-v_g\left|\right|}^{\mathrm{no}-1}{e}_{\mathrm{vrg}}+{\mathrm{l\α}}_l{\left|\right|v-v_{\mathrm{line}}\left|\right|}^{l-1}{e}_{\mathrm{vrline}},$ $f_{\mathrm{atta}}\left(a\right)=-\∂u_{\mathrm{att}}(q,v,a)/\∂a={\mathrm{p\α}}_a{\left|\right|a-a_g\left|\right|}^{p-1}{e}_{\arg }$ $+{\mathrm{l\α}}_{\mathrm{line}}{\left|\right|a-a_{\mathrm{line}}\left|\right|}^{l-1}{e}_{\mathrm{arline}}$ Then F _att (q,v,a)=F _attq (q)+F _attv (v)+F _atta (a), where e _qrg indicates that the robot position points to the target object position, and e _vrg indicates that the robot velocity vector points to the target object speed Vector, e _arg indicates that the acceleration vector of the robot points to the acceleration vector of the target object, and the other symbols are the same as those described in step 5.

From the repulsion function in step 5, the repulsion force on the robot can be deduced as $f_{\mathrm{rep}}(q,v,a)=-{\▿u}_{\mathrm{rep}}(q,v,a)=f_{\mathrm{repq}}\left(q\right)+f_{\mathrm{repv}}\left(v\right)+f_{\mathrm{repa}}\left(a\right).$ F _repq (q), F _repv (v), and F _repa (a) are the repulsive forces caused by the relative position, velocity, and acceleration of the robot and the obstacle, respectively. $f_{\mathrm{repq}}={\mathrm{\α}}_q{e}_{\mathrm{or}}/{\mathrm{\ρ}}_{\mathrm{obs}}^2+{\mathrm{\α}}_v{v}_{\mathrm{ro}\⊥}{e}_{\mathrm{ro}\⊥}/{\mathrm{\ρ}}_{\mathrm{obs}}+{\mathrm{\α}}_a{a}_{\mathrm{ro}\⊥}{e}_{\mathrm{ro}\⊥}/{\mathrm{\ρ}}_{\mathrm{obs}},$ F _repv =α _v v _ro e _or , F _repa =α _a v _ro e _or , the three terms of F _repq α _v v _ro⊥ e _ro⊥ /ρ _obs , α _a a _ro⊥ e _ro⊥ /ρ _obs are respectively defined as F _repq1 , F _repq2 , and F _repq3 According to the above rules, the resultant force diagram of the robot's repulsive force is shown in Figure 2 Show. e _or and e _ro⊥ represent the direction of the obstacle pointing to the robot, and the direction of the robot pointing to the obstacle after turning clockwise at a right angle, v _ro⊥ e _ro⊥ and a _ro⊥ e _ro⊥ represent v _ro and a _ro in e component in the _ro⊥ direction. F _repq is split into F _repq2 and F repq3 in the e _ro⊥ direction, and F _repq1 in the e _or direction is merged with F _repqv and F _repqa to obtain F _repq3 , and the rest of the symbols are the same as _those described in step 5.

Step 7: Determine whether the current position of the robot reaches the local target point described in step 4. If it reaches the local target point, update the target point to the next key point as the local target point and turn to step 5 to reconstruct the local potential field environment.

The robot can know the current position of the robot through the visual sensor. If the robot reaches the local target point, it will take the next key point in 1...N mentioned in step 4, and mark the previous key point as visited.

Step 8: If the current local target point is the global target point of the environment, the method ends when the robot reaches the global target point.

The present invention adopts a hybrid obstacle avoidance method for partially known environmental information, first uses genetic algorithm to carry out global planning to obtain a global path, and then uses the improved artificial potential field method to carry out local planning. The improved artificial potential function adds speed information and acceleration information, which can not only make the robot avoid obstacles better but also make the robot reach the target point without excessive speed. The improved artificial potential function also adds a line potential field to better track the desired path. This method is simple and feasible, and can meet the requirements of real-time path planning for robots.

Claims (1)

1. a hybrid robot dynamic path planning method, is characterized in that, comprises the steps: Step 1: Use the visual sensor to obtain environmental information, including known static obstacle information in the environment, target point information, and the position information of the robot itself; Step 2: The environmental information obtained in step 1 is represented by a grid method to obtain a grid map, and the size of the grid depends on the planning accuracy; Step 3: Carry out global path planning with the genetic algorithm for the raster map obtained in step 2, and obtain a global path, which is a polyline; Step 4: Extract vertices, global target points, and starting points from the polylines obtained in step 3 as key points required for local planning, and these key points are local target points for local planning; Step 5: Using the dynamic and static obstacles around the mobile robot and the key points described in step 4 as local target points, the artificial potential field method (APF) is used to construct the potential field environment, and the potential field environment is also added to the potential field environment by step 4 The attractive potential field of the line segments connected by the adjacent key points obtained in the robot; this step is specifically: the function of constructing the potential field environment is as follows: U(q)=U _att (q)+U _rep (q), Where U _att (q) is a gravitational field function, U _rep (q) is a repulsive field function, and q is a robot position vector; Among them, the velocity information and acceleration information of the robot relative to the target point are introduced on the basis of the traditional gravitational field function, so that the force of the robot in the force field environment can be adjusted according to the position, velocity, and acceleration; in addition, in order to track a better In addition to being attracted by the target point, the robot is always attracted by an existing path. The improved formula is U _att (q,v,a)=α _q ||qq _g || ^m +α _v || vv _g || ⁿ +α _a ||aa _g || ^p +α _l (||qq _line || ^l +||vv _line || ^l +||aa _line || ^l ), where, q _g , v _g , a _g is the position, velocity and acceleration vector of the target point; q _line = (x ₀ ,r(x ₀ )) ^T , where r(x) is the equation of the ideal path curve obtained by global planning, (x ₀ , r(x ₀ )) indicates the coordinates of the nearest point on the curve to the current position of the mobile robot, and the coordinates will change accordingly as the robot moves; v _line and a _line are the velocity and acceleration vectors of the points on the path; || qq _g || ^m , ||vv _g || ⁿ , ||aa _g || ^p respectively represent the potential field information of the relative position, relative velocity and relative acceleration between the robot and the target point; m, n and p are positive Integer, the larger the value, the higher the strength of these potential field information; α _q , α _v , α _a are proportional coefficients, which represent the weight of the potential field information of the relative position, relative velocity, and relative acceleration in the gravitational function; similarly , ||qq _line || ^l , ||vv _line || ^l , ||aa _line || ^l respectively represent the potential field information of the relative position, relative velocity and relative acceleration between the robot and the ideal path curve, and their sum Indicates the line potential field information; l is a positive integer, the larger the value, the higher the intensity of the line potential field information; α _l is a real number between 0 and 1, indicating the weight of the line potential field information; Similarly, the repulsive force field also includes the position information, relative velocity information and relative acceleration information of the robot and the obstacle, so that the robot can synthesize the above information to obtain the obstacle avoidance force; the specific formula is as follows: U _rep (q, v, a) = α _q (1/ρ _obs -1/ρ ₀ )+α _v v _ro +α _a a _ro , where q, v, and a are the vectors of the robot's position, velocity, and acceleration respectively, and ρ ₀ represents the safety of the obstacle to the robot distance, the robot is repelled by the obstacle only within the range of ρ ₀ ; ρ _obs = ||qq _obs || is the distance from the center of the robot to the center of the obstacle, and α _v , α _a and α _q are the ratios The coefficient represents the weight of the potential field information of the above-mentioned relative velocity, relative acceleration and relative position in the repulsion function, v _ro and a _ro respectively represent the vector difference between the speed of the obstacle and the speed of the robot, the acceleration of the obstacle and the acceleration of the robot the vector difference; Step 6: The robot is subjected to attractive and repulsive forces in the potential field environment constructed in step 5, moves under the action of the resultant force, and performs local target planning; Step 7: Determine whether the current position of the robot has reached the local target point described in step 4. If it reaches the local target point, update the target point to the next key point as the local target point and turn to step 5 to reconstruct the local potential field environment; Step 8: If the current local target point is the global target point of the environment, the method ends when the robot reaches the global target point.