A mobile robot navigation method based on particle swarm optimization to control obstacle function

Abstract

The present invention relates to the field of robot navigation technology, and in particular to a mobile robot navigation method, device and computer-readable storage medium based on particle swarm optimization control obstacle function, comprising: after the mobile robot obtains two-dimensional map information through laser radar scanning, the actual speed and path of movement are calculated according to the control obstacle function, and the fitness value of the current path is judged by the particle swarm optimization algorithm, so as to update the expected factor and safety speed adjustment factor in the control obstacle function, and output the optimal path of the mobile robot after iteration. The present invention combines the security of the control obstacle function with the global search capability of particle swarm optimization, makes full use of global information to guide path planning, improves the optimization process of the overall path of the mobile robot, avoids falling into the local optimal solution during the path planning process, and improves the precision and accuracy of path planning.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a mobile robot navigation method, device and computer-readable storage medium based on particle swarm optimization control obstacle function. Background Art

[0002] In recent years, with the rapid development of science and technology, mobile robot navigation technology has been widely used in various fields, including logistics warehousing, medical care, agriculture and smart home. For example, in the logistics warehousing industry, the application of mobile robot navigation technology makes warehouse operations more efficient, accurate and automated, which plays a key role in improving the overall efficiency of the logistics industry. The robot needs to complete the path planning and navigation from the initial point to the target point within the specified time and according to certain task requirements. The movement of the robot is usually accompanied by the obstacle avoidance problem. Therefore, how to efficiently and safely enable the robot to avoid obstacles and reach the designated target point is the focus of navigation technology research.

[0003] Path planning is a key content in the field of mobile robot navigation, and its development provides an important foundation for the navigation of mobile robots. Traditional path planning mainly includes global path planning and local path planning. These algorithms can effectively find the optimal path from the starting point to the target point. At the same time, these algorithms include threshold detection based on distance sensors, path planning based on static maps, and simple obstacle avoidance behavior modes. However, in complex environments, such as warehouses full of obstacles, traditional path planning algorithms may not meet the requirements, so a more intelligent and flexible path planning method is needed.

[0004] Existing path planning methods in navigation include artificial potential field method, gradient descent method, etc. Among them, the artificial potential field method is a typical navigation and obstacle avoidance algorithm for mobile robots, which was first proposed by Khatib in 1985 and is widely used in robot navigation.

[0005] The artificial potential field method is inspired by the potential field theory in dynamics. It assumes that there is a potential field between the robot and the obstacle. The robot will be affected by the potential field and use the concept of potential field to plan the robot's path so that it can avoid obstacles and reach the target point. However, the artificial potential field method only considers the current state of the environment and cannot predict future changes. Therefore, in a complex obstacle environment, the robot may oscillate or fall into a local optimal solution.

[0006] The gradient descent method adjusts the parameter value based on the gradient information of the objective function so that the objective function value gradually approaches the minimum value. Its basic principle is to continuously update the parameter value along the negative gradient direction of the objective function in the parameter space until the minimum value is reached or the stopping condition is met. The gradient is a vector whose direction points to the direction in which the objective function value increases fastest, and its magnitude represents the rate of change in that direction. The gradient descent method determines the update direction and step size of the parameters by calculating the gradient of the objective function with respect to the parameters. However, the gradient descent method may face problems such as local optimal solution, initial point dependence, gradient vanishing, limited local gradient information and slow convergence when solving mobile robot navigation, resulting in poor path planning effect, poor stability and efficiency of the robot when avoiding obstacles. Summary of the invention

[0007] To this end, the technical problem to be solved by the present invention is to overcome the problem in the prior art that path planning is prone to fall into a local optimal solution and lacks security and efficiency.

[0008] In order to solve the above technical problems, the present invention provides a mobile robot navigation method based on particle swarm optimization to control obstacle function, comprising:

[0009] The mobile robot obtains two-dimensional map information through lidar scanning;

[0010] Set the initial point and target point of the mobile robot in the two-dimensional map, and obtain the coordinate information of the obstacle in the two-dimensional map;

