A Low-Energy Mobile Robot Path Planning Method

Abstract

The present invention discloses a low-energy mobile robot path planning method and recommendation system, including: using a grid method to construct a path planning environment map and determine the starting position and target position; using the ESA* algorithm to perform robot path planning in a static environment, and performing turning penalty, pruning, and smoothing on the planned path to obtain a global energy-saving path; extracting sub-target points of the global energy-saving path to generate a sub-target point sequence, and using the IDWA algorithm to perform local path planning between adjacent sub-target points until the target point; and optimizing the fusion algorithm using a global energy-saving path deviation evaluation and heading angle adaptive adjustment strategy. The present invention can be deployed and applied to mobile robot equipment with low configuration, can complete path planning for the mobile robot, and consider time, length, energy consumption, etc. to ensure that the robot can complete as many tasks as possible within limited energy; it can quickly plan a reasonable path, reduce workshop energy consumption, and save manpower expenses.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to a low-energy mobile robot path planning method. Background Art

[0002] During mobile robot operations, energy capacity fundamentally determines how long a robot can perform a given task. Currently, this is primarily achieved by increasing energy capacity and recharging times, but this improvement is limited and increases resource costs. When energy is limited and recharging is insufficient, mobile robots can reduce energy loss and improve their ability to operate in complex environments through energy-efficient and efficient path planning.

[0003] Common path planning methods include genetic algorithms, A* algorithms, fast random tree search methods, dynamic window approach (DWA), artificial potential field methods, etc., among which the A* algorithm is the most widely used; the A* algorithm is often used to solve the optimal path in a static environment. It selects a path with the optimal distance from the starting point to the end point in a static environment based on the cost function; based on the A* algorithm, researchers have successively proposed improved algorithms such as EA* and LPA*; how to extend the robot's operating time in complex environments and improve its dynamic obstacle avoidance capabilities when energy is limited or supply is insufficient has become an important research issue at present. Summary of the Invention

[0004] The purpose of this section is to summarize some aspects of the embodiments of the present invention and briefly introduce some preferred embodiments. Some simplifications or omissions may be made in this section and the abstract and title of this application to avoid obscuring the purpose of this section, the abstract and the title of the invention, and such simplifications or omissions should not be used to limit the scope of the present invention.

[0005] In view of the above existing problems, the present invention is proposed.

[0006] Therefore, the present invention provides a low-energy mobile robot path planning method, which can solve the problems of energy-saving path planning and dynamic obstacle avoidance of the robot in complex and narrow environments.

[0007] To solve the above technical problems, the present invention provides the following technical solution: a low-energy mobile robot path planning method, comprising:

[0008] Use the grid method to construct the path planning environment map and determine the starting position and target position;

[0009] The ESA* algorithm is used to plan robot paths in static environments. The planned paths are then penalized, pruned, and smoothed to obtain a global energy-saving path.

[0010] Extract the sub-target points of the global energy-saving path to generate a sub-target point sequence, and use the IDWA algorithm to perform local path planning between adjacent sub-target points until the target point;

[0011] The fusion algorithm is optimized using global energy-saving path deviation evaluation and heading angle adaptive adjustment strategy.

[0012] As a preferred solution of the low-energy mobile robot path planning method of the present invention, the robot path planning in a static environment using the ESA* algorithm includes: constructing a total energy consumption equation of the mobile robot based on the energy consumption of the mobile robot, expressed as:

[0013] E robot =(1+k)E motion =(1+k)[mg sin(φ)*s+μmgcos(φ)*s]

[0014] Where m is the total mass of the robot, g is the acceleration due to gravity, μ is the friction coefficient, and φ is the slope and its value range is: s is the moving distance; k is the energy consumption of the equipment E equipment and exercise energy consumption E motion The proportional coefficient.

[0015] As a preferred solution of the low-energy mobile robot path planning method of the present invention, wherein: the robot path planning in a static environment using the ESA* algorithm further includes: constructing a new path search criterion based on energy consumption cost of the ESA* algorithm, which is expressed as:

[0016] E(k)=g′(k-1)+A E (k-1, k)+A E (k, goal)

[0017] in, E(k) is the total energy consumption of the mobile robot traveling to the path node k; g′(k-1) is the energy consumption cost of the parent node of node k; A E (k-1, k) is the actual energy consumption cost between node k-1 and node k; A E (k, goal) is the estimated energy cost from node k to the target point.

[0018] As a preferred solution of the low-energy mobile robot path planning method of the present invention, the turning penalty includes an energy consumption evaluation function that introduces a penalty factor, which is expressed as:

[0019]

[0020] Among them, p is the turning penalty factor, ψ is the angle between the extended node and the dynamic rotation reference.

[0021] As a preferred embodiment of the low-energy mobile robot path planning method of the present invention, the pruning process includes performing triangular pruning on the initial path nodes to further eliminate redundant nodes and reduce the number of turns and turning angles. The specific steps are as follows:

[0022] When the ESA* algorithm initially determines the energy-saving path generation point, it performs path node pruning and starts from the initial path node set N{n i ,1≤i≤m}, select three nodes in turn as the triangle point Q, the middle node M and the end point E and perform triangle determination; if it is not a triangle, the middle node M is a redundant point, delete it, and update N; otherwise, connect QE, perform obstacle determination on the line segment QE, if there is no obstacle, the middle node M is a redundant point, delete it, and update N; otherwise, do not update N; when the path node set N is updated, the triangle point Q remains unchanged, and in the node set N, start from the triangle point Q again and perform pruning optimization on each node in turn until the target point n is reached. m When the initial path node set N is not updated, the intermediate node M is updated to the triangle point Q. Similarly, the next node in the node set N is pruned again with the new triangle point Q until the target point n is reached. m .

[0023] As a preferred embodiment of the low-energy mobile robot path planning method of the present invention, the smoothing process includes: using a dynamic chord-arc transition method to smooth the turning points to generate a smooth, continuous, low-energy path. The specific steps are as follows:

