CN111290408A - Mobile robot trajectory tracking control method - Google Patents
Mobile robot trajectory tracking control method Download PDFInfo
- Publication number
- CN111290408A CN111290408A CN202010248723.9A CN202010248723A CN111290408A CN 111290408 A CN111290408 A CN 111290408A CN 202010248723 A CN202010248723 A CN 202010248723A CN 111290408 A CN111290408 A CN 111290408A
- Authority
- CN
- China
- Prior art keywords
- path
- mobile robot
- formula
- function
- population
- 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.)
- Withdrawn
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 239000002245 particle Substances 0.000 claims abstract description 32
- 238000013178 mathematical model Methods 0.000 claims abstract description 12
- 230000003068 static effect Effects 0.000 claims abstract description 12
- 230000002068 genetic effect Effects 0.000 claims abstract description 11
- 230000007246 mechanism Effects 0.000 claims abstract description 3
- 230000000739 chaotic effect Effects 0.000 claims description 17
- 230000003044 adaptive effect Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000006978 adaptation Effects 0.000 claims description 5
- 108090000623 proteins and genes Proteins 0.000 claims description 4
- 238000013507 mapping Methods 0.000 claims description 3
- 230000035772 mutation Effects 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 27
- 230000007547 defect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000006467 substitution reaction 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
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses a mobile robot track tracking control method, relates to the technical field of robots, and solves the problems that the path length obtained by a robot path planning method in the prior art is too large and is difficult to reach an optimal value, and the technical key points are as follows: the method comprises the following steps: s1, establishing a mathematical model; s2, planning a global static path by adopting an improved genetic algorithm introducing a self-adaptive mechanism according to the mathematical model; s3, planning a local dynamic path by adopting an improved particle swarm algorithm for randomly assigning the speed of the active particles according to the mathematical model; the invention plans the global static path and the local dynamic path of the robot by improving the genetic algorithm and the particle swarm algorithm, so that the global static path obtained by the robot is short, the local dynamic path is quickly planned, the local obstacle avoidance capability of the mobile robot is improved, and the working efficiency of the mobile robot is improved.
Description
Technical Field
The invention relates to the technical field of robots, in particular to a mobile robot trajectory tracking control method.
Background
The wheel type mobile robot is the most common robot, has multiple functions of environmental perception, dynamic decision and planning, behavior control and execution and the like, has high self-planning, self-organization and self-adaption capabilities, has incomparable advantages when being applied to the aspects of automatic material carrying, special crowd service, emergency rescue and disaster relief, unknown and dangerous region exploration and the like, is widely applied to the technical fields of industry and agriculture, service industry, emergency rescue and disaster relief and the like, and has positive and deep influence on the production and life of the human society. The wheeled mobile robot can autonomously move to a destination through autonomous path planning under unmanned intervention and complex environment, and a specific work function is completed at the destination, wherein the path planning is one of the most important operation steps when the wheeled mobile robot carries out tasks to the destination, the time for the robot to reach the destination determines the time for reaching the destination, the time for the robot to reach the destination determines the efficiency for the robot to carry out the tasks, and particularly in tasks such as rescue and disaster relief, the time for the robot to spend on the road is a crucial factor.
At present, in the path planning of robots, a Logistic model is mostly adopted in a genetic algorithm to generate a chaotic sequence as a primary population solution or chaotic random disturbance is added in a variation operation, so that a global path is obtained, but the defects of large search blind area, low convergence speed and the like exist, and the obtained path is often too long and is difficult to reach an optimal value.
Disclosure of Invention
The invention aims to solve the technical problem that the path length obtained by a robot path planning method in the prior art is too large and is difficult to reach an optimal value.
In order to achieve the purpose, the invention provides the following technical scheme:
a mobile robot track tracking control method comprises the following steps:
s1, environment modeling, namely establishing a mathematical model according to the working environment of the mobile robot;
s2, planning a global static path according to the mathematical model, wherein the global static path planning adopts an improved genetic algorithm introducing an adaptive mechanism;
and S3, planning a local dynamic path for obstacle avoidance according to the mathematical model, wherein the local dynamic path planning adopts an improved particle swarm algorithm for randomly assigning the speed of the active particles.
As a further aspect of the present invention, in S2, the improved genetic algorithm comprises the steps of:
s2-1, generating a Start point Start and a Goal of a global path according to a mathematical model established by the working environment of the robot, and initializing relevant parameters;
s2-2, generating an initialization population by using an information entropy method, wherein the size of the initialization population is M, and the iteration number is T;
s2-3, formula
Calculating each individual adaptive value, wherein W is the number of intersections of the global path i and the obstacles in the environment;
s2-4, formula
Selecting an individual i, wherein M is the size of the population; selecting individuals with higher fitness in the current population according to a game board method; according to the formula
Performing crossover, mutation operations, wherein fmaxFor the maximum fitness function value of the current population, favgFor the current population mean adaptation function value, f′The fitness function value of the individual with the greater fitness function among the hybrid individuals, wherein f is the fitness function value of the individual with the variation, pc1、pc2、pm1And pm2Is a normal number;
s2-5, performing chaotic disturbance on individuals with the fitness of 10% in the population, wherein the chaotic disturbance is subjected to chaotic evolution along the direction of the vertical line segment where each individual is located;
s2-6, screening the discrete points after the chaotic disturbance according to a formula
Inverse mapping is carried out to obtain a gene sequence, and then according to a formula,
calculating a new fitness function F'iJudging whether the discrete point is still a free point or not, if the discrete point is still a free point, and F'i≥FiFrom P'iIn place of PiOtherwise, the original path P is reservedi
S2-7, judging whether the chaotic iteration reaches the maximum times, if not, turning to S2-4, otherwise, carrying out S2-8;
s2-8, automatically adding 1 to the iteration time T, judging whether the T reaches the specified iteration time T, if not, turning to S2-3, otherwise, carrying out S2-9;
and S2-9, outputting the optimal individuals in the current population.
As a further scheme of the invention, in S2-4, the calculation formula of f (i) is as follows:
wherein, FmaxFor the current maximum adaptation value, FminAnd the current minimum adaptive value a is a constant larger than zero, T is the current iteration time, and T is the maximum iteration time.
As a further aspect of the present invention, in S2-5, the evolution method is as follows:
let a discrete point on the vertical line segment Li
The corresponding value range is [ a ]i，bi]Then the discrete point evolves to:
the calculation formula is as follows:
as a further aspect of the present invention, in S3, the improved particle swarm algorithm includes the following steps:
s3-1, defining a shortest penalty function:
wherein lminThe Euclidean distance from the current point to the target point, and l is the length of the trajectory;
s3-2, judging the minimum distance d between the path and all the obstaclesminAnd a preset safety distance DsafeWhen d is a relationship ofminIs less than DsafePunishing is carried out, and a punishment function is a reduction function of the minimum distance; when d isminGreater than or equal to DsafeTime, security penalty function and dmin＝DsafeThe security penalty function is:
Dsafeconstant d reflecting the influence of obstaclesminIs the minimum distance between the path of travel and the dynamic obstacle, dminThe calculation formula of (2) is as follows:
wherein, x (t), y (t) are determined by Ferguson spline expression formula, and O is the set of all obstacles in the space of the mobile robot;
s3-3, defining a fitness function of the improved particle swarm optimization:
f＝f1+af2
wherein α is a weight coefficient.
As a further scheme of the invention, in S3-1, the calculation formula of l is as follows: l ═ i
As a further aspect of the present invention, the value of a is 1.
In conclusion, compared with the prior art, the invention has the following beneficial effects:
the invention plans the global static path and the local dynamic path of the robot by improving the genetic algorithm and the particle swarm algorithm, so that the global static path obtained by the robot is short, the local dynamic path is quickly planned, the local obstacle avoidance capability of the mobile robot is improved, and the working efficiency of the mobile robot is improved.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, rather than all embodiments, and all other embodiments obtained by a person of ordinary skill in the art without any creative work based on the embodiments of the present invention belong to the protection scope of the present invention.
A mobile robot track tracking control method comprises environment modeling, global path planning and local dynamic obstacle avoidance path planning, and specifically comprises the following steps: after modeling the working environment of the mobile robot, planning a global static path which can avoid collision with all static obstacles in the working environment, and then, in the process that the mobile robot travels along the planned global path, carrying out local path planning aiming at the dynamic obstacles to carry out real-time obstacle avoidance on the dynamic obstacles;
the environment modeling method is the prior art and is not described in detail herein;
the global static path planning adopts an improved genetic algorithm, and comprises the following steps:
step 1), generating a starting point Start and a Goal of a global path according to a mathematical model established in a working environment of the robot, and initializing relevant parameters;
step 2), generating an initialization population by using an information entropy method, wherein the size of the initialization population is M and the iteration times T;
step 3) according to the formula
Calculating each individual adaptive value, wherein W is the number of intersections of the global path i and the obstacles in the environment;
step 4) according to the formula
Selecting an individual i, wherein M is the size of the population, and the calculation formula of f (i) is as follows:
Fmaxfor the current maximum adaptation value, FminThe current minimum adaptive value, a is a constant larger than zero, T is the current iteration frequency, and T is the maximum iteration frequency;
selecting individuals with higher fitness in the current population according to a game board method; according to the formula
Performing crossover, mutation operations, wherein fmaxFor the maximum fitness function value of the current population, favgFor the current population mean adaptation function value, f′The fitness function value of the individual with the greater fitness function among the hybrid individuals, wherein f is the fitness function value of the individual with the variation, pc1、pc2、pm1And pm2Is a normal number;
step 5), carrying out chaotic disturbance on individuals with the fitness of 10% in the population, wherein the chaotic disturbance is carried out in a chaotic evolution mode along the direction of the vertical line segment where each individual is located, and the evolution method comprises the following steps:
let a discrete point on the vertical line segment Li
The corresponding value range is [ a ]i，bi]Then the discrete point evolves to:
wherein the content of the first and second substances,
the following formula is obtained:
step 6), screening the discrete points after the chaotic disturbance according to a formula
Inverse mapping to obtain gene sequence, and then obtaining the gene sequence according to the formula
Calculating a new fitness function F'iIf the discrete point is still a free point, and F'i≥FiThen use Pi ′In place of PiOtherwise, the original path P is reservedi：
Step 7), if the maximum number of chaotic iterations is not reached, turning to the step 4), otherwise, turning to the step 8);
step 8), automatically adding 1 to the iteration times T, judging whether the T reaches the specified iteration times T, if not, turning to the step 3), and if not, continuing;
and 9) outputting the optimal individuals in the current population.
The local dynamic path planning adopts an improved particle swarm algorithm, and comprises the following steps:
step 1), defining a shortest penalty function:
wherein lminThe Euclidean distance from the current point to the target point, l is the length of the trajectory, and l is calculated by the following formula:
step 2), judging the minimum distance d between the path and all the obstaclesminAnd a preset safety distance DsafeWhen d is a relationship ofminIs less than DsafePunishment is carried out, and the punishment function is a reduction function of the minimum distance; when d isminGreater than or equal to DsafeConsidering the shortest path, the penalty function of safety and dmin＝DsafeThe values of (a) are continuous to ensure the global minimum uniqueness of the finally constructed fitness function, and a security penalty function is defined as follows:
Dsafeto reflect the influence of the obstacle, for example, if the radius of the obstacle is about 60, D is takensafe>60, in order to avoid the situation that the divisor is 0, the numerator and denominator of the numerator are simultaneously added with 1 and dminThe minimum distance between the travel path and the dynamic obstacle is calculated by the following formula:
wherein, x (t), y (t) are determined by Ferguson spline expression formula, and O is the set of all obstacles in the space of the mobile robot;
step 3), defining a fitness function of the improved particle swarm algorithm:
f＝f1+af2
wherein α is a weight coefficient for adjusting the weight occupied by the shortest route and the security in the route planning, in this embodiment, α is 1.
In summary, the working principle of the invention is as follows:
the crossing rate and the variation rate of the genetic algorithm are difficult to select, the convergence rate of the genetic algorithm is low due to the small crossing rate and the small variation rate, and the convergence rate is low or even incapable of converging due to the fact that the excellent individuals at the later stage are damaged due to the large crossing rate and the large variation rate;
in the conventional particle swarm algorithm, assuming that the velocity Vi (t) of the particle is (Vi1, Vi2, …, ViD), if any component Vid is smaller than-Vmax or larger than Vmax, it is set to-Vmax or Vmax, respectively. The traditional particle swarm algorithm limits the random optimizing capability of high-speed active particles, so that the initial decline of an evolutionary curve of the particle swarm algorithm is slow, and a large number of particles exceed an optimizing boundary due to overlarge speed. The invention introduces a method for randomly assigning values to the 'active' particle speed to overcome the defects of the traditional particle swarm algorithm: when the absolute value of the particle velocity is larger than the set threshold value of the velocity component, the value is randomly assigned between [ -Vmax, Vmax ], the number of the large velocity particles is large in the initial stage of particle swarm evolution, the velocity of the large velocity particles is randomly and evenly distributed, and the initial random optimizing capability of the large velocity particles can be exerted to the maximum extent. And when the later stage of the group evolution is reached, the particle swarm algorithm enters a convergence stage, and the number of the high-speed particles is sharply reduced. The random assignment method can enable part of particles to obtain smaller speed through random assignment so as to escape from local excellence, and meanwhile enables part of particles to obtain larger speed through random assignment so as to escape from local excellence. Therefore, the improved particle swarm optimization adopted by the invention has excellent performance in the aspects of convergence speed and global convergence capability.
Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that changes, modifications, substitutions and alterations can be made in these embodiments without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.
Claims (7)
1. A mobile robot track tracking control method is characterized by comprising the following steps:
s1, environment modeling, namely establishing a mathematical model according to the working environment of the mobile robot;
s2, planning a global static path according to the mathematical model, wherein the global static path planning adopts an improved genetic algorithm introducing an adaptive mechanism;
and S3, planning a local dynamic path for obstacle avoidance according to the mathematical model, wherein the local dynamic path planning adopts an improved particle swarm algorithm for randomly assigning the speed of the active particles.
2. The mobile robot trajectory tracking control method according to claim 1, wherein in S2, the improved genetic algorithm comprises the steps of:
s2-1, generating a Start point Start and a Goal of a global path according to a mathematical model established by the working environment of the robot, and initializing relevant parameters;
s2-2, generating an initialization population by using an information entropy method, wherein the size of the initialization population is M, and the iteration number is T;
s2-3, formula
Calculating each individual adaptive value, wherein W is the number of intersections of the global path i and the obstacles in the environment;
s2-4, formula
Selecting an individual i, wherein M is the size of the population; selecting individuals with higher fitness in the current population according to a game board method; according to the formula
Performing crossover, mutation operations, wherein fmaxFor the maximum fitness function value of the current population, favgIs the average adaptive function value of the current population, f' is the adaptive function value of the individual with larger adaptive function in the hybrid individuals, wherein f is the adaptive function value of the variant individual, pc1、pc2、pm1And pm2Is a normal number;
s2-5, performing chaotic disturbance on individuals with the fitness of 10% in the population, wherein the chaotic disturbance is subjected to chaotic evolution along the direction of the vertical line segment where each individual is located;
s2-6, screening the discrete points after the chaotic disturbance according to a formula
Inverse mapping is carried out to obtain a gene sequence, and then according to a formula,
calculating a new fitness function F'iJudging whether the discrete point is still a free point or not, if the discrete point is still a free point, and F'i≥FiFrom P'iIn place of PiOtherwise, the original path P is reservedi
S2-7, judging whether the chaotic iteration reaches the maximum times, if not, turning to S2-4, otherwise, carrying out S2-8;
s2-8, automatically adding 1 to the iteration time T, judging whether the T reaches the specified iteration time T, if not, turning to S2-3, otherwise, carrying out S2-9;
and S2-9, outputting the optimal individuals in the current population.
3. The method for controlling tracking of a mobile robot path according to claim 2, wherein in S2-4, the formula for f (i) is:
wherein, FmaxFor the current maximum adaptation value, FminAnd the current minimum adaptive value a is a constant larger than zero, T is the current iteration time, and T is the maximum iteration time.
4. The mobile robot trajectory tracking control method according to claim 3, wherein in S2-5, the evolution method is as follows:
let a discrete point on the vertical line segment Li
The corresponding value range is [ a ]i，bi]Then the discrete point evolves to:
the calculation formula is as follows:
5. the mobile robot trajectory tracking control method according to any one of claims 1 to 4, wherein in S3, the improved particle swarm algorithm comprises the following steps:
s3-1, defining a shortest penalty function:
wherein lminThe Euclidean distance from the current point to the target point, and l is the length of the trajectory;
s3-2, judging the minimum distance d between the path and all the obstaclesminAnd a preset safety distance DsafeWhen d is a relationship ofminIs less than DsafePunishing is carried out, and a punishment function is a reduction function of the minimum distance; when d isminGreater than or equal to DsafeTime, security penalty function and dmin＝DsafeThe security penalty function is:
Dsafeconstant d reflecting the influence of obstaclesminIs the minimum distance between the path of travel and the dynamic obstacle, dminThe calculation formula of (2) is as follows:
wherein, x (t), y (t) are determined by Ferguson spline expression formula, and O is the set of all obstacles in the space of the mobile robot;
s3-3, defining a fitness function of the improved particle swarm optimization:
f＝f1+af2
wherein α is a weight coefficient.
6. According to claimThe method for controlling the trajectory tracking of the mobile robot is characterized in that in S3-1, the calculation formula of l is as follows:
7. the mobile robot trajectory tracking control method according to claim 6, wherein in S3-3, a takes a value of 1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010248723.9A CN111290408A (en) | 2020-04-01 | 2020-04-01 | Mobile robot trajectory tracking control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010248723.9A CN111290408A (en) | 2020-04-01 | 2020-04-01 | Mobile robot trajectory tracking control method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111290408A true CN111290408A (en) | 2020-06-16 |
Family
ID=71031201
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010248723.9A Withdrawn CN111290408A (en) | 2020-04-01 | 2020-04-01 | Mobile robot trajectory tracking control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111290408A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112254727A (en) * | 2020-09-23 | 2021-01-22 | 锐捷网络股份有限公司 | TEB-based path planning method and device |
| CN114397896A (en) * | 2022-01-10 | 2022-04-26 | 贵州大学 | Dynamic path planning method for improving particle swarm optimization |
-
2020
- 2020-04-01 CN CN202010248723.9A patent/CN111290408A/en not_active Withdrawn
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112254727A (en) * | 2020-09-23 | 2021-01-22 | 锐捷网络股份有限公司 | TEB-based path planning method and device |
| CN112254727B (en) * | 2020-09-23 | 2022-10-14 | 锐捷网络股份有限公司 | TEB-based path planning method and device |
| CN114397896A (en) * | 2022-01-10 | 2022-04-26 | 贵州大学 | Dynamic path planning method for improving particle swarm optimization |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112631294B (en) | Intelligent path planning method for mobile robot | |
| Dou et al. | Genetic scheduling and reinforcement learning in multirobot systems for intelligent warehouses | |
| CN113867368B (en) | Robot path planning method based on improved gull algorithm | |
| CN101441736B (en) | Path planning method of motor crane robot | |
| CN111290408A (en) | Mobile robot trajectory tracking control method | |
| CN110053052B (en) | Task allocation and path planning method for multilayer SOM heterogeneous welding robot | |
| Al Dabooni et al. | Heuristic dynamic programming for mobile robot path planning based on Dyna approach | |
| Li et al. | Path planning of mobile robot based on genetic algorithm and gene rearrangement | |
| CN109540163A (en) | A kind of obstacle-avoiding route planning algorithm combined based on differential evolution and fuzzy control | |
| Hajebi et al. | Online adaptive fuzzy logic controller using genetic algorithm and neural network for networked control systems | |
| Chen et al. | Path planning of mobile robot based on an improved genetic algorithm | |
| Oleiwi et al. | Multi objective optimization of trajectory planning of non-holonomic mobile robot in dynamic environment using enhanced GA by fuzzy motion control and A | |
| CN112415997A (en) | Path planning method and system for multi-robot cooperation | |
| CN113741454A (en) | Multi-agent path planning method and system based on search | |
| Yang et al. | A knowledge based GA for path planning of multiple mobile robots in dynamic environments | |
| Yang et al. | Construction of novel self-adaptive dynamic window approach combined with fuzzy neural network in complex dynamic environments | |
| CN108227718B (en) | Self-adaptive switching automatic carrying trolley path planning method | |
| CN112327853B (en) | Robot flat sliding track planning method capable of guaranteeing distance from obstacle and based on hard constraint optimization problem | |
| Zhang et al. | A novel self-organizing emotional cmac network for robotic control | |
| Shill et al. | Genetic algorithm based fully automated and adaptive fuzzy logic controller | |
| Tubaileh | Layout of flexible manufacturing systems based on kinematic constraints of the autonomous material handling system | |
| CN115729238A (en) | Dynamic path planning method for autonomous obstacle avoidance of mobile robot | |
| CN116048071A (en) | Mobile robot path planning method based on particle swarm and differential evolution algorithm | |
| CN115202357A (en) | Autonomous mapping method based on impulse neural network | |
| Huang et al. | Multi-robot collision avoidance based on buffered voronoi diagram |
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 | ||
| WW01 | Invention patent application withdrawn after publication | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20200616 |