[0011] Construct control barrier function;

[0012] The expected speed and safe speed are introduced into the obstacle control function to calculate the actual speed of the mobile robot at each position; the next position is calculated according to the current position and actual speed of the mobile robot until the path of the mobile robot from the initial point to the target point is obtained;

[0013] The expected factor and the safe speed adjustment factor are used to adjust and constrain the expected speed and the safe speed respectively. The particle swarm optimization algorithm is used to calculate the fitness value based on the path length, turning angle and smoothing speed. The target expected factor and the target safe speed adjustment factor are iteratively solved, and the path at this time is output as the target path.

[0014] Preferably, constructing a control barrier function includes:

[0015] The formula for controlling the barrier function h(x) is:

[0016]

[0017] Where N is the number of obstacles, is the coordinate information of the jth obstacle, x is the position of the mobile robot, R ob is the radius of the obstacle, D inflation is the range of the expansion layer of the obstacle; the control obstacle function satisfies the condition h(x)≥0;

[0018] The gradient function of the control barrier function h(x) is:

[0019]

[0020] For every position x of the mobile robot, there exists an input velocity v that satisfies Where α is an extended K ∞ Function,is strictly increasing;

[0021] Therefore, there is always an input speed v that satisfies the condition

[0022] Preferably, the introduction of the expected speed and the safe speed into the obstacle control function to calculate the actual speed of the mobile robot at each position includes:

[0023] Define the expected speed v d v d =λ d *(x g -x), where λ d is the expected factor, x g is the target point;

[0024] According to the expected speed v d , transform the control obstacle function into a quadratic programming problem and obtain the actual speed v * The expression is:

[0025]

[0026]

[0027] Where v is the input velocity, x is the position of the mobile robot, and N is the number of obstacles. is the coordinate information of the jth obstacle, α is an extended K ∞ Function, h(x) is the control obstacle function;

[0028] Define safe speed v s , the formula is:

[0029]

[0030] in, v d is the expected speed, L gis the Lie derivative form;

[0031] The actual speed v * =v d -v s .

[0032] Preferably, a segment distance trigger condition is introduced into the control obstacle function h(x), and the segment function φ is obtained as follows:

[0033]

[0034] Among them, min(h(x)) represents the minimum safe distance between the mobile robot and the obstacle, and δ0 is the safe distance threshold;

[0035] Introducing the piecewise function φ into the process of solving the actual speed, the actual speed is expressed as:

[0036]

[0037] Among them, v d is the expected speed, v s is the safe speed, λ r is the safety speed adjustment factor.

[0038] Preferably, the particle swarm optimization algorithm is used to calculate the fitness value based on the path length, turning angle and smoothing speed, and the target expectation factor and the target safety speed adjustment factor are iteratively solved, including:

[0039] Set the particle swarm population size, number of iterations and search space, randomly initialize the position and velocity of each particle in the particle swarm, and randomly select particles as the optimal solution of the initial population;

[0040] The path corresponding to each particle is calculated according to the control obstacle function, and the fitness value of each particle is calculated based on the path length, turning angle and smoothing speed; the fitness value of each particle is compared with the individual optimal solution. If the fitness value of the current particle is less than the individual optimal solution, the individual optimal solution is updated with the current particle, otherwise the individual optimal solution is not updated;

[0041] In the entire particle swarm, the individual optimal solution with the smallest fitness is selected as the population optimal solution;

[0042] Each particle updates its speed and position according to the individual optimal solution and the population optimal solution, and uses the control obstacle function to recalculate the path corresponding to the updated particle, and recalculate the fitness value of the updated particle, so as to iterate.

[0043] The iteration ends when the number of iterations is reached or the particle position change is less than the preset threshold; the optimal solution of the population at this time is output as the target expectation factor and the target safety speed adjustment factor.

[0044] Preferably, the fitness value of each particle is calculated based on the path length, turning angle and smoothing speed, and the formula is:

[0045]