[0024] Calculate the turning angle and draw its angle bisector T i T i ′, on the angle bisector T i T i Choose any point on ′ as the chord point C i ;

[0025] Draw the angle bisector T through the chord point i T i ′, and the perpendicular line to the turning edge segment T i-1 T i 、T i T i+1 Intersect at two points h i 、h i ′, we get the chord h i h i ';

[0026] Chord h i h i 'Obstacle assessment, if there is an obstacle, use Ci ′=C i +λ|C i T i |Dynamically update the chord point and re-acquire the chord line; otherwise, through point h i Draw line segment T i-1 T i The perpendicular line intersects the angle bisector T i T i ′ at point O i , calculate the curvature of the smooth path |O i h i | -1 Where, C i ′ is the new chord point, λ is the dynamic update factor, |C i T i | is line segment C i T i length;

[0027] Determine whether the curvature of the smooth path meets the requirements of safety and smoothness. If not, return to the chord point acquisition step and execute again. If it meets the requirements, use point O i is the center of the circle, and the radius |O i h i | Make a bad arc inferior arc is the desired smooth path.

[0028] As a preferred solution of the low-energy mobile robot path planning method of the present invention, the local path planning using the IDWA algorithm includes: designing an energy consumption evaluation subfunction to evaluate the energy consumption of the simulated trajectory, and the energy consumption evaluation subfunction is expressed as:

[0029]

[0030] Among them, v i 、ω i is the velocity and angular velocity of the i-th sampling point in the dynamic window; are the linear acceleration and angular acceleration of the i-th sampling point in the dynamic window; m is the total mass of the robot.

[0031] As a preferred solution of the low-energy mobile robot path planning method of the present invention, the method of using the IDWA algorithm for local path planning further includes constructing a global energy-saving path deviation evaluation subfunction Globdist(v,ω), which is expressed as:

[0032]

[0033] in, are the coordinates of the sample points on the global planning path, is the coordinate of the sampling point on the local motion trajectory, and N is the total number of sampling points.

[0034] As a preferred solution of the low-energy mobile robot path planning method of the present invention, the method of using the IDWA algorithm for local path planning further includes adding a global energy-saving path deviation evaluation subfunction Globdist(v, ω) to the evaluation function of the IDWA algorithm to form a final evaluation function, which is expressed as:

[0035] G(v,ω)=σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)+δEnergy(v,ω)+εGlobdist(v,ω)]

[0036] Among them, αhead(v,ω), dist(v,ω), vel(v,ω), Energy(v,ω), and Globdist(v,ω) are the azimuth, distance, velocity evaluation, energy consumption, and bias sub-functions respectively; σ is a smoothing function; α, β, γ, δ, and ε are the weighting coefficients of each evaluation sub-function.

[0037] As a preferred solution of the low-energy mobile robot path planning method described in the present invention, the heading angle adaptive adjustment strategy includes: calculating the tilt angle between the sub-target points and converting it into radians, performing dynamic obstacle inspection on the path segment between the two sub-target points, and if there is an obstacle, determining whether there is a static obstacle in the left and right areas between the robot and the dynamic obstacle. If there is a static obstacle in the left area, the ideal heading angle yaw is used. idea The heading angle deviation threshold yaw set for the reference deviation to the right out Adjust the heading angle and update the robot state parameters as the next state parameters; otherwise, deviate to the left from yaw out Adjust the heading angle; if there is no obstacle, obtain the robot's real-time heading angle yaw based on the robot's state parameters now , will yaw now with yaw idea For comparison, if the heading angle error threshold yaw is set in ≤|yaw now -yaw idea |, the heading angle dynamic adjustment formula is used for adjustment; otherwise, no heading angle adjustment is performed;

[0038] The formula for dynamic adjustment of heading angle is expressed as:

[0039]

[0040] Among them, yaw newis the dynamically adjusted heading angle, the starting heading angle of the next state; κ is the robot's turning ability coefficient.

[0041] Beneficial effects of the present invention: The present invention proposes a global energy-saving path planning method based on an energy consumption cost optimization strategy, and a path optimization method based on a combination of dynamic benchmark turning penalty, triangle pruning and chord-arc transition method to ensure the optimal global path; through global energy-saving path deviation evaluation and adaptive adjustment of heading angle, redundant paths are avoided in the local planning of the mobile robot, and falling into local solutions is avoided; the present invention can be deployed and applied to mobile robot equipment with low configuration, and can complete path planning for the mobile robot, and consider time, length, energy consumption, etc. to determine whether the robot can complete as many tasks as possible under limited energy; the present invention can quickly plan a reasonable path, reduce workshop energy consumption, and save manpower expenditure. Since the path planning method has the function of automatic obstacle avoidance, workers do not need to consider the robot's movement route when moving in normal work in the workshop, which is convenient for workers to work; the present invention can run directly on the mobile robot without the need for additional hardware facilities, and is simple and convenient to use, which reduces the cost of use. It is very conducive to widespread use in production workshops, logistics workshops and other places, and has extremely high scalability. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive effort. Among them:

[0043] Figure 1 A method flow chart of a low-energy mobile robot path planning method provided by one embodiment of the present invention;

[0044] Figure 2 A schematic diagram of the force applied to a mobile robot during movement according to one embodiment of the present invention;

[0045] Figure 3 An embodiment of the present invention provides a ground-level adjacency matrix A φ Build a schematic diagram;

[0046] Figure 4 A schematic diagram of a dynamic angle reference provided by one embodiment of the present invention;

[0047] Figure 5 A schematic diagram of path triangle pruning provided by one embodiment of the present invention;

[0048] Figure 6 A schematic diagram of path turning smoothing processing provided by one embodiment of the present invention;

[0049] Figure 7 A schematic diagram of an energy-saving path deviation evaluation sub-function provided by one embodiment of the present invention;

