A amphibious robot path planning control method

Abstract

The application discloses an amphibious robot path planning control method, characterized in that comprising the following steps: step 1: according to the environment terrain elevation model already introduced in the amphibious robot system, the global optimal path from the starting point to the target point is calculated by using the HHO algorithm; step 2: according to the real-time geographical position of the amphibious robot, the navigation is started according to the global optimal path; step 3: when the amphibious robot encounters an obstacle, it is judged whether the obstacle is a known obstacle, and the amphibious robot is controlled to avoid the obstacle according to the specific situation of the obstacle. In the process of navigation of the robot, two different obstacle avoidance strategies are carried out according to whether the obstacle is a known obstacle, the real-time position of the robot can be dynamically positioned and corrected, and dynamic obstacle avoidance can be safely and quickly carried out, so that the robot is more accurate in the process of navigation on the preset route.

Description

Technical Field

[0001] This invention relates to the field of amphibious robot control technology, specifically to a path planning and control method for amphibious robots. Background Technology

[0002] Path planning is a key technology for autonomous underwater vehicles (AUVs). It refers to the process of pre-calculating, setting, and optimizing the navigation direction and route of a planned device to reach a specific target or complete a task. Path planning technology, to a certain extent, indicates the level of intelligence of a robot. With the rapid development of computer control technology, artificial intelligence technology, and advanced control theory, scholars both domestically and internationally have applied various intelligent algorithms to robot path planning, such as the artificial potential field method, rapid stepping method, A* algorithm, particle swarm optimization algorithm, ant colony optimization algorithm, genetic algorithm, and EDA algorithm. However, traditional path planning algorithms have various drawbacks in practical applications. For example, the artificial potential field method is prone to local minima, and the ant colony algorithm is not suitable for real-time search. HHO, proposed by Heidari et al. in 2019, is a novel swarm intelligence algorithm derived from the cooperative behavior of the Harris Hawk (also known as the Chestnut-winged Hawk) during hunting. It features simple principles, few control parameters, and excellent global search capabilities, and has been widely used and optimized in fields such as image segmentation, machine learning, transportation, photovoltaic system modeling, and neural networks. For example, Guo Yuxin et al. proposed a multi-strategy optimization HHO, which improves the algorithm's global search performance and search efficiency by introducing adaptive weight factors and Cauchy distribution functions. Liu Xiaolong et al. proposed an HHO based on square neighborhoods and random arrays. By setting a square neighborhood topology with multiple subgroups, they guide individuals within each subgroup to randomly forage in both directions. The random arrays enhance information exchange between subgroups, effectively improving the convergence speed of the HHO.

[0003] This 2022 master's thesis from Hangzhou Dianzi University proposes an improved Harris Eagle Optimization Algorithm (IHHO) for path optimization. Primarily targeting the path planning problem of mobile robots, it proposes an IHHO algorithm. Based on the traditional HHO algorithm, this method addresses the issues of low population diversity and low optimization performance by incorporating the optimal point set method and a nonlinear energy factor update strategy balancing algorithm, thus improving the algorithm's global optimization performance. While HHO is primarily used for terrestrial mobile robots, it cannot meet the path planning requirements of amphibious robots operating in complex and variable environments. Furthermore, the existing IHHO algorithm still suffers from weak local optimization capabilities and is prone to premature convergence.

[0004] Furthermore, while the robot is navigating along the globally optimal path, it may encounter obstacles. These obstacles fall into two categories: firstly, due to the accumulated errors in GPS inertial navigation and Doppler logs during path tracking, the robot may deviate from its course and encounter known obstacles at terrain edges; secondly, there are unknown obstacles not recorded in the environmental terrain elevation model. If these two types of obstacles are not distinguished, and the robot blindly avoids them, its navigation path will deviate significantly from the preset route. Therefore, it is necessary to identify the type of obstacle during navigation and implement corresponding obstacle avoidance strategies. Summary of the Invention

[0005] This invention provides a path planning and control method for amphibious robots to solve the problems of limited application scenarios, weak local mining capabilities, and excessive deviation between the robot's navigation route and the preset route in existing path planning methods.

[0006] This invention provides a path planning and control method for an amphibious robot, comprising the following steps:

[0007] Step 1: Based on the environmental terrain elevation model already imported into the amphibious robot system, use the HHO algorithm to calculate the globally optimal path from the starting point to the target point;

