Robot path planning method and system based on fusion algorithm

Abstract

The application belongs to the technical field of robot path planning, and specifically discloses a robot path planning method and system based on a fusion algorithm, which comprises the following steps: establishing an environment model, a motion coordinate system and a robot kinematics model; initializing all nodes in the environment model, determining a starting node, a target node and the position of a current node of the robot; planning a safe and reliable path from the starting node to the target node by using an optimized A* algorithm, optimizing the path, and then smoothing the path; creating a motion trajectory space of the robot, introducing an evaluation function to score the motion trajectory space, and selecting the trajectory with the highest score in the motion trajectory space as an optimal trajectory output. According to the technical scheme, the A* algorithm is fused with the dynamic window method, the dynamic programming algorithm is applied according to the key nodes of the global path in sections, the key nodes are reduced, the search efficiency is improved, the robot path is optimized, and the robot can avoid obstacles in real time.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of robot path planning, and relates to a robot path planning method and system based on a fusion algorithm. BACKGROUND

[0002] In an era of rapid technological development, robot technology has great application prospects. Indoor robots have attracted widespread attention in the field of robots, and the role of indoor robots covers multiple fields and applications. In a home environment, it undertakes tasks such as cleaning and organizing items, providing people with convenience and support for life; in social interaction, it has the ability of speech recognition and speech synthesis, and can interact with people.

[0003] Due to the increasing complexity of indoor environments, as a key to the autonomous movement of robots, the research on path planning technology has encountered many challenges to be solved, such as how to dynamically avoid obstacles in motion and how to improve the efficiency of path planning.

[0004] Existing indoor path planning algorithms use many classification methods, and the two major classification methods recognized in the industry are as follows:

[0005] 1. Global (static) planning method. It considers the overall environmental information of the robot, and aims to find a complete route from the starting point to the end point. At present, there are mainly BFS (Breadth-First Search), Dijkstra, A*(A-star), D*(D-star), RRT (Rapidly Exploring Random Trees), PRM (Probabilistic Roadmap), ant colony algorithm, etc.

[0006] 2. Local (dynamic) planning method. It considers the local environmental information of the robot, and can handle the problem of dynamic obstacle avoidance of the robot in a complex environment. At present, there are mainly Dynamic Window Approach (DWA algorithm), Time Elastic Band (TEB), HRAT (Hybrid Receding Horizon Avoidance and Tracking), potential field method, etc.

[0007] Both static and dynamic methods have limitations. Static planning methods can provide a complete route for the robot, but cannot avoid dynamically appearing obstacles; dynamic planning methods may cause the robot to fall into a local optimal solution, so it cannot reach the target point. SUMMARY

[0008] The application aims to provide a robot path planning method and system based on a fusion algorithm, which optimizes the robot path and ensures that the robot can avoid obstacles in real time.

[0009] To achieve the above-mentioned purpose, the basic scheme of the application is as follows: a robot path planning method based on a fusion algorithm, comprising the following steps:

[0010] Obtaining known environment information of a robot workspace, selecting a grid map type to establish an environment model, establishing a motion coordinate system with the robot as the center, and establishing a robot kinematics model;

[0011] Initializing all nodes in the environment model, determining the positions of a starting node, a target node and a current node of the robot in the environment model;

[0012] Planning a safe and reliable path from the starting node to the target node by using an optimized A* algorithm;

[0013] Optimizing the path by using a one-time polyline optimization and a secondary polyline optimization method, and then smoothing the path;

[0014] Based on the robot kinematics model, creating a motion trajectory space of the robot, introducing an evaluation function to score the motion trajectory space, and selecting the trajectory with the highest score in the motion trajectory space as the optimal trajectory output.

[0015] The working principle and beneficial effects of the basic scheme are as follows: the A* algorithm and the DWA algorithm are fused in the technical scheme to form a new path planning algorithm that combines static planning and dynamic planning. The algorithm first obtains a static path planning by the optimized A* algorithm, thereby obtaining key nodes of the whole path; then the DWA algorithm is used to perform dynamic path planning between the key nodes, so as to ensure that the robot can avoid obstacles on the whole path in real time, thereby smoothly reaching the set end point.