[0050] Figure 8 A schematic diagram of a heading angle adjustment according to an embodiment of the present invention is provided. Figure 8 (a) is a schematic diagram of random heading angles. Figure 8 (b) is a schematic diagram of the heading angle falling into a local solution. Figure 8 (c) Schematic diagram of the cumulative error of the heading angle;

[0051] Figure 9 A graph showing the result of smoothing the turning points of a path provided by one embodiment of the present invention;

[0052] Figure 10 A performance comparison chart of a path smoothing method provided by one embodiment of the present invention;

[0053] Figure 11 A schematic diagram of an experimental simulation environment provided by an embodiment of the present invention, Figure 11 (a) is a 15*15 simulation environment. Figure 11 (b) is a 30*30 simulation environment. Figure 11 (c) is a 60*60 simulation environment;

[0054] Figure 12 A schematic diagram of path planning trajectories using different algorithms provided in one embodiment of the present invention. Figure 12 (a) shows the path planning trajectories of different algorithms in a 15*15 simulation environment. Figure 12 (b) is the path planning trajectory of different algorithms in a 30*30 simulation environment. Figure 12 (c) Path planning trajectories of different algorithms in a 60*60 simulation environment;

[0055] Figure 13 A graph showing the results of a multi-index normalized analysis provided by an embodiment of the present invention. Figure 13 (a) is the result of multi-index normalization analysis in a 15*15 simulation environment. Figure 13 (b) is the result of multi-index normalization analysis in a 30*30 simulation environment. Figure 13 (c) is the result of multi-index normalization analysis in 60*60 simulation environment;

[0056] Figure 14 A schematic diagram of a path trajectory planned in an open environment provided by an embodiment of the present invention. Figure 14 (a) is the path trajectory planned by the ESA* algorithm, Figure 14 (b) is the path trajectory planned by the IDWA algorithm. Figure 14(c) is the path trajectory planned by the traditional fusion algorithm. Figure 14 (d) Path trajectory planned for the improved fusion algorithm;

[0057] Figure 15 A schematic diagram of a path trajectory planned in a dense environment provided by an embodiment of the present invention. Figure 15 (a) is the path trajectory planned by the ESA* algorithm, Figure 15 (b) is the path trajectory planned by the IDWA algorithm. Figure 15 (c) is the path trajectory planned by the traditional fusion algorithm. Figure 15 (d) Path trajectory planned for the improved fusion algorithm;

[0058] Figure 16 A comparison chart of obstacle avoidance results in a static environment using a fusion algorithm provided by one embodiment of the present invention. Figure 16 (a) is the obstacle avoidance process of the traditional fusion algorithm. Figure 16 (b) To improve the obstacle avoidance process of the fusion algorithm;

[0059] Figure 17 A comparison chart of obstacle avoidance results in a dynamic environment using a fusion algorithm provided by one embodiment of the present invention. Figure 17 (a) is the obstacle avoidance process of the traditional fusion algorithm. Figure 17 (b) Obstacle avoidance process of the improved fusion algorithm. DETAILED DESCRIPTION

[0060] To make the above-mentioned objects, features, and advantages of the present invention more clearly understood, the following detailed description of the specific embodiments of the present invention is given in conjunction with the accompanying drawings. It is obvious that the described embodiments are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary persons in this field without creative work should fall within the scope of protection of the present invention.

[0061] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Those skilled in the art may make similar generalizations without violating the connotation of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0062] Secondly, the term "one embodiment" or "embodiment" herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in various places throughout this specification does not necessarily refer to the same embodiment, nor does it refer to a separate or selective embodiment that is mutually exclusive of other embodiments.

[0063] The present invention is described in detail with reference to schematic diagrams. For ease of illustration, cross-sectional views of device structures may be partially enlarged and not to scale when describing embodiments of the present invention. Furthermore, the schematic diagrams are merely illustrative and should not limit the scope of the present invention. Furthermore, in actual production, the three-dimensional dimensions of length, width, and depth should be included.

[0064] In the description of the present invention, it should be noted that the terms "upper, lower, inner, and outer" and other references to orientations or positional relationships are based on the orientations or positional relationships shown in the accompanying drawings and are intended solely to facilitate and simplify the description of the present invention. They are not intended to indicate or imply that the devices or components referred to must have, be constructed, or operate in a specific orientation, and therefore should not be construed as limitations on the present invention. Furthermore, the terms "first, second, or third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0065] In this disclosure, unless otherwise specified or limited, the terms "mounted," "connected," and "connected" should be interpreted broadly. For example, they may refer to fixed, removable, or integral connections. They may also refer to mechanical, electrical, or direct connections, indirect connections through an intermediary, or internal communication between two components. Those skilled in the art will understand the specific meanings of these terms in this disclosure.

[0066] Example 1

[0067] Reference Figures 1 to 8 , which is the first embodiment of the present invention, provides a low-energy mobile robot path planning method, comprising:

[0068] S1: Use the grid method to construct the path planning environment map and determine the starting position and target position.

[0069] Specifically, the size of the image is obtained, horizontal and vertical lines are drawn to form a grid image, the positions of obstacles are defined, the starting position and the target position are defined, and finally a grid map is generated.

[0070] S2: Use the ESA* algorithm to plan the robot path in a static environment, and perform turning penalties, pruning, and smoothing on the planned path to obtain a global energy-saving path.

[0071] Specifically, the total energy consumption equation of the mobile robot is constructed based on the energy consumption of the mobile robot, which is expressed as:

[0072] E robot =E equipment +E motion =∫ t P equipment dt+∫ t P motiondt (1)

[0073] Among them, P motion Indicates the power loss during movement, P equipment Indicates the power consumption of the robot's internal resistance, sensors, and other electronic devices.

[0074] It is important to know that the energy loss of a mobile robot mainly comes from the energy consumed by electronic equipment such as sensors and the energy consumed during movement (ignoring the heat energy transferred to the air).

[0075] Furthermore, assuming that the mobile robot moves at a constant speed under the condition that the road slope is constant and the road roughness is the same, Figure 2 The force conditions during the movement of the mobile robot show that the energy consumed by the mobile robot in overcoming the traction resistance during the movement can be expressed as:

[0076] W=P motion *Δt=F traction *Δs=(F f +F air +F g )*L (2)

[0077] Where W is the total work done during the movement; F traction is the total traction force during the movement; F f 、F air 、F g They are respectively the friction force, air resistance and horizontal component of gravity during the movement; L is the moving distance.

[0078] Furthermore, considering that the mobile robot always maintains a constant speed and low speed during the movement, the influence of air resistance is ignored. Therefore, from formulas (1) and (2), the total energy consumption E of the robot during the movement can be obtained as robot It can be expressed as:

[0079] E robot =(1+k)E motion =(1+k)[mgsin(φ)*s+μmgcos(φ)*s] (3)

[0080] Where m is the total mass of the robot, g is the acceleration due to gravity, μ is the friction coefficient, and φ is the slope and its value range is: s is the moving distance; k is the energy consumption of the equipment E equipment and exercise energy consumption E motion The proportional coefficient.

[0081] Furthermore, we construct the distance-based adjacency matrix A D With the energy-based adjacency matrix A E , with the energy matrix AE (i, j) is used as the search criterion of the ESA* algorithm and is expressed as:

[0082] A E (i, j) = E robot =(1+k)[sin(φ)+μ i,j cos(φ)]mgA D (i, j) (4)

[0083] Among them, μ i,j is the average friction coefficient from position i to position j.

[0084] It should be noted that the distance-based adjacency matrix and the energy-based adjacency matrix are constructed to represent the robot's moving distance and energy consumption in a complex environment.

[0085] Furthermore, Figure 3 As shown, construct the adjacency matrix A based on the ground level φ (i, j), expressed as:

[0086] A φ (i, j) = φ j -φ i (5)

[0087] Among them, A φ (i, j) represents the slope size and slope state from position i to position j. The absolute value of the difference between the values of adjacent positions i and j represents the slope between the corresponding areas. The positive and negative values indicate the climbing and descending states of the mobile robot.

[0088] Furthermore, a new path search criterion based on energy cost of the ESA* algorithm is obtained, which is expressed as:

[0089] E(k)=g′(k-1)+A E (k-1, k)+A E (k, goal) (6)

[0090] Among them, αhead(v,ω), dist(v,ω), vel(v,ω), Energy(v,ω), and Globdist(v,ω) are the azimuth, distance, velocity evaluation, energy consumption, and bias sub-functions respectively; σ is a smoothing function; α, β, γ, δ, and ε are the weighting coefficients of each evaluation sub-function.

[0091] Furthermore, a turning penalty factor based on a dynamic corner benchmark is introduced in node expansion;

[0092] Specifically, such as Figure 4As shown in the figure, the robot's moving direction is first determined by the coordinate relationship between the parent node and the current node, and the moving direction is used as the turning angle reference; then, according to the positional relationship between the extended node and the turning angle reference, the turning penalty factor is used in the energy consumption evaluation function to penalize it.

[0093] The energy consumption evaluation function with the penalty factor introduced is expressed as:

[0094]

[0095] Among them, p is the turning penalty factor, ψ is the angle between the extended node and the dynamic rotation reference.

[0096] Furthermore, the initial path nodes are subjected to triangular pruning to further eliminate redundant nodes and reduce the number and angle of turns;

[0097] Specifically, such as Figure 5 As shown in the figure, when the ESA* algorithm initially determines the energy-saving path generation point, it performs path node pruning processing, starting from the initial path node set N{n i ,1≤i≤m}, select three nodes in turn as the triangle point Q, the middle node M and the end point E and perform triangle determination; if it is not a triangle, the middle node M is a redundant point, delete it, and update N; otherwise, connect QE, perform obstacle determination on the line segment QE, if there is no obstacle, the middle node M is a redundant point, delete it, and update N; otherwise, do not update N; when the path node set N is updated, the triangle point Q remains unchanged, and in the node set N, start from the triangle point Q again and perform pruning optimization on each node in turn until the target point n is reached. m When the initial path node set N is not updated, the intermediate node M is updated to the triangle point Q. Similarly, the next node in the node set N is pruned again with the new triangle point Q until the target point n is reached. m .

[0098] It should be noted that the present invention solves the problem of extra energy consumption caused by the ESA* algorithm due to many redundant nodes, many path turns, and large turning angles by setting up a two-stage optimization method: in the first stage, a penalty factor is introduced to penalize the turning of path nodes; in the second stage, triangle pruning is performed on the initial path nodes.

[0099] Furthermore, the dynamic chord-arc transition method is used to smooth the turning points and generate a smooth, continuous and low-energy path;

[0100] It should be noted that the turning angles of the path cause the robot to start and stop frequently, which will lead to alternating speeds and cause a large amount of energy loss, thus affecting the durability of the robot's operation. Therefore, the turning points need to be smoothed to reduce losses.

[0101] Specifically, such as Figure 6 As shown, in an optional embodiment, the specific steps of smoothing are:

[0102] (1) Calculate the turning angle And draw its angle bisector T i T i ′, on the angle bisector T i T i Choose any point on ′ as the chord point C i ;

[0103] (2) Draw the angle bisector T through the chord point i T i ′, and the perpendicular line to the turning edge segment T i-1 T i 、T i T i+1 Intersection with point h i 、h i ′, we get the chord h i h i ';

[0104] (3) Chord h i h i 'Obstacle assessment: if there is an obstacle, dynamically update the chord point according to formula (8) and go to step (2); otherwise, go to step (4);

[0105] C i ′=C i +λ|C i T i | (8)

[0106] Among them, C i ′ is the new chord point, λ is the dynamic update factor, |C i T i | is line segment C i T i length;

[0107] (4) Passing point h i Draw line segment T i-1 T i The perpendicular line intersects the angle bisector T i T i ′ at point O i , calculate the curvature of the smooth path |O i h i | -1 If the requirements of safety and smoothness are met, go to step (5); otherwise, update the chord points according to formula (8) and repeat steps (2) to (4).