[0008] Step 2: Based on the amphibious robot's real-time geographical location, begin navigation along the globally optimal path;

[0009] Step 3: When the amphibious robot encounters an obstacle, it determines whether the obstacle is a known obstacle.

[0010] When there are known obstacles, correct the navigation data of each amphibious robot until the amphibious robot returns to the globally optimal path;

[0011] When the obstacle is unknown, a dynamic path planning process is performed within the area.

[0012] The local dynamic path planning process includes:

[0013] When the distance between the amphibious robot and the obstacle is greater than the safe distance, the amphibious robot is controlled to avoid obstacles by differential speed turning obstacle avoidance method.

[0014] When the distance between the amphibious robot and an obstacle is less than or equal to the safe distance, the amphibious robot can avoid the obstacle by turning in place or moving backward.

[0015] After the amphibious robot avoids unknown obstacles, the HHO algorithm is used to calculate the local optimal path between the robot's current position and the nearest point on the global optimal path. Based on the local optimal path, the amphibious robot is controlled to return to the global optimal path.

[0016] Furthermore, steps 1 and 3 also include: optimizing the HHO algorithm using the SGD algorithm, and then using the optimized HHO algorithm to calculate the global optimal path and the local optimal path;

[0017] Specifically, the optimization of the HHO algorithm using the SGD algorithm is as follows:

[0018] Use the SGD algorithm to generate the step size for the next iteration cycle in the HHO algorithm.

[0019] Furthermore, the fitness function of the HHO algorithm is:

[0020] F fitness =L price +S smooth +O security

[0021] Among them, L price =Q1(x1,y1,y1),Q2(x2,y2,y2),...,Q i (x i ,y i ,y i ),Q i (x i ,y i ,y i Q is the grid cost obtained based on the speed cost and energy cost, and D is the set of accessible coordinate points in the environment.

[0022] Furthermore, the specific formula for obtaining the grid cost based on the speed cost and energy cost is as follows:

[0023]

[0024] Among them, V i For the price of speed; E i is the energy cost; i is the grid number; a is the velocity cost factor; b is the energy cost factor.

[0025] Furthermore, in the specific formula for obtaining the grid cost based on the speed cost and energy cost:

[0026] When only the shortest path time is required, b = 0, that is...

[0027] When only the minimum energy consumption is required, a = 0, that is...

[0028] When only the shortest path is required, a = b = 0, i.e., Q i =1.

[0029] Furthermore, in the specific formula for obtaining the grid cost based on the speed cost and energy cost, V i The speed at which the robot moves through a grid is preset; the faster the speed, the smaller the V value.

[0030] E i The energy consumption of the robot when it passes through a grid is preset, and the higher the energy consumption, the higher the E value.

[0031] The beneficial effects of this invention are:

[0032] This invention incorporates a grid cost function into two factors: movement speed and energy conversion efficiency, accurately and intuitively reflecting the cost of a path under different working environments. Based on the HHO algorithm, this invention improves upon the SGD algorithm by optimizing the population update steps of the HHO algorithm, reducing the number of iterations and avoiding the slow convergence caused by excessive iterations. This also significantly improves the HHO algorithm's performance in local mining, making the paths found by the algorithm more accurate and reasonable, and faster in amphibious robot path planning applications. During navigation, the robot employs two different obstacle avoidance strategies depending on whether an obstacle is known. This allows for dynamic positioning and correction of the robot's real-time position, as well as safe and rapid dynamic obstacle avoidance, resulting in more precise navigation along the preset route. Attached Figure Description

[0033] The features and advantages of the invention will be more clearly understood by referring to the accompanying drawings, which are schematic and should not be construed as limiting the invention in any way. In the drawings:

[0034] Figure 1 This is a schematic diagram of the controller composition in a specific embodiment of the present invention;

[0035] Figure 2 The flowchart of the SGD-HHO algorithm is shown in a specific embodiment of the present invention. Detailed Implementation

[0036] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0037] This invention provides a path planning and control method for an amphibious robot, such as... Figure 1 As shown, it includes:

[0038] Step 1: Based on the environmental terrain elevation model already imported into the amphibious robot system, use the SGD-HHO algorithm to calculate the globally optimal path from the starting point to the target point;