[0016] Further, the method for establishing the robot kinematics model is as follows:

[0017] With the robot as the center, the linear velocity v of the robot is positive when the direction of v is the same as that of the x-axis, and is negative when the direction of v is opposite to that of the x-axis; when the robot rotates counterclockwise, the angular velocity ω is positive, and vice versa; the velocity information of the robot is [v ω] T , and the pose information is [X Y θ] T , X is the horizontal coordinate, and Y is the vertical coordinate;

[0018] Let the velocity of the robot at time t be v, the left wheel velocity be v l , the right wheel velocity be v r , the angular velocity be ω, and the velocity information be [v ω]T Then the robot's velocity information at time t is represented as:

[0019]

[0020] Let the robot's pose information at time t be... If the motion time is Δt, then the robot's pose information at the current moment is represented as:

[0021]

[0022] Where T represents the transpose function, and θ is the angle between the robot's head and the Y-axis.

[0023] Before performing kinematic analysis, it is necessary to define the robot's coordinate system and velocity direction, which will facilitate subsequent robot path planning.

[0024] Furthermore, the estimation function of the A* algorithm is:

[0025] F(n) = G(n) + W × H(n)

[0026] Where F(n) represents the overall priority of node n; G(n) represents the cost from the starting point to node n; H(n) represents the estimated cost from node n to the target point; and W is a manually set coefficient to speed up the algorithm's search.

[0027] The path search speed of the A* algorithm can be changed by modifying the weights of the estimation function. Specifically, a coefficient W is added before the estimation function to change its weights and optimize the A* algorithm.

[0028] Furthermore, the method for planning a safe and reliable path from the starting node to the target node is as follows:

[0029] Initialize all nodes and determine if they can be expanded. If they can be expanded, add them to array A.

[0030] For nodes in array A, sort them according to their movement index, and filter the top 50% of adjacent nodes after sorting. If a node is directly above or below the current node and is not in the closed list, add it to array B; if an adjacent node is directly to the left or right of the current node and is not in the closed list, add it to array C, indicating that there are other paths to choose from for the current node.

[0031] The node added to arrays B and C is removed from array A, indicating that it will not be selected in the path;

[0032] The remaining nodes in array A are filtered and sorted, and the first and second adjacent nodes are selected. If both nodes are in the front left or front right of the current node, the one closer to the front is selected and added to the closed list, and the other is discarded. If only one of the two nodes is in the front left or front right, and the other node is in the direct left or direct right of the current node, both nodes are added to the closed list to obtain a safe and reliable path.

[0033] By processing adjacent nodes, it is ensured that in certain cases, paths that come into contact with obstacles are avoided. This helps to improve path planning, avoid collisions, and improve the safety and feasibility of the path.

[0034] Further, the method for determining whether a node can be expanded is as follows:

[0035] The offset angle of the search direction and the final destination, the obstacles in the next step area, and the obstacle blocking area are obtained to determine the search direction travel index:

[0036]

[0037] where i is the search direction number, i = 2, 3, …, 8, β i is the search index of the i-th search direction, θ i is the angle between the i-th search direction and the destination, α i is whether there is an obstacle in the i-th search direction, 0 means there is an obstacle, 1 means there is no obstacle, υ i is the form speed of the i-th search direction obstacle, s i is the footprint area of the i-th search direction obstacle, t i is the estimated time from the current position to the i-th search direction obstacle, s is the standard area of the search area, and l is the search step length.

[0038] Filtering and processing adjacent nodes helps to improve path planning.

[0039] Further, the one-time polyline optimization and two-time polyline optimization methods are used to optimize the path as follows:

[0040] The steps of one-time polyline optimization are as follows:

[0041] All nodes under the path are traversed to determine the angle between the nodes;