[0108] (5) Point O i is the center of the circle, and the radius |O i hi | Make a bad arc inferior arc is the desired smooth path.

[0109] What you need to know is that the string|h i h i The shorter the ′|, the safer the mobile robot is to pass through and the less likely it is to collide with obstacles, but the path is not smooth enough. Therefore, the dynamic chord-fixed arc transition method can generate a corresponding smooth path according to different requirements such as safety and smoothness.

[0110] S3: Extract the sub-target points of the global energy-saving path to generate a sub-target point sequence, and use the IDWA algorithm to perform local path planning between adjacent sub-target points until the target point.

[0111] Specifically, an energy consumption evaluation sub-function is designed to evaluate the energy consumption of the simulation trajectory. The energy consumption evaluation sub-function is expressed as:

[0112]

[0113] Among them, v i 、ω i is the velocity and angular velocity of the i-th sampling point in the dynamic window; are the linear acceleration and angular acceleration of the i-th sampling point in the dynamic window; m is the total mass of the robot.

[0114] It should be noted that the traditional DWA algorithm uses an evaluation function to score the trajectories of the velocity space simulation and select the optimal trajectory. However, the energy consumed by the trajectory is not taken into account in the trajectory scoring, so the optimal trajectory is not necessarily the one with the least energy loss. Therefore, it is necessary to specially design an energy consumption evaluation subfunction to evaluate the energy consumption of the simulated trajectories.

[0115] The evaluation function of IDWA is expressed as:

[0116] G(v,ω)=σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)+δEnergy(v,ω)] (10)

[0117] Among them, αhead(v,ω), dist(v,ω), vel(v,ω), and Energy(v,ω) are the azimuth, distance, velocity evaluation, and energy consumption sub-functions respectively; σ is a smoothing function; α, β, γ, and δ are the weighting coefficients of each evaluation sub-function.

[0118] Furthermore, the global energy-saving path deviation evaluation subfunction Globdist(v,ω) is added to the evaluation function of the fusion algorithm, which is expressed as:

[0119]

[0120] in, are the coordinates of the sample points on the global planning path, is the coordinate of the sampling point on the local motion trajectory, and N is the total number of sampling points.

[0121] The evaluation function after optimization by the global path planning algorithm ESA* and the local path planning algorithm IDWA is expressed as:

[0122] G(v,ω)=σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)+δEnergy(v,ω)+εGlobdist(v,ω)] (12)

[0123] Among them, ε is the weighted coefficient of the Globaldist(v,ω) evaluation subfunction.

[0124] It should be noted that if Figure 7 As shown in the figure, the global energy-saving path deviation evaluation subfunction is used to calculate the distance between the local motion trajectory and the global energy-saving path; the global energy-saving path deviation evaluation subfunction is added to the evaluation function of the fusion algorithm to avoid additional energy loss, improve safety, and make the path planned by the fusion algorithm closer to the global energy-saving path.

[0125] S4: Optimize the fusion algorithm using global energy-saving path deviation evaluation and heading angle adaptive adjustment strategy.

[0126] It should be noted that the optimization of the fusion algorithm using the global energy-saving path deviation evaluation and heading angle adaptive adjustment strategy is to solve problems such as large deviation between the planned path and the global energy-saving path, path redundancy, falling into local solutions, and path-finding failure.

[0127] It should also be noted that when the fusion algorithm plans a path in a narrow space, the correct navigation angle is crucial to successfully avoid dynamic obstacles and quickly reach the target point; in a narrow space, when the robot is moving towards a dynamic obstacle, such as Figure 8 As shown in (a), the traditional fusion algorithm only randomly adjusts the heading angle to the left or right according to the evaluation function, without considering whether the robot can pass through the space between the dynamic obstacle and the static obstacle in front. This can easily cause the robot to circle around and adjust the heading angle or collide with the dynamic obstacle. When the mobile robot moves in the same direction as the dynamic obstacle, Figure 8 As shown in (b), the dynamic window of the traditional fusion algorithm may fall into a local solution and can only move forward slowly following the dynamic obstacle, which is time-consuming and causes the robot to frequently switch motion modes, which is not conducive to the stability of the robot's motion; when the robot plans a path between adjacent sub-target points, the heading angle will produce cumulative errors, such as Figure 8As shown in (c), it will lead to path redundancy and fall into a local solution; to address the above problem, the present invention adopts an adaptive adjustment strategy to optimize the heading angle in the robot state parameters.

[0128] In an optional embodiment, the specific steps of optimizing using the adaptive adjustment strategy are:

[0129] Step 1: Calculate the sub-target point h i-1 、h i The inclination angle in the rectangular coordinate system is converted into radians and the calculation formula is:

[0130]

[0131] Among them, the value range of the arctan2() function is [-π,π]; is the increment of the horizontal and vertical coordinates of adjacent sub-target points; is the path segment h i-1 h i The tilt angle in the rectangular coordinate system; yaw i-1i is the robot path segment h i-1 h i Ideal yaw angle for movement idea ;

[0132] Step 2: For the path segment h i-1 h i Perform dynamic obstacle check. If an obstacle is detected, go to step 3 to execute heading angle adjustment strategy 1. Otherwise, go to step 4 to execute heading angle adjustment strategy 2.

[0133] Step 3: Heading Angle Adjustment Strategy 1: Determine whether there are static obstacles in the left and right areas between the robot and the dynamic obstacle. If there are static obstacles in the left area, use yaw idea Deviate to the right from yaw out Adjust the heading angle and update the robot state parameters (x, y, yaw out ,v,w), and use it as the next state parameter; otherwise, it deviates to the left yaw out Adjust the heading angle; yaw out This is the heading angle deviation threshold, which can be set according to actual conditions. Execute step 3 and go to step 6.