[0046] Among them, FIT i is the fitness value of the ith particle, n is the number of nodes in the path, ΔL p is the path length change term, w1 is the inertia weight of the path length, Δθ p is the steering angle change term, w2 is the inertia weight of the steering angle, Δv p is the smoothing speed change term, and w3 is the inertia weight of the smoothing speed.

[0047] Preferably, the path length change term ΔL p The formula is:

[0048]

[0049] Where n is the number of path nodes, (x p ,y p ) is the coordinate of the pth path node, (x p+1 ,y p+1 ) are the coordinates of the p+1th path node;

[0050] Steering angle change term Δθ p The formula is:

[0051]

[0052] Among them, θ p is the steering angle of the pth path node, θ p+1 is the steering angle of the p+1th path node, ranging from [0,π];

[0053] Smooth velocity change term Δv p The formula is:

[0054]

[0055] Among them, v p is the velocity of the pth path node, v p+1 is the velocity of the p+1th path node.

[0056] Preferably, each particle updates its speed and position according to the individual optimal solution and the population optimal solution, and the formula is:

[0057]

[0058]

[0059] in, is the velocity of the ith particle in the dth dimension in the t+1th iteration, w is the weight factor, is the velocity of the ith particle in the dth dimension in the tth iteration, c1 is the individual learning factor, is the individual optimal solution of the ith particle in the dth dimension in the tth iteration, is the position of the ith particle in the dth dimension in the tth iteration, c2 is the group learning factor, is the optimal solution for the population, is the position of the ith particle in the dth dimension in the t+1th iteration, r1 and r2 are random numbers in the range [0,,1]; dimension d = 1,2,…D, D is the dimension of the search range.

[0060] The present invention also provides a mobile robot navigation device based on particle swarm optimization to control obstacle function, comprising:

[0061] A map acquisition module is used to acquire two-dimensional map information through laser radar scanning;

[0062] A map digitization module is used to set the initial point and target point of the mobile robot in the two-dimensional map and obtain the coordinate information of obstacles in the two-dimensional map;

[0063] Function building module, used to build control barrier functions;

[0064] The path planning module is used to introduce the expected speed and the safe speed into the obstacle control function to calculate the actual speed of the mobile robot at each position; the next position is calculated according to the current position and the actual speed of the mobile robot until the path of the mobile robot from the initial point to the target point is obtained;

[0065] The speed optimization module is used to adjust and constrain the expected speed and the safe speed using the expected factor and the safe speed adjustment factor respectively. The particle swarm optimization algorithm is used to calculate the fitness value based on the path length, turning angle and smoothing speed, and the target expected factor and the target safe speed adjustment factor are iteratively solved, and the path at this time is output as the target path.

[0066] Preferably, the present invention also provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the steps of the above-mentioned mobile robot navigation method based on particle swarm optimization control obstacle function are implemented.

[0067] The above technical solution of the present invention has the following beneficial effects compared with the prior art:

[0068] The mobile robot navigation method based on particle swarm optimization control obstacle function described in the present invention combines the safety of the control obstacle function with the global search capability of particle swarm optimization, fully utilizes global information to guide path planning, improves the optimization process of the overall path of the mobile robot, avoids falling into the local optimal solution during the path planning process, and improves the precision and accuracy of path planning. The present invention enables the mobile robot to select a more flexible and adaptable path, and effectively responds to navigation needs in complex environments. Most importantly, compared with the prior art, the present invention has lower complexity and cost, and provides a more economical and efficient solution for the practical application of mobile robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] In order to make the content of the present invention more clearly understood, the present invention is further described in detail below according to specific embodiments of the present invention in conjunction with the accompanying drawings, wherein:

[0070] Figure 1 It is a framework diagram of a mobile robot navigation method based on particle swarm optimization to control obstacle function of the present invention;

[0071] Figure 2 is a schematic diagram of the path planning obtained in Example 2; wherein Figure 2 (a) is the path diagram obtained by using the control obstacle function in Example 2; Figure 2 (b) is the path diagram obtained by using particle swarm optimization to control the obstacle function in Example 2;