[0039] (1): Constructing the path and fitness function for the amphibious robot. The path is constructed using straight line segments. Each path contains a series of waypoints, and each pair of waypoints forms a path segment. These path segments then constitute the final path. The constructed fitness function is L. price Introducing raster cost Q i This improves the traditional path length optimization to a comprehensive optimization of path length, completion time, and power consumption, thereby enhancing the adaptability of path planning to complex environments.

[0040] L price =Q1(x1,y1,y1),Q2(x2,y2,y2),...,Q i (x i ,y i ,y i ),Q i (x i ,y i ,y i )∈D

[0041] L price S represents the path cost; a smaller value indicates a lower overall cost in terms of both speed and energy consumption. smooth This indicates the smoothness of the path; a smaller value indicates a smoother path, which is more conducive to the robot's navigation along the predetermined path. security This indicates path safety; a value of zero indicates a safe path.

[0042] L price =Q(1)(x1,y1,y1),Q(2)(x2,y2,y2),...,Q(i)(x i ,y i ,y i ),Q(i)(x i,y i ,y i )∈D

[0043] Q is the raster cost for each grid cell, and D is the set of accessible coordinates in the environment.

[0044]

[0045] V i As a speed-related cost, a smaller value indicates a faster movement speed, E i For energy cost, the smaller the value, the higher the energy conversion efficiency. i is the grid number, a is the speed cost factor, and b is the energy cost factor.

[0046]

[0047] N uneven This represents the number of path segments in a path whose curvature exceeds the amphibious robot's maximum turning radius. If a path's curvature exceeds the robot's maximum turning radius, that path is considered infeasible. N seg This represents the total number of path segments in a path.

[0048]

[0049] N obt N represents the number of waypoints that fall into the drivable zone. profile This indicates the number of three-dimensional facets.

[0050] (2): Using the SGD-HHO algorithm for global static path planning

[0051] After initializing the random swarm size and the number of iterations, multiple iterations are performed using the SGD-HHO algorithm. The Harris eagle's pursuit strategy is adjusted based on the prey's escape behavior, and the step size for the next iteration cycle is generated using the SGD algorithm. Finally, when the algorithm reaches the required number of iterations or a given precision, it stops and outputs the optimal path. The SGD-HHO path planning algorithm is constructed, including the following steps:

[0052] a) Initialize the population

[0053] b) Calculate the fitness of individual Harris Hawks

[0054] c) Update prey fitness and location

[0055] d) Update prey escape energy and jump energy

[0056] e) Encirclement and capture strategy

[0057] f) SGD method to update position

[0058] g) Algorithm termination condition

[0059] Step a specifically includes: initializing the total number of groups M and the maximum number of iterations T.

[0060] Step b specifically includes: calculating the fitness function value F. fitness Carefully examine and monitor the search space [lb,ub] to find prey, search for prey in random locations according to two strategies, and update the position with probability p during iteration.

[0061] Step c specifically includes: calculating the fitness function value and location of the prey.

[0062] Step d specifically includes: updating the prey's escape energy, which is significantly reduced during the prey's escape behavior.

[0063] Step e specifically includes: based on the prey's escape behavior and the Harris Eagle's pursuit strategy, four possible strategies are proposed in HHO to simulate the attack phase, where the prey always tries to escape the threatening situation. The algorithm then enters the development phase, capturing prey through four strategies (soft encirclement strategy, hard encirclement strategy, cumulative speed dive soft encirclement strategy, and cumulative speed dive hard encirclement strategy).

[0064] Step f specifically includes: using a random sample to determine the gradient direction for each population, and then iteratively solving the problem step by step using the gradient descent method to obtain the minimized loss function and model parameter values.

[0065] Step g specifically includes: algorithm stopping condition, whereby the algorithm stops and gives the result after reaching the number of iterations or the given precision.

[0066] Step 2: Based on the amphibious robot's real-time geographical location, begin navigation along the globally optimal path;

[0067] After the calculations were completed, the amphibious robot began to navigate along a preset route based on its real-time geographical location measured by GPS inertial navigation and a Doppler log.

[0068] Step 3: When the amphibious robot encounters an obstacle, it determines whether the obstacle is a known obstacle. The underwater lights are turned on, the underwater camera takes a picture of the obstacle, and compares it with the obstacle features in the known underwater environment elevation model. If the comparison is successful, it is a known obstacle; if the comparison fails, it is an unknown obstacle.