[0134] Step 4: Heading Angle Adjustment Strategy 2: Get the robot's real-time heading angle yaw based on the robot's state parameters (x, y, yaw, v, w) now , will yaw now with yaw idea For comparison, if yaw in ≤|yaw now-yaw idea |, then jump to step 5, otherwise, do not adjust the heading angle and go to step 6; among them, yaw in It is the heading angle error threshold, which can be set according to the actual situation;

[0135] Step 5: If yaw now Greater than yaw i a, then the robot's heading angle is dynamically reduced and the robot state parameters (x, y, yaw new ,v,w) and use it as the next state parameter; otherwise, the navigation angle is dynamically increased.

[0136] The formula for dynamic adjustment of heading angle is expressed as:

[0137]

[0138] Among them, yaw new is the dynamically adjusted heading angle, the starting heading angle of the next state; κ is the robot's turning ability coefficient.

[0139] Step 6: The heading angle adjustment is completed.

[0140] Furthermore, the global energy-saving path deviation evaluation function is used to comprehensively evaluate the planned path, and the heading angle of the planned path is judged and adjusted in real time using the heading angle adaptive adjustment strategy, ultimately obtaining the global energy-saving path of the mobile robot.

[0141] Example 2

[0142] Reference Figures 9 to 17 , which is an embodiment of the present invention, provides a low-energy mobile robot path planning method and device. In order to verify the beneficial effects of the present invention, scientific demonstration is carried out through experiments.

[0143] I. Verify and analyze the effectiveness of smoothing

[0144] The ESA* algorithm can plan a low-energy and relatively smooth path, but the path still has peaks, which is a discontinuous smooth curve. This is not conducive to the motion following of the mobile robot and affects the smoothness of the robot's operation. Therefore, the turning peaks of the path need to be smoothed.

[0145] This embodiment uses the Bezier curve smoothing method (BCSM), the transition arc method (TAM) and the string-definite arc transition method (SDAM) proposed in this invention to compare and verify the path smoothing. The smoothing results are shown in Figure 2. Figure 10 The experimental data are shown in Table 1, where bending energy (BE) and path length energy (TBE) are used as evaluation indicators of the robot path smoothness.

[0146] Table 1 Comparison of experimental results of path smoothing methods

[0147]

[0148] Depend on Figure 9 It can be seen that under the same environment, BCSM, TAM and SDAM all complete the smoothing of turning peaks and obtain smooth and continuous paths, but from Table 1 and Figure 10 It can be seen that: in terms of path curvature, BCSM has the largest path curvature, while TAM and SDAM have similar path curvatures but are much smaller than BCSM. The BE and TBE values are reduced by 92.62%, 93.41%, 88.89% and 90.07% respectively compared with BCSM. In terms of path length, BCSM, TAM and SDAM are all reduced by 1.701%, 1.774% and 2.101% respectively compared with the unsmoothed path. Since the curvature and path length of BCSM after smoothing are the largest, the TBE index value is larger. Figure 9 From the local detail diagram of the path, it can be seen that the path smoothed by TAM will collide with obstacles. This is because the selection principle of the transition arc radius is the proportional value of the shortest adjacent edge of the turning point. The smoothing process cannot dynamically change the arc radius. However, BCSM and SDAM can ensure the safety of the optimized path.

[0149] Based on the above analysis, SDAM can design a smooth, continuous path with appropriate curvature and high safety.

[0150] II. Comparison and Analysis of Comprehensive Performance of ESA* Algorithms

[0151] This embodiment is constructed as Figure 11 The three simulation environments shown are 15*15, 30*30, and 60*60. The numbers in the grid cells represent the ground friction coefficient μ. The green box indicates that the slope of the grid cell in the box to the ground level is 8 degrees, while the red one is 12 degrees. In the same environment, the ACO algorithm, Dijkstra algorithm, traditional A* algorithm, greenhouse robot path planning based on the fusion of improved A* and DWA algorithm (algorithm in reference [8]), energy-saving A* algorithm based on energy consumption optimization (algorithm in reference

[10] ) and ESA* algorithm are used to perform global path planning of the robot. The planned trajectory is shown in the figure. Figure 12The performance index results are shown in Table 2. In order to more intuitively compare the performance of the ESA* algorithm with other methods on these performance indicators, the performance indicators are normalized and expressed in the form of histograms, as shown in Figure 13 As shown in the figure, the “-” in the figure indicates that the value is inverted and calculated using the Min-Max standard normalization. If the index value is closer to 1, it means that the performance is better.

[0152] Table 2 Comparison of experimental results of different path planning methods for mobile robots

[0153]

[0154] from Figure 12 From the path trajectory, we can see that the above algorithm can successfully plan the path in environments with different complexities. Figure 13 It can be seen from the table that the overall performance of the ESA* algorithm is better than that of other algorithms. In terms of energy consumption and path length, the length of the paths planned by the ACO algorithm, Dijkstra algorithm, traditional A* algorithm and the algorithm in reference [8], which take path length as the optimization target, is not much different from the path length planned by the ESA* algorithm, but they consume much more energy than the ESA* algorithm. In the 15*15, 30*30 and 60*60 environments, they consume at least 68.6098J, 265.1747J and 204.8519J more energy, or even more. Compared with the algorithm in reference

[10] which takes energy consumption as the optimization target, the ESA* algorithm reduces energy loss and path length by an average of 6.93% and 4.81%, effectively improving the path quality. The energy consumption per unit distance is obtained by combining the energy consumption and length of the path. As shown in Table 2, the energy consumption per unit distance of the ESA* algorithm is significantly better than that of other algorithms, especially in small-scale maps.

[0155] In terms of path smoothness, the ESA* algorithm reduces the number of turns by an average of 22.72% and the number of instructions by 70.41% compared with the traditional A* algorithm. At the same time, the cumulative turning angle of the path is also reduced by an average of 38.67%, which significantly improves the smoothness of the path. Compared with the path smoothing method in reference [8], when the number of turning points is similar to the number of instruction nodes, the average turning angle of the path is reduced from 600 degrees to 404.8 degrees, which effectively reduces the turning angle. Compared with other algorithms such as ACO and Dijkstra, the ESA* algorithm has greatly improved in terms of the number of turning points, the number of instruction nodes and the turning angle.