[0072] Figure 3 is a schematic diagram of the path planning obtained in Example 3; wherein Figure 3 (a) is the path diagram obtained by using the control obstacle function in Example 3; Figure 3 (b) is the path diagram obtained by using particle swarm optimization to control the obstacle function in Example 3. DETAILED DESCRIPTION

[0073] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments so that those skilled in the art can better understand the present invention and implement it, but the embodiments are not intended to limit the present invention.

[0074] Embodiment 1

[0075] Reference Figure 1 As shown, the present invention provides a mobile robot navigation method based on particle swarm optimization to control obstacle function, and the specific steps are introduced below.

[0076] S1. The mobile robot obtains two-dimensional map information through lidar scanning.

[0077] S2. Set the initial point and target point of the mobile robot in the two-dimensional map, and obtain the coordinate information of the obstacle in the two-dimensional map.

[0078] S3. Construct a control barrier function, including:

[0079] The formula for controlling the barrier function h(x) is:

[0080]

[0081] Where N is the number of obstacles, is the coordinate information of the jth obstacle, x is the position of the mobile robot, R ob is the radius of the obstacle, D inflation is the range of the expansion layer of the obstacle; the control obstacle function satisfies the condition h(x)≥0;

[0082] The gradient function of the control barrier function h(x) is:

[0083]

[0084] For every position x of the mobile robot, there exists an input velocity v that satisfies Where α is an extended K ∞ Function,is strictly increasing;

[0085] Therefore, there always exists an input speed v that satisfies the condition:

[0086]

[0087] S4. Introduce the expected speed and safe speed into the obstacle control function to calculate the actual speed of the mobile robot at each position, including:

[0088] Define the expected speed v d v d =λ d *(x g -x), where λ d is the expected factor, x g is the target point;

[0089] According to the expected speed v d , transform the control obstacle function into a quadratic programming problem and obtain the actual speed v * The expression is:

[0090]

[0091]

[0092] Where v is the input velocity, x is the position of the mobile robot, and N is the number of obstacles. is the coordinate information of the jth obstacle, α is an extended K ∞ Function, h(x) is the control obstacle function;

[0093] Define safe speed v s , the formula is:

[0094]

[0095] in, v d is the expected speed, L g is the Lie derivative form;

[0096] The actual speed v * =v d -v s .

[0097] The next position is calculated based on the current position and actual speed of the mobile robot until the path of the mobile robot from the initial point to the target point is obtained.

[0098] Preferably, in view of the requirement of safe navigation of the robot, this embodiment introduces a safe dynamic speed adjustment mechanism into the control obstacle function to ensure that the mobile robot safely navigates to the target point. When calculating the actual speed of the mobile robot at each position, the segmented distance trigger condition is introduced into the control obstacle function h(x), and the segmented function φ is obtained as follows:

[0099]

[0100] Among them, min(h(x)) represents the minimum safe distance between the mobile robot and the obstacle, and δ0 is the safe distance threshold;

[0101] Introducing the piecewise function φ into the process of solving the actual speed, the actual speed is expressed as:

[0102]

[0103] Among them, λ r The safety speed adjustment factor determines the extent of the safety speed reduction and plays a role in speed regulation when the mobile robot needs it, allowing the mobile robot to adjust the speed more carefully to ensure higher safety.

[0104] S5. Considering the limitations of the control obstacle function, such as the lack of global path optimization capability, the present invention introduces a particle swarm optimization method to overcome this shortcoming to ensure the safety and flexibility of the mobile robot path planning. The optimization steps are:

[0105] The expected factor and the safe speed adjustment factor are used to adjust and constrain the expected speed and the safe speed respectively. The particle swarm optimization algorithm is used to calculate the fitness value based on the path length, turning angle and smoothing speed. The target expected factor and the target safe speed adjustment factor are iteratively solved, and the path at this time is output as the target path.

[0106] Specifically, the process of iteratively solving the target expectation factor and the target safety speed adjustment factor using the particle swarm optimization algorithm includes:

[0107] S501. Calculate the path corresponding to each particle according to the control obstacle function, and calculate the fitness value of each particle according to the path length, turning angle and smoothing speed. The formula is:

[0108]

[0109] Among them, FIT i is the fitness value of the ith particle, n is the number of nodes in the path, ΔL p is the path length change term, w1 is the inertia weight of the path length, Δθ p is the steering angle change term, w2 is the inertia weight of the steering angle, Δv p is the smoothing speed change term, w3 is the inertia weight of the smoothing speed;

[0110] Specifically, the path length change term ΔL p The formula is:

[0111]

[0112] Where n is the number of path nodes, (x p ,y p ) is the coordinate of the pth path node, (x p+1 ,y p+1 ) are the coordinates of the p+1th path node;

[0113] Steering angle change term Δθ p The formula is:

[0114]

[0115] Among them, θ p is the steering angle of the pth path node, θ p+1 is the steering angle of the p+1th path node, ranging from [0,π];

[0116] Smooth velocity change term Δv p The formula is:

[0117]

[0118] Among them, v p is the velocity of the pth path node, v p+1 is the speed of the p+1th path node;

[0119] Compare the fitness value of each particle with the individual optimal solution. If the fitness value of the current particle is less than the individual optimal solution, the individual optimal solution is updated with the current particle, otherwise the individual optimal solution is not updated.

[0120] S502, in the entire particle swarm, select the individual optimal solution with the smallest fitness as the population optimal solution;

[0121] S503, each particle updates its speed and position according to the individual optimal solution and the population optimal solution, the formula is:

[0122]

[0123]

[0124] in, is the velocity of the ith particle in the dth dimension in the t+1th iteration, w is the weight factor, is the velocity of the ith particle in the dth dimension in the tth iteration, c1 is the individual learning factor, is the individual optimal solution of the ith particle in the dth dimension in the tth iteration, is the position of the ith particle in the dth dimension in the tth iteration, c2 is the group learning factor, is the optimal solution for the population, is the position of the ith particle in the dth dimension in the t+1th iteration, r1 and r2 are random numbers in the range [0,,1]; dimension d = 1,2,…D, D is the dimension of the search range;

[0125] The control obstacle function is used to recalculate the path corresponding to the updated particle, and the fitness value of the updated particle is recalculated, and iteration is performed until the number of iterations is reached or the position change of the particle is less than the preset threshold, and the iteration ends; the optimal solution of the population at this time is output as the target expectation factor and the target safety speed adjustment factor.

[0126] In summary, the control obstacle function algorithm effectively enhances the safety of the mobile robot. The use of the control obstacle function to constrain the robot's path can effectively prevent the mobile robot from colliding and entering unsafe areas. The particle swarm algorithm is good at global search and can find solutions within the optimal solution search space. The present invention combines the safety of the control obstacle function with the global search capability of the particle swarm optimization, fully utilizes global information to guide path planning, improves the optimization process of the overall path of the mobile robot, avoids falling into the local optimal solution during the path planning process, and improves the precision and accuracy of path planning. The present invention enables the mobile robot to choose a more flexible and adaptable path, and effectively respond to navigation needs in complex environments. Most importantly, compared with the prior art, the present invention has lower complexity and cost, and provides a more economical and efficient solution for the practical application of mobile robots.

[0127] Embodiment 2

[0128] Based on the mobile robot navigation method based on particle swarm optimization to control obstacle function described in Example 1, this embodiment performs path planning for a mobile robot in a warehouse with obstacles, specifically including the following steps.

[0129] S1. The mobile robot obtains two-dimensional map information through lidar scanning.

[0130] S2. Set the initial point and target point of the mobile robot in the two-dimensional map, and obtain the coordinate information of the obstacle in the two-dimensional map.

[0131] The obstacle coordinate information of this embodiment is shown in Table 1.

[0132] Table 1. Obstacle coordinate information of a warehouse map