[0042] When the nodes are on the same straight line, the parent node is updated to the child node to achieve the purpose of removing redundant nodes;

[0043] When the nodes are not on the same straight line, it is determined whether there is an obstacle between the nodes and the parent node:

[0044] If there is an obstacle, the parent node is not updated;

[0045] If there is no obstacle, the parent node is updated to the child node;

[0046] The steps of the quadratic polyline optimization are as follows:

[0047] Take out the current node, the child node and the child node of the child node, and calculate the nodes on the connecting line between the current point and the child node;

[0048] Traverse the nodes on the connecting line and determine whether there is an obstacle between the node and the current point:

[0049] If there is a connectable node, it is added to the new path;

[0050] If there is no connectable node, the child node is added to the new path.

[0051] The global path obtained by A* is subjected to polyline optimization processing, so as to eliminate the polyline segment and reduce the polyline angle.

[0052] Further, the segmented cubic interpolation method is used to perform the curve smoothing processing steps of the path as follows:

[0053] The segmented cubic interpolation divides the data into multiple adjacent intervals, and a cubic polynomial is used on each interval to approximate the curve:

[0054] y(x)=a(x-x i ) 3 +b(x-x i ) 2 +c(x-x i )+d

[0055] Where x and x i are adjacent points, a, b, c, d are the coefficients in the polynomial; y(x) is the estimated value of the point x after curve approximation.

[0056] The segmented cubic interpolation method is used to perform the curve smoothing processing of the path, and the path is optimized.

[0057] Further, the method for creating the motion trajectory space of the robot is as follows:

[0058] According to the kinematic model of the robot, the motion trajectory space of the robot is created as:

[0059]

[0060] Where [X Y θ] t TThe pose information of the robot at time t, the motion time is Δt; the speed of the robot at time t is v, the angular velocity is ω, and the speed information is [v ω] T ;

[0061] The speed space of the robot is established, and the robot is constrained by its maximum linear speed, maximum angular speed, minimum linear speed, and minimum angular speed:

[0062] V A = {(v, ω) | v ∈ [v MIN , v MAX ] ∩ ω ∈ [ω MIN , ω MAX ]}

[0063] Where [v MIN , v MAX ] represents the maximum linear speed and minimum linear speed of the robot itself; [ω MIN , ω MAX ] represents the maximum angular speed and minimum angular speed of the robot itself.

[0064] The robot is constrained by the motor performance:

[0065]

[0066] Where v1 and ω1 represent the linear speed and angular speed at the current time; and represent the maximum deceleration and maximum deceleration that the linear speed can reach under the influence of the motor; and represent the maximum acceleration and maximum deceleration that the angular speed can reach under the influence of the motor;

[0067] The robot is constrained by obstacles:

[0068]

[0069] Where dist(v, ω) represents the distance of the trajectory corresponding to the speed space from the obstacle;

[0070] Under the condition of meeting the constraint conditions, the speed space is sampled to obtain the corresponding trajectory space.

[0071] With the change of the linear acceleration and angular acceleration of the motor, the speed space will dynamically change, which is called dynamic window. By dividing the motion space of the robot into different windows, and selecting the best window according to the current environment and the state of the robot, safe and efficient movement can be realized.

[0072] Further, the trajectory evaluation function G(v, ω) is:

[0073] G(v, w) = d (a*Heading(v, w) + b*Dist(v, w) + l*Vel(v, w))

[0074] Wherein, Heading(v, w) is a heading angle evaluation function, evaluating the angle gap between the end of the trajectory and the target point at the current set speed of the robot; Dist(v, w) is a distance evaluation function, evaluating the distance between the robot at the end of the predicted trajectory and the nearest obstacle on the map; Vel(v, w) is a linear speed evaluation function, evaluating the linear speed value of the robot at the current time; d, a, b, l are weights.

[0075] The evaluation function is introduced to score the trajectory space, and the trajectory with the optimal score is selected in the trajectory space.