[0156] Overall, the ESA* algorithm can plan a low-energy and relatively smooth path for the robot in a static environment.

[0157] III. Simulation Experiment and Analysis of Fusion Algorithm

[0158] To verify the feasibility of the improved fusion algorithm, comparative simulation experiments were conducted in two environments with different complexity: open and dense. The ESA* algorithm, IDWA algorithm, ESA* algorithm and traditional DWA unimproved fusion algorithm (traditional fusion algorithm), and ESA* algorithm and IDWA improved fusion algorithm (improved fusion algorithm) were used. The results are shown in Figure 2. Figures 15-16 shown.

[0159] Depend on Figure 14 (a) and 15(a), we can see that the ESA* algorithm can plan a continuous and smooth global path in both open and dense environments. However, since the ESA* algorithm itself does not have the characteristics of local path planning, it cannot avoid random dynamic and static obstacles in real time in a dynamic environment. Figure 14 As can be seen from (b) and 15(b), the IDWA algorithm can successfully reach the target point in an open environment, but the path is curved and not very smooth. When in a dense and complex environment, the IDWA algorithm falls into a local solution and path planning fails.

[0160] Depend on Figure 14 From the analysis of Figures 15(c~d) and 15(c~d), it can be seen that both the traditional fusion algorithm and the improved fusion algorithm can successfully reach the target point in an open environment, but the deviation of the path planned by the traditional fusion algorithm from the global energy-saving path is significantly greater than that of the improved fusion algorithm, which easily causes additional energy loss; in a dense environment, the improved fusion algorithm can successfully reach the target point, while the traditional fusion algorithm falls into a local solution due to the accumulated deviation of the heading angle and fails to find the path.

[0161] In general, compared with the traditional fusion algorithm, the improved fusion algorithm is closer to the global energy-saving path, can effectively reduce the loss of additional energy, adaptively adjust the heading angle, avoid the generation of redundant paths, and prevent falling into local solutions.

[0162] IV. Obstacle Avoidance Experiment and Analysis of Fusion Algorithm

[0163] In order to verify the effectiveness and superiority of the improved fusion algorithm in avoiding dynamic and static obstacles in a narrow space, simulation comparison experiments were carried out on the traditional fusion algorithm and the improved fusion algorithm. The experimental results are shown in Table 3.

[0164] Table 3 Obstacle avoidance experimental results of fusion algorithm

[0165]

[0166] In a narrow static environment, three static obstacles are set on the path, represented by blue squares. Figure 16It can be seen that when static obstacles appear in the environment, both the traditional fusion algorithm and the improved fusion algorithm can successfully avoid and reach the target point. However, the obstacle avoidance path of the traditional fusion algorithm deviates greatly from the global energy-saving path, and the path is redundant and curved. As shown in Table 3, compared with the global energy-saving path, the path lengths of the improved fusion algorithm and the traditional fusion algorithm increase by 0.73% and 2.83%, respectively. The improved fusion algorithm can avoid the generation of redundant paths while successfully avoiding obstacles, and the improved fusion algorithm can reach the target point faster after successfully avoiding obstacles.

[0167] In the dynamic environment of a narrow space, three types of dynamic obstacles are set up: same direction, opposite direction, and side direction. They are represented by yellow squares, and the arrows indicate the moving direction of the dynamic obstacles.

[0168] from Figure 17 As shown in (a), when the robot moves in the same direction as a dynamic obstacle in a narrow environment and the speed of the dynamic obstacle is slower than that of the mobile robot, the traditional fusion algorithm will be trapped in a local solution and can only follow the slow-moving dynamic obstacle and cannot jump out, which wastes a lot of time. When the mobile robot and the dynamic obstacle move towards each other, the mobile robot cannot correctly adjust its heading angle and circles in place, resulting in redundant paths and serious pathfinding failure.

[0169] according to Figure 17 As can be seen from Figure 3 (b) and Table 3, the improved fusion algorithm can effectively avoid dynamic obstacles in a dynamic environment with narrow space, adaptively adjust the heading angle, avoid generating redundant paths, and prevent falling into a local solution.

[0170] In summary, the improved fusion algorithm in this paper can complete the obstacle avoidance of dynamic and static obstacles in a narrow space environment, and the path is continuous and smooth, closely following the global energy-saving path.

[0171] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

[0172] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

[0173] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code. The scheme in the embodiment of the present application can be implemented in various computer languages, for example, object-oriented programming language Java and literal translation scripting language JavaScript, etc.

[0174] 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 produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the steps in the process. 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.

[0175] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work 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 The function specified in one or more boxes.

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

[0177] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.

[0178] Obviously, those skilled in the art may make various changes and modifications to this application without departing from the spirit and scope of this application. Thus, if these modifications and variations of this application fall within the scope of the claims of this application and their equivalents, this application is intended to include these modifications and variations.

Claims

1. A low-energy mobile robot path planning method, characterized by: include, Use the grid method to construct the path planning environment map and determine the starting position and target position; The ESA* algorithm is used to plan robot paths in static environments. The planned paths are then penalized, pruned, and smoothed to obtain a global energy-saving path. The smoothing process includes: using a dynamic chord-arc transition method to smooth the turning points to generate a smooth, continuous, and low-energy path. The specific steps are as follows: Calculate the turning angle and draw its angle bisector T i T i ′, on the angle bisector T i T′ i Choose any point on the chord point C i ; Draw the angle bisector T through the chord point i T i ′, and the perpendicular line to the turning edge segment T i-1 T i 、T i T i+1 Intersect at two points h i 、h i ′, we get the chord h i h i '; Chord h i h i 'Obstacle assessment, if there is an obstacle, use C' i =C i +λ|C i T i |Dynamically update the chord point and re-acquire the chord line; otherwise, through point h i Draw line segment T i-1 T i The perpendicular line intersects the angle bisector T i T′ i At point O i , calculate the curvature of the smooth path |O i h i | -1 Where C′ i is the new chord point, λ is the dynamic update factor, |C i T i | is line segment C i T i length; Determine whether the curvature of the smooth path meets the requirements of safety and smoothness. If not, return to the chord point acquisition step and execute again. If it meets the requirements, use point O i is the center of the circle, and the radius |O i h i | Make a bad arc inferior arc is the desired smooth path; Extract the sub-target points of the global energy-saving path to generate a sub-target point sequence, and use the IDWA algorithm to perform local path planning between adjacent sub-target points until the target point; The fusion algorithm is optimized using global energy-saving path deviation evaluation and heading angle adaptive adjustment strategy.