[0133] Serial number Obstacle coordinates Serial number Obstacle coordinates 1 [2.5,4] 6 [5.5,4] 2 [2.5,4.5] 7 [5.5,6] 3 [2.5,5] 8 [6,3.5] 4 [2.5,5.5] 9 [6,4] 5 [5,4] 10 [6,5.5]

[0134] S3. Construct a control barrier function.

[0135] S4. Introduce the expected speed and safe speed into the obstacle control function to calculate the actual speed of the mobile robot at each position; calculate the next position according to the current position and actual speed of the mobile robot until the path of the mobile robot from the initial point to the target point is obtained.

[0136] The values ​​of the parameters in the control barrier function are shown in Table 2.

[0137] Table 2. Control barrier function parameter setting table

[0138]

[0139] S5. Use the expected factor and the safe speed adjustment factor to adjust and constrain the expected speed and the safe speed respectively. Use the particle swarm optimization algorithm to calculate the fitness value based on the path length, turning angle and smoothing speed. Iteratively solve the target expected factor and the target safe speed adjustment factor, and output the path at this time as the target path.

[0140] Among them, the values ​​of various parameters in the particle swarm optimization algorithm are shown in Table 3.

[0141] Table 3. Particle swarm optimization algorithm parameter setting table

[0142]

[0143] The path diagram obtained in this embodiment refers to Figure 2 As shown, Figure 2 (a) is the path diagram obtained by using the control obstacle function in this embodiment; Figure 2 (b) is the path diagram obtained by using particle swarm optimization to control the barrier function in this embodiment. It can be seen that the path obtained after optimizing the barrier function using the particle swarm optimization algorithm is smoother and more efficient.

[0144] Embodiment 3

[0145] Based on the mobile robot navigation method based on particle swarm optimization control obstacle function described in Example 1 and the parameter settings of the obstacle control function and particle swarm optimization algorithm in Example 2, this implementation changes the coordinate position of the obstacle in the warehouse and plans the path of the mobile robot again. The changed obstacle coordinates are shown in Table 4.

[0146] Table 4. Obstacle coordinate information of a warehouse map

[0147] Serial number Obstacle coordinates Serial number Obstacle coordinates 1 [1,2] 11 [6,5.5] 2 [1.5,2] 12 [5,4] 3 [2,2] 13 [5.5,4] 4 [2.5,2] 14 [6,4] 5 [3,2] 15 [6,3.5] 6 [2.5,4] 16 [6,3] 7 [2.5,4.5] 17 [6,2.5] 8 [2.5,5] 18 [5.5,2.5] 9 [2.5,5.5] 19 [5,2.5] 10 [5.5,6]

[0148] The path diagram obtained in this embodiment refers to Figure 3 As shown, Figure 3 (a) is the path diagram obtained by using the control obstacle function in this embodiment; Figure 3 (b) is a path diagram obtained by controlling the obstacle function using particle swarm optimization in this embodiment. It can be seen that no matter what kind of space has obstacles, the present invention can obtain a smooth and efficient path.

[0149] Embodiment 4

[0150] Based on the above-mentioned mobile robot navigation method based on particle swarm optimization to control obstacle function, this embodiment further provides a mobile robot navigation device based on particle swarm optimization to control obstacle function, including:

[0151] A map acquisition module is used to acquire two-dimensional map information through laser radar scanning;

[0152] A map digitization module is used to set the initial point and target point of the mobile robot in the two-dimensional map and obtain the coordinate information of obstacles in the two-dimensional map;

[0153] Function building module, used to build control barrier functions;

[0154] The path planning module is used to introduce the expected speed and the safe speed into the obstacle control function to calculate the actual speed of the mobile robot at each position; the next position is calculated according to the current position and the actual speed of the mobile robot until the path of the mobile robot from the initial point to the target point is obtained;

[0155] The speed optimization module is used to adjust and constrain the expected speed and the safe speed using the expected factor and the safe speed adjustment factor respectively. The particle swarm optimization algorithm is used to calculate the fitness value based on the path length, turning angle and smoothing speed, and the target expected factor and the target safe speed adjustment factor are iteratively solved, and the path at this time is output as the target path.