[0076] The application also provides a robot path planning system based on a fusion algorithm, comprising a processing module installed on a robot, wherein the processing module executes the method of the application to plan the path of the robot.

[0077] The system proposes improvement methods such as evaluation function optimization, search neighborhood optimization, obstacle avoidance strategy optimization, path polyline optimization and path curve smoothing, and the improved static planning algorithm can maximize the avoidance of the above-mentioned defects to obtain a perfect global path. BRIEF DESCRIPTION OF DRAWINGS

[0078] Figure 1 Fig. 1 is a flowchart of the robot path planning method based on the fusion algorithm of the application;

[0079] Figure 2 Fig. 2 is a structural diagram of the kinematic model of the robot in the robot path planning method based on the fusion algorithm of the application;

[0080] Figure 3 Fig. 3 is a flowchart of the optimization of the path by using the one-time polyline optimization and the two-time polyline optimization method in the robot path planning method based on the fusion algorithm of the application;

[0081] Figure 4 Fig. 4 is a flowchart of the curve smoothing processing of the path in the robot path planning method based on the fusion algorithm of the application. DETAILED DESCRIPTION

[0082] The embodiments of the application are described in detail below, and examples of the embodiments are shown in the drawings, wherein the same or similar notations represent the same or similar elements or elements with the same or similar functions throughout. The embodiments described below by referring to the drawings are exemplary and are only used to explain the application, and cannot be understood as a limitation of the application.

[0083] In the description of the present application, it should be understood that the terms "longitudinal", "transverse", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer" and the like indicate the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as a limitation of the present application.

[0084] In the description of the present application, unless otherwise specified and limited, it should be noted that the terms "mounting", "connection", "connection" should be understood broadly, for example, it can be mechanical connection or electrical connection, it can be the communication between two elements, it can be direct connection or indirect connection through intermediate medium, and the specific meaning of the above terms can be understood by those skilled in the art according to the specific circumstances.

[0085] In view of the defects that global planning cannot avoid dynamic obstacles and local planning may fall into local optimum, the present application discloses a robot path planning method based on fusion algorithm, the evaluation function is optimized aiming at the slow search speed of traditional A* algorithm; the search neighborhood is optimized aiming at the four-wheel differential robot motion model; a new obstacle avoidance strategy is introduced aiming at the multiple path polyline segments of traditional A* algorithm, the path is optimized by polyline and curve smoothing; aiming at the defects of global and local planning, improved A* algorithm and DWA algorithm are fused, dynamic planning algorithm is applied according to the key node segmentation of global path, and it is ensured that the robot can avoid obstacles in real time. Compared with single static algorithm, the fusion algorithm can pass through random obstacles and reach the destination smoothly.

[0086] As shown in Figure 1 The robot path planning method comprises the following steps:

[0087] Known environment information of a robot workspace is acquired, a grid map type is selected to establish an environment model, a motion coordinate system is established with the robot as the center, and a robot kinematics model is established;

[0088] All nodes in the environment model are initialized, the position of the starting node, the target node and the current node of the robot in the environment model is determined;

[0089] A safe and reliable path from the starting node to the target node is planned by using the optimized A* algorithm;

[0090] The path is optimized by using one-fold line optimization and two-fold line optimization methods, and then is smoothed, and linear interpolation, piecewise cubic interpolation, spline interpolation and cubic spline interpolation can be used for smoothing;

[0091] Based on the robot kinematics model, a robot motion trajectory space is created, and an evaluation function is introduced to score the motion trajectory space, and the trajectory with the highest score is selected from the motion trajectory space as the optimal trajectory output.

[0092] In one preferred embodiment of the present application, as shown in Figure 2 The method for establishing the robot kinematics model is as follows:

[0093] Taking a robot (such as a differential robot, subjected to non-omnidirectional constraint) as the center, the linear velocity v of the robot is positive when the direction of the linear velocity v is the same as the direction of the x-axis, and is negative when the direction of the linear velocity v is opposite to the direction of the x-axis; when the robot rotates counterclockwise, the angular velocity ω is positive, and vice versa; the velocity information of the robot is [v ω] T , and the pose information of the robot is [X Y θ] T X is the horizontal coordinate, and Y is the vertical coordinate; by controlling the speed difference Δv of the driving wheel, turning and steering actions with different radii can be realized.

[0094] Since v = ω × r, the angular velocity of the robot can be expressed as:

[0095]

[0096] The angular velocity can be further expressed as:

[0097] ω = (v r -v l ) / L

[0098] Since v = (v r +v l ) / 2, the steering radius R c can be expressed as:

[0099]

[0100] Suppose that the velocity of the robot at time t is v, the left wheel speed is v l , the right wheel speed is v r , the angular velocity is ω, and the velocity information is [v ω] T , then the velocity information of the robot at time t is expressed as:

[0101]

[0102] Suppose that the pose information of the robot at time t is The motion time of the robot is Δt, and the pose information of the robot at the current time is represented as:

[0103]

[0104] wherein T represents a transpose function, and θ is an angle value between the head of the robot and the Y axis.

[0105] In a preferred scheme of the present application, the estimation function of the A* algorithm is:

[0106] The robot used in the present embodiment is a four-wheel differential robot, and therefore the estimation function used is Euclidean distance, and the search speed of the A* algorithm is changed by modifying the weight of the estimation function, that is, a coefficient W is added before the estimation function, so as to change the weight of the estimation function, and the evaluation function is specifically as follows:

[0107] F(n) = G(n) + W x H(n)

[0108] wherein F(n) represents the comprehensive priority of the node n, G(n) represents the generation value from the starting point to the node n, H(n) represents the estimated cost value of the node n to the target point (which can be but is not limited to Euclidean distance), and W is a manually set coefficient, and the purpose is to accelerate the search speed of the algorithm, and when the estimated cost value of the current position of the robot to the end point is greater than half of the estimated cost value of the starting point to the end point, a larger value, such as 50, is used; otherwise, a smaller value, such as 10, is used.

[0109] In a preferred scheme of the present application, the method for planning a safe and reliable path from the starting node to the target node is as follows:

[0110] The robot motion model used in the present application uses eight directions or six directions; by processing the adjacent nodes, it is ensured that in a specific case, the path in contact with the obstacle is avoided. This helps to improve the path planning, avoid collision, and improve the safety and feasibility of the path.

[0111] All nodes are initialized, and it is determined whether the node can be expanded, and if the node can be expanded, the node is added to array A;

[0112] For the nodes in array A, the adjacent nodes of 50% of the nodes after sorting are screened according to the advancing index, if the node is directly above or directly below the current node and is not in the closed list, the node is added to array B; if the adjacent node is directly to the left or directly to the right of the current node and is not in the closed list, the node is added to array C, indicating that there is another path that can be selected for the current node;

[0113] The nodes added to arrays B and C are deleted from array A, indicating that the nodes will not be selected in the path;

[0114] The remaining nodes in the array A are screened and sorted first and second adjacent nodes, if two nodes are in the front left or right of the current node, the one in front is selected and added to the output to the closed list, and the other is discarded, if only one of the two nodes is in the front left or right, and the other node is in the front left or right of the current node, both nodes are added to the closed list to obtain a safe and reliable path.

[0115] In a preferred scheme of the application, the method for judging whether the node can be expanded is:

[0116] The offset angle of the search direction and the final destination, the obstacles in the next step area, and the obstacle blocking area are obtained, and the search direction travel index is determined:

[0117]

[0118] Wherein, i is the search direction number, i=2, 3, ……8, β i is the search index of the i-th search direction, θ i is the angle between the i-th search direction and the destination (the current position is the origin, the 0 degree direction of the destination, and the angle of the search direction is obtained by counterclockwise rotation), α i is whether there is an obstacle in the i-th search direction, 0 is with obstacle, 1 is without obstacle, υ i is the form speed of the i-th search direction obstacle (the same direction as the search direction is positive, and the angle is greater than 90 degrees is negative), s i is the footprint area of the i-th search direction obstacle, t i is the estimated time from the current position to the i-th search direction obstacle, s is the standard area of the search area, and l is the search step.