2. The low-energy mobile robot path planning method according to claim 1, wherein: The robot path planning in a static environment using the ESA* algorithm includes: constructing a total energy consumption equation of the mobile robot based on the energy consumption of the mobile robot, which is expressed as: E robot =(1+k)E motion =(1+k)[mg sin(φ)*s+μmg cos(φ)*s] Where m is the total mass of the robot, g is the acceleration due to gravity, μ is the friction coefficient, and φ is the slope and its value range is: s is the moving distance; k is the energy consumption of the equipment E equipment and exercise energy consumption E motion The proportional coefficient.

3. The low-energy mobile robot path planning method according to claim 1 or 2, characterized in that: The robot path planning in a static environment using the ESA* algorithm also includes: constructing a new path search criterion based on energy consumption cost of the ESA* algorithm, which is expressed as: E(k)=g'(k-1)+A E (k-1,k)+A E (k,goal) in, E(k) is the total energy consumption of the mobile robot traveling to path node k; g'(k-1) is the energy consumption cost of the parent node of node k; A E (k-1, k) is the actual energy consumption cost between node k-1 and node k; A E (k, goal) is the estimated energy cost from node k to the target point.

4. The low-energy mobile robot path planning method according to claim 3, wherein: The transition penalty includes: an energy consumption evaluation function that introduces a penalty factor, which is expressed as: Among them, p is the turning penalty factor, ψ is the angle between the extended node and the dynamic rotation reference.

5. The low-energy mobile robot path planning method according to claim 1 or 4, characterized in that: The pruning process includes: performing triangular pruning on the initial path nodes to further eliminate redundant nodes and reduce the number of turns and turning angles. The specific steps are as follows: When the ESA* algorithm initially determines the energy-saving path generation point, it performs path node pruning and starts from the initial path node set N{n i ,1≤i≤m}, select three nodes in turn as the triangle point Q, the middle node M and the end point E and perform triangle determination; if it is not a triangle, the middle node M is a redundant point, delete it, and update N; otherwise, connect QE, perform obstacle determination on the line segment QE, if there is no obstacle, the middle node M is a redundant point, delete it, and update N; otherwise, do not update N; when the path node set N is updated, the triangle point Q remains unchanged, and in the node set N, start from the triangle point Q again and perform pruning optimization on each node in turn until the target point n is reached. m When the initial path node set N is not updated, the intermediate node M is updated to the triangle point Q. Similarly, the next node in the node set N is pruned again with the new triangle point Q until the target point n is reached. m .

6. The low-energy mobile robot path planning method according to claim 5, wherein: The local path planning using the IDWA algorithm includes: designing an energy consumption evaluation sub-function to evaluate the energy consumption of the simulation trajectory, and the energy consumption evaluation sub-function is expressed as: Among them, v i 、ω i is the velocity and angular velocity of the i-th sampling point in the dynamic window; are the linear acceleration and angular acceleration of the i-th sampling point in the dynamic window; m is the total mass of the robot.

7. The low-energy mobile robot path planning method according to claim 6, wherein: The local path planning using the IDWA algorithm also includes: constructing a global energy-saving path deviation evaluation subfunction Globdist(v,ω), which is expressed as: in, are the coordinates of the sample points on the global planning path, is the coordinate of the sampling point on the local motion trajectory, and N is the total number of sampling points.

8. The low-energy mobile robot path planning method according to claim 7, wherein: The method of using the IDWA algorithm for local path planning further includes: adding a global energy-saving path deviation evaluation subfunction Globdist(v,ω) to the evaluation function of the IDWA algorithm to form a final evaluation function, which is expressed as: G(v,ω)=σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)+δEnergy(v,ω)+εGlobdist(v,ω)] Among them, αhead(v,ω), dist(v,ω), vel(v,ω), Energy(v,ω), and Globdist(v,ω) are the azimuth, distance, velocity evaluation, energy consumption, and bias sub-functions respectively; σ is a smoothing function; α, β, γ, δ, and ε are the weighting coefficients of each evaluation sub-function.

9. The low-energy mobile robot path planning method according to claim 8, characterized in that: The heading angle adaptive adjustment strategy includes: calculating the tilt angle between the sub-target points and converting it into radians, checking the path segment between the two sub-target points for dynamic obstacles, and if there is an obstacle, determining whether there is a static obstacle in the left and right areas between the robot and the dynamic obstacle. If there is a static obstacle in the left area, the robot will be guided to the ideal heading angle yaw. idea The heading angle deviation threshold yaw set for the reference deviation to the right out Adjust the heading angle and update the robot state parameters as the next state parameters; otherwise, deviate to the left from yaw out Adjust the heading angle; if there is no obstacle, obtain the robot's real-time heading angle yaw based on the robot's state parameters now , will yaw now with yaw idea For comparison, if the heading angle error threshold yaw is set in ≤|yaw now -yaw idea |, the heading angle dynamic adjustment formula is used for adjustment; otherwise, no heading angle adjustment is performed; The formula for dynamic adjustment of heading angle is expressed as: Among them, yaw new is the dynamically adjusted heading angle, the starting heading angle of the next state; κ is the robot's turning ability coefficient.

Images