[0156] A mobile robot navigation device based on particle swarm optimization control obstacle function in this embodiment is used to implement the aforementioned mobile robot navigation method based on particle swarm optimization control obstacle function. Therefore, the specific implementation method of a mobile robot navigation device based on particle swarm optimization control obstacle function can be seen in the embodiment part of a mobile robot navigation method based on particle swarm optimization control obstacle function in the previous text, wherein the map acquisition module, the map digitization module, the function construction module, the path planning module, and the speed optimization module are respectively used to implement steps S1, S2, S3, S4 and S5 in the aforementioned mobile robot navigation method based on particle swarm optimization control obstacle function. Therefore, its specific implementation method can refer to the description of the corresponding each part of the embodiment, which will not be repeated here.

[0157] This embodiment also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned mobile robot navigation method based on particle swarm optimization to control obstacle function are implemented.

[0158] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0159] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0160] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0161] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0162] Obviously, the above embodiments are merely examples for the purpose of clear explanation and are not intended to limit the implementation methods. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the implementation methods here. The obvious changes or modifications derived therefrom are still within the scope of protection of the present invention.

Claims

1. A mobile robot navigation method based on particle swarm optimization to control obstacle function, characterized in that: include: The mobile robot obtains two-dimensional map information through lidar scanning; Set the initial point and target point of the mobile robot in the two-dimensional map, and obtain the coordinate information of the obstacle in the two-dimensional map; Construct control barrier function; The expected speed and safe speed are introduced into the control obstacle function to calculate the actual speed of the mobile robot at each position, including: Introduce segment distance trigger conditions into the control obstacle function to obtain the segment function for: ； in, is the position of the mobile robot, is the control barrier function, Indicates the minimum safe distance between the mobile robot and obstacles. is the safety distance threshold; The piecewise function Introducing the solution process of the actual speed, the actual speed is expressed as: ； in, is the actual speed, is the expected speed, For safe speed, is the safety speed adjustment factor, is the gradient function of the control obstacle function; The next position is calculated based on the current position and actual speed of the mobile robot until the path of the mobile robot from the initial point to the target point is obtained; The expected factor and the safe speed adjustment factor are used to adjust and constrain the expected speed and the safe speed respectively. The particle swarm optimization algorithm is used to calculate the fitness value based on the path length, turning angle and smoothing speed. The target expected factor and the target safe speed adjustment factor are iteratively solved, and the path at this time is output as the target path.

2. A mobile robot navigation method based on particle swarm optimization to control obstacle function according to claim 1, characterized in that: Construct a control barrier function, including: Control barrier function The formula is: ； in, is the number of obstacles, is the coordinate information of the jth obstacle, is the position of the mobile robot, is the radius of the obstacle, is the range of the expansion layer of the obstacle; the control barrier function meets the conditions ; Control barrier function The gradient function is: ； For each position of the mobile robot , there is an input speed satisfy ,in For an extension Function,is strictly increasing; Therefore, there is always an input speed Satisfy the conditions .

3. A mobile robot navigation method based on particle swarm optimization to control obstacle function according to claim 2, characterized in that: The desired speed and the safe speed are introduced into the obstacle control function to calculate the actual speed of the mobile robot at each position, including: Defining the desired speed for ,in is the expectation factor, is the target point; According to the expected speed , transform the control obstacle function into a quadratic programming problem and obtain the actual speed The expression is: ； ； in, is the input speed, is the position of the mobile robot, is the number of obstacles, is the coordinate information of the jth obstacle, For an extension function, is the control barrier function; Defining safe speed , the formula is: ； in, , is the expected speed, is the Lie derivative form; The actual speed .