[0119] In a preferred scheme of the application, as shown in Figure 3 The path optimization method is as follows:

[0120] The steps of the one-line optimization are as follows:

[0121] All nodes under the path are traversed, and the angle between the nodes is judged;

[0122] When the nodes are on the same straight line, the parent node is updated to the child node to achieve the purpose of removing redundant nodes;

[0123] When the nodes are not on the straight line, it is judged whether there is an obstacle between the nodes and the parent node:

[0124] If there is an obstacle, the parent node is not updated;

[0125] If there is no obstacle, the parent node is updated as the child node;

[0126] The steps of the quadratic polyline optimization are as follows:

[0127] The current node, the child node and the child node of the child node are taken out, and the nodes on the connecting line between the current point and the child node are calculated;

[0128] The nodes on the connecting line are traversed, and it is judged whether there is an obstacle between the node and the current point:

[0129] If there is a connectable node, it is added to the new path;

[0130] If there is no connectable node, the child node is added to the new path.

[0131] In a preferred scheme of the present application, as shown in the figure, the piecewise cubic interpolation method is used for the curve smoothing processing step of the path as follows: Figure 4

[0132] The path nodes are sorted in the order of the starting point-end point, the interpolation between each node is calculated, the array h is obtained, the reciprocal and the cubic polynomial coefficients a, b, c and d corresponding to each node are calculated, the interval is determined according to the given interpolation point, the interpolation result is obtained according to the obtained polynomial coefficients.

[0133] The piecewise cubic interpolation divides the data into multiple adjacent intervals, and a cubic polynomial is used on each interval to approximate the curve:

[0134] y(x)=a(x-x i ) 3 +b(x-x i ) 2 +c(x-x i )+d

[0135] Where x and x i are adjacent points, a, b, c and d are coefficients in the polynomial; y(x) is the estimated value of the point x after curve approximation.

[0136] In a preferred scheme of the present application, in order to enable the robot to avoid the dynamic obstacle appearing in the process of movement, it is necessary to use a dynamic planning algorithm on the basis of a static planning algorithm. By dividing the movement space of the robot into different windows and selecting the best window according to the current environment and the state of the robot, safe and efficient movement is realized, and the navigation problem of the robot in the dynamic environment is solved.

[0137] The method for creating the movement trajectory space of the robot is as follows: ​

[0138] According to the kinematic model of the robot, the motion trajectory space of the robot is created as:

[0139]

[0140] wherein, is the pose information of the robot at time t, the motion time is Δt; the speed of the robot at time t is v, the angular velocity is ω, the speed information is [v ω] T ;

[0141] The speed space of the robot is established, and the robot is constrained by the maximum linear speed, the maximum angular speed, the minimum linear speed and the minimum angular speed of itself:

[0142] V A ={(v,ω)∣v∈[v MIN ,v MAX ]∩ω∈[ω MIN ,ω MAX ]}

[0143] wherein, [v MIN ,v MAX ] represents the maximum linear speed and the minimum linear speed of the robot itself; [ω MIN ,ω MAX ] represents the maximum angular speed and the minimum angular speed of the robot itself;

[0144] The robot is constrained by the motor performance:

[0145]

[0146] wherein, v1 and ω1 represent the linear speed and the angular speed at the present time; and represent the maximum deceleration and the maximum deceleration that the linear speed can reach under the influence of the motor; and represent the maximum acceleration and the maximum deceleration that the angular speed can reach under the influence of the motor;

[0147] The robot is constrained by the obstacle:

[0148]

[0149] wherein, dist(v,ω) represents the closest distance of the trajectory corresponding to the speed space to the obstacle;