[0069] When there are known obstacles, correct the navigation data of each amphibious robot until the amphibious robot returns to the globally optimal path;

[0070] The data from GPS inertial navigation and Doppler log are corrected to determine the robot's actual position. If the robot deviates from the preset route, the motion control system is controlled to return the robot to the preset route and continue to navigate along the preset path.

[0071] When the obstacle is unknown, a dynamic path planning process is performed within the area.

[0072] The local dynamic path planning process includes:

[0073] When the distance between the amphibious robot and the obstacle is greater than the safe distance, the amphibious robot is controlled to avoid obstacles by differential speed turning obstacle avoidance method.

[0074] When the distance between the amphibious robot and an obstacle is less than or equal to the safe distance, the amphibious robot can avoid the obstacle by turning in place or moving backward.

[0075] After the amphibious robot avoids unknown obstacles, the SGD-HHO algorithm is used to calculate the local optimal path between the amphibious robot's current position and the nearest point on the global optimal path. Based on the local optimal path, the amphibious robot is controlled to return to the global optimal path.

[0076] Although embodiments of the invention have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of the invention, and such modifications and variations all fall within the scope defined by the appended claims.

Claims

1. A path planning and control method for an amphibious robot, characterized in that, The method comprises the following steps: Step 1: calculating a global optimal path from a starting point to a target point by using a HHO algorithm according to an imported terrain elevation model of an environment in an amphibious robot system; Step 2: starting to navigate according to the global optimal path according to a real-time geographical position of the amphibious robot; Step 3: when the amphibious robot encounters an obstacle, judging whether the obstacle is a known obstacle, when the obstacle is a known obstacle, correcting navigation data of the amphibious robot until the amphibious robot returns to the global optimal path; when the obstacle is an unknown obstacle, performing a local dynamic path planning process; The local dynamic path planning process comprises: when a distance between the amphibious robot and the obstacle is greater than a safety distance, controlling the amphibious robot to avoid the obstacle by using a differential cornering obstacle avoidance method; when the distance between the amphibious robot and the obstacle is less than or equal to the safety distance, controlling the amphibious robot to avoid the obstacle by using a spot turning or retreating method; when the amphibious robot avoids the unknown obstacle, calculating a local optimal path between a current position of the amphibious robot and a nearest point on the global optimal path by using the HHO algorithm, and controlling the amphibious robot to return to the global optimal path according to the local optimal path.

2. The amphibious robot path planning control method of claim 1, wherein, The step 1 and the step 3 further comprise: optimizing the HHO algorithm by using an SGD algorithm, and then calculating the global optimal path and the local optimal path by using the optimized HHO algorithm; The optimization of the HHO algorithm by using the SGD algorithm specifically comprises: generating a step length of a next iteration cycle in the HHO algorithm by using the SGD algorithm.

3. The amphibious robot path planning control method according to claim 1 or 2, characterized by, The fitness function of the HHO algorithm is: F fitness = L price + S smooth + O security ; wherein L price = Q1(x1, y1, y1), Q2(x2, y2, y2),..., Q i (x i , y i , y i ), Q i (x i , y i , y i ) e D, Q is a grid cost obtained according to a speed cost and an energy cost, and D is a set of environment passable coordinate points.

4. The amphibious robot path planning control method of claim 3, wherein, A specific formula of a grid cost obtained according to a speed cost and an energy cost is: ； where V i is the velocity cost; E i is the energy cost; i is the grid number; a is the velocity cost factor; and b is the energy cost factor.

5. The amphibious robot path planning control method of claim 4, wherein, In the specific formula of the grid cost obtained according to the speed cost and the energy cost: When only the shortest path time is required, b = 0, i.e. ; When only the minimum energy consumption is required, a = 0, i.e. ; when only a shortest path is required, a=b=0, that is, Q1=1.

6. The amphibious robot path planning control method according to claim 4 or 5, characterized by, In the specific formula of the grid cost obtained according to the speed cost and the energy cost, V i According to the moving speed of the robot when passing through a certain grid, the speed is faster and the V value is smaller. E i The value of E is set in advance according to the energy consumption of the robot when it passes through a certain grid. The greater the energy consumption, the greater the value of E.

Images