4. The mobile robot navigation method based on particle swarm optimization control obstacle function according to claim 1, characterized in that: The particle swarm optimization algorithm is used to calculate the fitness value based on the path length, turning angle and smoothing speed, and iteratively solve the target expectation factor and the target safety speed adjustment factor, including: Set the particle swarm population size, number of iterations and search space, randomly initialize the position and velocity of each particle in the particle swarm, and randomly select particles as the optimal solution of the initial population; The path corresponding to each particle is calculated according to the control obstacle function, and the fitness value of each particle is calculated based on the path length, turning angle and smoothing speed; the fitness value of each particle is compared with the individual optimal solution. If the fitness value of the current particle is less than the individual optimal solution, the individual optimal solution is updated with the current particle, otherwise the individual optimal solution is not updated; In the entire particle swarm, the individual optimal solution with the smallest fitness is selected as the population optimal solution; Each particle updates its speed and position according to the individual optimal solution and the population optimal solution, and uses the control obstacle function to recalculate the path corresponding to the updated particle, and recalculate the fitness value of the updated particle, so as to iterate. The iteration ends when the number of iterations is reached or the particle position change is less than the preset threshold; the optimal solution of the population at this time is output as the target expectation factor and the target safety speed adjustment factor.

5. The mobile robot navigation method based on particle swarm optimization control obstacle function according to claim 1, characterized in that: The fitness value of each particle is calculated based on the path length, turning angle and smoothing speed. The formula is: ； in, is the fitness value of the ith particle, is the number of nodes in the path, is the path length change term, is the inertia weight of the path length, is the steering angle change term, is the inertia weight of the steering angle, is the smoothing speed variation term, is the inertia weight for smoothing velocity.

6. The mobile robot navigation method based on particle swarm optimization control obstacle function according to claim 1, characterized in that: Path length change The formula is: ； in, is the number of path nodes, is the coordinate of the pth path node, is the coordinate of the p+1th path node; Steering angle change The formula is: ； in, is the steering angle of the pth path node, is the steering angle of the p+1th path node, and its value range is ; Smooth speed change The formula is: ； in, is the velocity of the pth path node, is the velocity of the p+1th path node.

7. The mobile robot navigation method based on particle swarm optimization control obstacle function according to claim 1, characterized in that: Each particle updates its speed and position according to the individual optimal solution and the population optimal solution, and the formula is: ； ； in, is the velocity of the ith particle in the dth dimension in the t+1th iteration, is the weight factor, is the velocity of the ith particle in the dth dimension in the tth iteration, is the individual learning factor, is the individual optimal solution of the ith particle in the dth dimension in the tth iteration, is the position of the ith particle in the dth dimension in the tth iteration, is the group learning factor, is the optimal solution for the population, is the position of the ith particle in the dth dimension in the t+1th iteration, and For the range Random numbers within; dimensions , The dimension of the search range.

8. A mobile robot navigation device based on particle swarm optimization to control obstacle function, characterized in that: include: A map acquisition module is used to acquire two-dimensional map information through laser radar scanning; A map digitization module is used to set the initial point and target point of the mobile robot in the two-dimensional map and obtain the coordinate information of obstacles in the two-dimensional map; Function building module, used to build control barrier functions; The path planning module is used to introduce the expected speed and safe speed into the obstacle control function to calculate the actual speed of the mobile robot at each position, including: Introduce segment distance trigger conditions into the control obstacle function to obtain the segment function for: ； in, is the position of the mobile robot, is the control barrier function, Indicates the minimum safe distance between the mobile robot and obstacles. is the safety distance threshold; The piecewise function Introducing the solution process of the actual speed, the actual speed is expressed as: ； in, is the actual speed, is the expected speed, For safe speed, is the safety speed adjustment factor, is the gradient function of the control obstacle function; The next position is calculated based on the current position and actual speed of the mobile robot until the path of the mobile robot from the initial point to the target point is obtained; The speed optimization module is used to adjust and constrain the expected speed and the safe speed using the expected factor and the safe speed adjustment factor respectively. The particle swarm optimization algorithm is used to calculate the fitness value based on the path length, turning angle and smoothing speed, and the target expected factor and the target safe speed adjustment factor are iteratively solved, and the path at this time is output as the target path.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by the processor, the steps of a mobile robot navigation method based on particle swarm optimization control obstacle function as described in any one of claims 1 to 7 are implemented.

Images