[0150] The speed space should satisfy the above three constraint conditions at the same time. With the change of the linear acceleration and the angular acceleration of the motor, the speed space will dynamically change, which is called dynamic window. In the case of satisfying the constraint conditions, the speed space is sampled to obtain the corresponding trajectory space.

[0151] In a preferred scheme of the present application, the trajectory evaluation function G(v, omega) is:

[0152] G(v, omega) = delta * (alpha * Heading(v, omega) + beta * Dist(v, omega) + lambda * Vel(v, omega))

[0153] wherein Heading(v, omega) is a heading angle evaluation function, evaluating the angle gap between the end of the trajectory and the target point at the current set speed of the robot; Dist(v, omega) is a distance evaluation function, evaluating the distance between the robot at the end of the predicted trajectory and the nearest obstacle on the map; Vel(v, omega) is a linear speed evaluation function, evaluating the linear speed value of the robot at the current time; and delta, alpha, beta and lambda are weights.

[0154] The technical scheme fuses the A* algorithm and the DWA algorithm, forms a new path planning algorithm combining static planning and dynamic planning together. The algorithm firstly obtains a static path planning through the optimized A* algorithm, thereby obtaining key nodes of the whole path; and then performs dynamic path planning between the key nodes through the DWA algorithm, ensures that the robot can avoid obstacles on the whole path in real time, and thereby successfully reaches the set end point.

[0155] The present application also provides a robot path planning system based on the fusion algorithm, comprising a processing module, the processing module being installed on the robot, and the processing module performing the method of the present application to plan the path of the robot. The system proposes improvement methods such as evaluation function optimization, search neighborhood optimization, obstacle avoidance strategy optimization, path polyline optimization and path curve smoothing, and the improved static planning algorithm can maximally avoid the above-mentioned defects and obtain a perfect global path.

[0156] In the description of the present specification, the description of the terms "one embodiment", "some embodiments", "an example", "a specific example", or "some examples" and the like means that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. In the present specification, the illustrative description of the above terms does not necessarily mean the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0157] Although the embodiments of the present application have been shown and described, those skilled in the art can understand that various changes, modifications, replacements and variations can be made to the embodiments without departing from the principles and spirit of the present application, and the scope of the present application is defined by the claims and their equivalents.

Claims

1. A robot path planning method based on a fusion algorithm, characterized in that, Includes the following steps: Acquire known environmental information of the robot's workspace, select grid map type to build environmental model, establish motion coordinate system with robot as the center, and build robot kinematic model; Initialize all nodes within the environment model, and determine the starting node, target node, and current node position of the robot within the environment model; Using the optimized A* algorithm, plan a safe and reliable path from the starting node to the target node; The path is optimized using a first-order and second-order polyline optimization method, and then smoothed. Based on the robot's kinematics model, a motion trajectory space for the robot is created, and an evaluation function is introduced to score the motion trajectory space. The trajectory with the highest score in the motion trajectory space is selected as the optimal trajectory output. The method for planning a safe and reliable path from the starting node to the target node is as follows: Initialize all nodes and determine if they can be expanded. If they can be expanded, add them to array A. For nodes in array A, sort them according to their movement index, and filter the top 50% of adjacent nodes after sorting. If an adjacent node is directly above or below the current node and is not in the closed list, add it to array B; if an adjacent node is directly to the left or right of the current node and is not in the closed list, add it to array C, indicating that there are other paths to choose from for the current node. The node added to arrays B and C is removed from array A, indicating that it will not be selected in the path; For the remaining nodes in array A, filter the adjacent nodes that are sorted first and second. If the two nodes are to the left or right front of the current node, select the one sorted first and add it to the closed list, and discard the other. If only one of the two nodes is to the left or right front, and the other node is directly to the left or right of the current node, add both nodes to the closed list to obtain a safe and reliable path. The method for determining whether a node can be expanded is as follows: Obtain the offset angle between the search direction and the final destination, the obstacles in the next movement area, and the obstruction area of ​​the obstacles, and determine the movement index of the search direction: ， Where i is the search direction index, i = 1, 2, 3, ... 8, Let be the search index for the i-th search direction. Let be the angle between the i-th search direction and the destination. To determine whether there are obstacles in the i-th search direction, 0 indicates there are obstacles, and 1 indicates there are no obstacles. Let be the speed of the obstacle in the i-th search direction. Let be the area occupied by the obstacle in the i-th search direction. Let s be the estimated time to travel from the current position to the obstacle in the i-th search direction, and s be the standard area of ​​the search area. This is the search step size.

2. The robot path planning method based on fusion algorithm as described in claim 1, characterized in that, The method for establishing a robot kinematic model is as follows: Centered on the robot, the robot's linear velocity When the direction is the same as the x-axis, it is positive; when it is opposite, it is negative. When rotating counterclockwise, the angular velocity... Positive values ​​indicate speed, while negative values ​​indicate speed; the robot's speed information is... The pose information is X is the horizontal axis, Y is the vertical axis, and θ is the angle between the robot's head and the Y-axis. set up The robot's speed at that moment is The speed of the left wheel is The speed of the right wheel is angular velocity is Speed ​​information is ,but The robot's speed information at that moment is represented as follows: ， Let the robot's pose information at time t be... Its movement time is Then, at the current moment, the robot's pose information is represented as: ， Where T represents the transpose function.

3. The robot path planning method based on a fusion algorithm as described in claim 1, characterized in that, The estimation function of the A* algorithm is: ， in, Represents a node Overall priority; Indicates the distance from the starting point to the node. The value of the time, Represents a node The estimated cost to the target point, where W is a manually set coefficient.

4. The robot path planning method based on fusion algorithm as described in claim 1, characterized in that, The steps for smoothing the path using piecewise cubic interpolation are as follows: Piecewise cubic interpolation divides the data into multiple adjacent intervals, and uses a cubic polynomial to approximate the curve in each interval: ， in, and Let a, b, c, and d be adjacent points, and let a, b, c, and d be the coefficients of the polynomial, respectively. Let x be the estimated value after approximation by the curve.

5. The robot path planning method based on a fusion algorithm as described in claim 1, characterized in that, The method for creating the robot's motion trajectory space is as follows: Based on the robot's kinematics model, the robot's motion trajectory space is created as follows: ， in, Let be the robot's pose information at time t, and let be the motion time. ; The robot's speed at that moment is angular velocity is Speed ​​information is X is the horizontal axis, Y is the vertical axis, and θ is the angle between the robot's head and the Y-axis. Establish the robot's velocity space, which is constrained by the robot's maximum linear velocity, maximum angular velocity, minimum linear velocity, and minimum angular velocity: ， in, This represents the robot's maximum and minimum linear velocities. This represents the robot's maximum and minimum angular velocities. Robots are constrained by the performance of their motors: ， in, and This represents the linear velocity and angular velocity at the current moment; and This indicates the maximum acceleration and maximum deceleration that can be achieved under the influence of the motor's linear velocity; and This indicates the maximum acceleration and maximum deceleration that the angular velocity can achieve under the influence of the motor; Robots are constrained by obstacles: ， in, This represents the closest distance between the trajectory corresponding to the obstacle in velocity space; Under the condition that the constraints are met, the velocity space is sampled to obtain the corresponding trajectory space.

6. The robot path planning method based on a fusion algorithm as described in claim 5, characterized in that, Trajectory evaluation function for: ， in, This is a direction angle evaluation function that evaluates the angular difference between the end of the robot's trajectory and the target point at the current set speed. This is a distance evaluation function that evaluates the distance between the robot and the nearest obstacle on the map when the robot is at the end point of the predicted trajectory. This is the linear velocity evaluation function, which evaluates the robot's linear velocity value at the current moment. , , , As weight.

7. A robot path planning system based on a fusion algorithm, characterized in that, The system includes a processing module mounted on the robot, which executes the method described in any one of claims 1-6 to plan the robot path.

Images