Unmanned aerial vehicle path planning method based on RRT improved algorithm

Abstract

The invention discloses an unmanned aerial vehicle path planning method based on an RRT improved algorithm. The method comprises the following steps: establishing a four-rotor unmanned aerial vehicle mathematical model; determining a working space; establishing an ellipsoid sampling domain; setting a sampling strategy; sampling by adopting a Gaussian sampling method; expanding by adopting an artificial potential field algorithm; obtaining a group of path points and an initial path; and obtaining a final smooth path. According to the method, random sampling is replaced by a Gaussian sampling strategy, and different sampling frequencies are selected according to different obstacle densities; a sampling domain is limited, sampling points during sampling are limited in an ellipsoid sampling domain, redundant sampling points are greatly reduced, an adaptive step length and an artificial potential field expansion strategy are added, and a more flexible expansion strategy is made under the condition of facing different obstacle densities; and difference smoothing is carried out on a split-line-shaped initial path generated by using the improved RRT algorithm by using a cubic B-spline interpolation method, so that the efficiency and quality of a final path are improved, and the generated final path is enabled to better conform to kinematics and dynamics characteristics of the unmanned aerial vehicle.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) technology, and specifically relates to a UAV path planning method based on an improved RRT (Rapidly-exploring RandomTree) algorithm. Background Technology

[0002] With the rapid development of drone technology, path planning has become a core technological pillar for achieving autonomous flight. Whether performing logistics delivery, aerial surveying and mapping, agricultural plant protection, or search and rescue and inspection missions, drones must plan a flight path within a specific three-dimensional workspace to safely and efficiently reach the target point. This path is not only the blueprint for mission execution but also a crucial bridge connecting commands and actions. An optimized path can maximize the drone's ability to avoid static obstacles such as buildings and trees, as well as dynamic threats such as other aircraft, ensuring its own safety and mission reliability. Simultaneously, it can significantly improve execution efficiency by reducing unnecessary flight distances and time, directly lowering energy consumption, and extending endurance, which is crucial for cost control in commercial applications.

[0003] However, when faced with complex and ever-changing 3D real-world environments, especially in cities, dense forests, or indoor warehouses filled with high-density, irregular obstacles, traditional path planning algorithms based on random sampling, such as the RRT algorithm, reveal significant limitations. The core problem lies in the algorithm's reliance on uniform random sampling throughout the entire space. This "aimless" search method easily generates a large number of invalid sampling points and tortuous branching in vast and complex spaces. These invalid explorations not only waste valuable computational resources, leading to prolonged planning time, but also produce many illogical, redundant, or even unflyable paths, severely impacting the algorithm's real-time performance and practicality, making it difficult to meet the stringent requirements of UAVs for online real-time replanning.

[0004] Therefore, in-depth improvement and innovation of traditional sampling algorithms in complex 3D environments has become a major research topic for promoting the intelligent upgrade of UAVs. By introducing heuristic information, such as target-biased sampling, incorporating environmental semantics, optimizing probabilistic sampling strategies, combining machine learning to predict key regions, and strengthening post-path optimization, it is expected that the search efficiency and path quality of sampling algorithms can be greatly improved. This research not only has important theoretical value, but also lays a solid technical foundation for the widespread application of UAVs, enabling them to perform better in more complex scenarios. Summary of the Invention

[0005] To address the aforementioned problems, the present invention aims to provide a UAV path planning method based on an improved RRT algorithm.

[0006] To achieve the above objectives, the UAV path planning method based on the improved RRT algorithm includes the following steps performed in sequence:

[0007] Step 1: Based on the shape, kinematics, and dynamics of the quadrotor UAV, establish a mathematical model of the quadrotor UAV, including a dynamic model, an attitude model, and a position model;

[0008] Step 2: Determine the 3D workspace for simulating the path planning of a quadcopter UAV using the improved RRT algorithm, and determine the starting point q. start And the target point is q goal Then, a queue is created to store the path points obtained by expanding along the sampling direction, and then the starting point q is... start Place it in the queue, and simultaneously import the quadcopter UAV mathematical model established in step 1 into the workspace; and set kinematic and dynamic constraints;

[0009] Step 3: Establish the starting point q in the workspace determined in Step 2. start And the target point is q goal The ellipsoidal sampling domain enclosed within the sampling points is used to limit the sampling points to the ellipsoidal sampling domain and determine the extent of the ellipsoidal sampling domain.

[0010] Step 4: Set the sampling point density when sampling in the above ellipsoidal sampling domain using the Gaussian sampling method, and use the Artificial Potential Field (APF) algorithm to limit the expansion direction and step size of the sampling points, thereby determining the sampling strategy;

[0011] Step 5: Following the sampling strategy determined in Step 4, use the Gaussian sampling method in the aforementioned ellipsoidal sampling domain X. ellipsoid From the starting point q start Sampling begins, a pair of sampling points is obtained and obstacle collision detection is performed, and new sampling points are determined based on the detection results;

[0012] Step 6: After obtaining the new sampling points, use the artificial potential field algorithm to adjust the starting point q. start Using the new sampling point as the direction, expand using an adaptive step size strategy to obtain the first path point q1 and put it into the queue;

[0013] Step 7: Repeat steps 5 and 6 to obtain the second sampling point q. sample1 Then the first path point q1 is used with the second sampling point q sample1 The direction is expanded to obtain the second path point q2 and placed in the queue; then sampling is performed in the same way to obtain the third and subsequent sampling points q. sample2 q sample3...and corresponding multiple path points q3, q4... until the last sampling point is the target point q. goal At this point, sampling is complete, and the queue contains a set of path points q. start ,q1,q2······q goal Finally, redundant sampling points are trimmed. Starting from the previous path point, try to connect the current path point with the sampling points obtained from multiple samplings. If the connecting line segment does not collide with any obstacles, it means that all the sampling points in the middle are redundant and they are removed. Repeat this process until the path cannot be shortened further.

[0014] Step 8: Transfer the above set of path points q start ,q1,q2······q goal Connect them sequentially according to the order in which they were placed in the queue to obtain an initial path in the shape of a broken line;

[0015] Step 9: Smooth the initial path obtained in Step 8 using cubic B-spline interpolation to conform to the dynamics and kinematics characteristics of the quadcopter UAV, and obtain the final smooth path.

[0016] In step 1, the dynamic model is as follows:

[0017]

[0018]

[0019] τ z =c M (ω1 2 -ω2 2 +ω3 2 -ω4 2 )

[0020] T = c T (ω1 2 +ω2 2 +ω3 2 +ω4 2 )

[0021] Among them, T m ω is the motor response time constant. m This is the steady-state speed of the motor. Let ω1(t), ω2(t), ω3(t), and ω4(t) be the derivatives of the rotational speeds of the four motors on the quadcopter drone, respectively; σ1(t), σ2(t), σ3(t), and σ4(t) be the throttle values ​​of the four motors, respectively; and τ be the derivatives of the rotational speeds of the four motors on the quadcopter drone. x ,τ y ,τz The torques are the x, y, and z axes, respectively, and d is... The drone's wheelbase, T, is the sum of the lift of the four propeller blades, and c... T c is the thrust coefficient of a single blade. M For a single blade moment coefficient;

[0022] The attitude model is established based on Euler's dynamic equations as follows:

[0023]

[0024] Among them, J xx J yy J zz Let x, y, and z be the moments of inertia of rotation along the x, y, and z axes, respectively. θ, These are the pitch angle, roll angle, and yaw angle of the quadcopter drone. Let be the derivatives of the angular velocities along the x, y, and z axes, respectively. These are the derivatives for pitch, roll, and yaw angles, respectively.

[0025] The location model is as follows:

[0026]

[0027] Among them, v x v y v z The velocities p are the velocities along the x, y, and z axes, respectively. x p y p z The x, y, and z axes represent the positions, g is the acceleration due to gravity, and m is the mass of the quadcopter drone. Let be the derivatives of the velocities along the x, y, and z axes, respectively. These are the derivatives of the positions along the x, y, and z axes, respectively.

[0028] In step 2, the three-dimensional workspace for simulating the path planning of the quadcopter UAV using the improved RRT algorithm is determined, and the starting point q is determined. start And the target point is q goal Then, a queue is created to store the path points obtained by expanding along the sampling direction, and then the starting point q is... start Place it in the queue, and simultaneously import the quadcopter UAV mathematical model established in step 1 into the workspace; and set the kinematic and dynamic constraints as follows:

[0029] In Python, a 3D workspace is set up for path planning simulation of a quadrotor drone, which includes randomly generated obstacles. Then, based on the kinematic characteristics of the drone, kinematic and dynamic constraints, including the turning angle and climb angle, are applied to the quadrotor drone in the workspace. The turning angle of the quadrotor drone from the current path point to the next path point is constrained to change only within a certain range. At the same time, the angle of the quadrotor drone when climbing from the current path point to the next path point is also constrained to change only within a certain range.

[0030] The formula for calculating the steering angle is:

[0031]

[0032] The formula for calculating the climb angle is:

[0033]

[0034] Among them, (x i ,y i ,z i Let be the coordinates of the path points, i = 0, 1, 2, 3, ....

[0035] In step 3, the starting point q is established in the workspace determined in step 2. start And the target point is q goal The method for confining sampling points to the ellipsoidal sampling domain and determining the extent of the ellipsoidal sampling domain is as follows:

[0036] Step 3.1: Define the parameters of the ellipsoidal sampling domain: Define the straight-line distance from the starting point of the path to the target point as C. min The cost of the path is C. best a is the major axis, b is the minor axis, and q c Let be the center of the ellipsoid, with major axis a, minor axis b, and center q of the ellipsoid. c The calculation formula is as follows:

[0037]

[0038] Step 3.2: Based on the major axis a and minor axis b, construct the starting point q as described above. start And the target point is q goal The ellipsoidal sampling domain is enclosed by a direction vector d = q. start -q goal Normalizing the direction vector d yields the normalized direction vector. This direction will be used as the major axis direction of the ellipsoid. Two unit vectors, u2 and u3, orthogonal to the normalized direction vector u1 will be constructed, such that u1, u2, and u3 form a set of orthonormal bases, serving as an orthogonal coordinate system within the ellipsoidal sampling domain. The unit vector u2 is constructed by first selecting an arbitrary vector v that is not parallel to the normalized direction vector u1, and then calculating the unit vector using the following formula. Obtain the unit vector perpendicular to the normalized direction vector u1; then calculate the unit vector u3 = u1 × u2 using the following formula, where the × operator is the cross product operation;

[0039] Step 3.3: Based on the above orthonormal basis, the unit sphere x ball The scaling mapping is applied to an ellipsoidal sampling domain; first, the scaling matrix L is defined:

[0040]

[0041] Then the ellipsoid is Lx ball Therefore, the ellipsoidal sampling domain X ellipsoid for:

[0042] x ellipsoid =Lx ball +q c

[0043] Where, q c It is the center of the ellipsoid.

[0044] In step 4, the density of sampling points is set when sampling in the above-mentioned ellipsoidal sampling domain using the Gaussian sampling method. At the same time, an artificial potential field algorithm is used to limit the expansion direction and step size of the sampling points. The sampling strategy is determined as follows:

[0045] The Gaussian sampling method employs a sampling strategy that increases sampling points in areas of high obstacle density and decreases them in areas of low obstacle density to adjust the sampling point density. The artificial potential field algorithm efficiently restricts the expansion direction and step size of sampling points when facing obstacles, making the sampling points more closely aligned with the target point q. goal The direction.

[0046] In step 5, the sampling strategy determined in step 4 is followed, and the Gaussian sampling method is used in the aforementioned ellipsoidal sampling domain x. eipsoid From the starting point q start Sampling begins, a pair of sampling points is obtained, and obstacle collision detection is performed. The method for determining new sampling points based on the detection results is as follows:

[0047] Step 5.1: In the ellipsoidal sampling domain x ellipsoid Inside, from the starting point q startTo begin, the first sampling point q is randomly generated using the randomsample function in Python. sample ;

[0048] Step 5.2: Using the first sampling point q sample Centered on a Gaussian distribution, a random offset vector Δq = randomnormal(0, σ, dimension) is generated from a Gaussian distribution with a mean of 0 and a standard deviation of σ, where randomnormal is the random normalization function in Python.

[0049] Step 5.3: Based on the first sampling point q mentioned above sample And a second sampling point q is generated using a random offset vector Δq. guass =q sample +Δq;

[0050] Step 5.4: Perform obstacle collision detection. If the first sampling point q sample and the second sampling point q guass If both are in free space, one of them is randomly selected as the new sampling point; if one falls in free space and the other falls in an obstacle, the sampling point in free space is selected; if both fall in an obstacle, the pair of sampling points is discarded and sampling is performed again.

[0051] In step 6, after obtaining the new sampling point, the artificial potential field algorithm is used to adjust the starting point q. start The method for expanding the path using the new sampling point as the direction and an adaptive step size strategy to obtain the first path point q1 and add it to the queue is as follows:

[0052] Step 6.1: Guide the expansion direction using the artificial potential field algorithm. During sampling, use the APF expansion strategy for iterative optimization. For each path point on the path, except for the starting and target points, calculate the gravitational field function U according to the following formula. altt (q); then the repulsive field function U is calculated according to the following formula. rep (q) Iterate through all obstacles and calculate the distance from the current sampling point to each obstacle. If the distance is less than the range ρ0 of the repulsive field, generate a repulsive vector from the obstacle to the current sampling point. Then, calculate the negative gradients of the obtained gravitational and repulsive field functions to obtain the gravitational and repulsive functions. Finally, vector synthesize all the gravitational and repulsive forces acting on the current sampling point to obtain the total potential force U. total (q);

[0053] The formulas for calculating the gravitational field function and the repulsive field function are as follows:

[0054]

[0055] U total (q)=U att (q)+U rep (q);

[0056] Among them, U att (q) represents the gravitational potential energy at position q, ξ represents the gravitational coefficient gain, and ρ(q, q) represents the gravitational potential energy at position q. qoal () represents the distance from position q to target point q. goal Euclidean distance; U rep (q) represents the repulsive potential energy at position q, η represents the repulsive coefficient gain, and ρ(q, q) represents the repulsive potential energy at position q. obs () represents the distance from position q to obstacle q. obs The closest distance, ρ0 is the effective range of the repulsive field, which defines the distance at which an obstacle will exert a repulsive force on the sampling point. An obstacle will only exert a repulsive force if it enters this range. U total (q) is the sum of gravitational potential energy and repulsive potential energy;

[0057] The formulas for calculating the gravitational and repulsive functions are as follows:

[0058]

[0059]

[0060] F total (q)=F att (q)+F rep (q);

[0061] in, For gradient calculation, F att (q) is the gravitational function, F rep (q) is the repulsive force function, F total (q) represents the total potential force;

[0062] Step 6.2: Adjust the sampling point position: Based on the total potential force F mentioned above total The current sampling point is moved a small step along the direction of (q) to obtain the first path point q1; the line connecting the first path point q1 and the next sampling point is denoted as L1, and the first path point q1 and the target point q are... goal Let L2 be the line connecting L1 and L2, and let ω be the angle between them. The step size after adaptive expansion is calculated using the following formula:

[0063]

[0064] In step 9, the initial path obtained in step 8 is smoothed using cubic B-spline interpolation to conform to the dynamics and kinematics characteristics of the quadcopter UAV. The method for obtaining the final smooth path is as follows:

[0065] Step 9.1: Transfer the above set of path points q start ,q1,q2······q goal As input for cubic B-spline interpolation;

[0066] Step 9.2: Construct the node vector and control points, which determine the domain and shape basis of the cubic B-spline curve; define the node vector. For a cubic B-spline curve, its node vector U is a non-decreasing sequence of parameters; the node spacing of a uniform cubic B-spline curve is equal. The node vector of a typical cubic uniform B-spline curve defined by m+1 control points can be expressed as: U=[u0, u1, u2, ..., u m+1 ] = [0, 0, 0, 0, 1, 2, 3, ..., m-3, m-2, m-1, m], control point L j Not the original path point q j Where j = 1, 2, 3, ..., the curve fitting method is adopted, and the path points in the initial path are regarded as the target points to be fitted by the cubic B-spline curve. The least squares method is used to solve a set of optimal control points in reverse, and a smooth curve that is closest to all path points in the initial path in the least squares sense is generated.

[0067] Step 9.3: Calculate the basis functions of the cubic B-spline curve: The shape of the cubic B-spline curve is determined by the control points and a set of basis functions; the basis functions are piecewise polynomials that define the control points L. j The influence weights of a node on a cubic B-spline curve are calculated using the Cox-deBox recursive formula for these basis functions;

[0068] Step 9.4: Calculate the path of the cubic B-spline curve: Uniformly sample within the effective interval formed by the node vector U. For each sampled node u... sample The corresponding points on the smoothed path are calculated according to the following formula. By calculating all nodes u sample The corresponding point S(u) is used to obtain a series of dense point sets that define smooth paths;

[0069] Step 9.5: Connect the corresponding points S(u) obtained in the previous step in order to obtain the final smooth path.

[0070] The UAV path planning method based on the improved RRT algorithm provided by this invention has the following beneficial effects: First, it replaces random sampling with a Gaussian sampling strategy, that is, the probability density function of obstacle distribution in the workspace is equivalent to a Gaussian function, and different sampling frequencies are selected according to different obstacle densities during sampling. Second, it restricts the sampling domain, limiting the sampling points to an ellipsoidal sampling domain. This improvement can greatly reduce redundant sampling points. Then, it incorporates adaptive step size and artificial potential field expansion strategies to make more flexible expansion strategies when facing different obstacle densities. Finally, it uses cubic B-spline interpolation to smooth the difference in the initial path generated by the improved RRT algorithm, which improves the efficiency and quality of the final path, making the generated final path more consistent with the kinematic and dynamic characteristics of the quadcopter UAV. Attached Figure Description

[0071] Figure 1 This is a schematic diagram of a quadcopter drone model in this invention.

[0072] Figure 2 The flowchart of the UAV path planning method based on the improved RRT algorithm provided by this invention is shown.

[0073] Figure 3 This is a cross-sectional view of the ellipsoidal sampling domain in this invention.

[0074] Figure 4 This is a side view of the final experimental results in this invention.

[0075] Figure 5 This is a top view of the final experimental results in this invention. Detailed Implementation

[0076] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0077] like Figure 2 As shown, the UAV path planning method based on the improved RRT algorithm provided by this invention includes the following steps performed in sequence:

[0078] Step 1: According to Figure 1 Based on the shape, kinematics, and dynamics of the quadrotor UAV shown, a mathematical model of the quadrotor UAV, including a dynamic model, an attitude model, and a position model, is established.

[0079] The dynamic model is as follows:

[0080]

[0081] τ z =c M (ω1 2 -ω2 2 +ω32 -ω4 2 )

[0082] T = c T (ω1 2 +ω2 2 +ω3 2 +ω4 2 )

[0083] Among them, T m ω is the motor response time constant. m This is the steady-state speed of the motor. Let ω1(t), ω2(t), ω3(t), and ω4(t) be the derivatives of the rotational speeds of the four motors on the quadcopter drone, respectively; σ1(t), σ2(t), σ3(t), and σ4(t) be the throttle values ​​of the four motors, respectively; and τ be the derivatives of the rotational speeds of the four motors on the quadcopter drone. x ,τ y ,τ z These are the torques along the x, y, and z axes, respectively, with d being... The drone's wheelbase, T, is the sum of the lift of the four propeller blades, and c... T c is the thrust coefficient of a single blade. M For a single blade moment coefficient;

[0084] The attitude model is established based on Euler's dynamic equations as follows:

[0085]

[0086] Among them, J xx J yy J zz These are the moments of inertia along the x, y, and z axes, respectively. θ, These are the pitch angle, roll angle, and yaw angle of the quadcopter drone. Let be the derivatives of the angular velocities along the x, y, and z axes, respectively. These are the derivatives for pitch, roll, and yaw angles, respectively.

[0087] The location model is as follows:

[0088]

[0089]

[0090] Where, v x ,v y ,v z The velocities p are the velocities along the x, y, and z axes, respectively. x ,p y ,p zThe x, y, and z axes represent the positions, g is the acceleration due to gravity, and m is the mass of the quadcopter drone. Let be the derivatives of the velocities along the x, y, and z axes, respectively. These are the derivatives of the positions along the x, y, and z axes, respectively.

[0091] Step 2: Determine the 3D workspace for simulating the path planning of a quadcopter UAV using the improved RRT algorithm, and determine the starting point q. start And the target point is q goal Then, a queue is created to store the path points obtained by expanding along the sampling direction, and then the starting point q is... start Place it in the queue, and simultaneously import the quadcopter UAV mathematical model established in step 1 into the workspace; and set kinematic and dynamic constraints;

[0092] The method is as follows:

[0093] The workspace refers to the real space in which path planning for a quadrotor drone is performed. A three-dimensional workspace for simulating quadrotor drone path planning is set up in Python, containing randomly generated obstacles. Then, based on the drone's kinematic characteristics, kinematic and dynamic constraints, including turning and climb angles, are applied to the quadrotor drone within this workspace. The turning angle of the quadrotor drone from the current path point to the next path point is constrained to change only within a certain range, not indefinitely, thus limiting the turning angle. This constraint essentially limits the minimum turning radius of the quadrotor drone. Simultaneously, the angle of the quadrotor drone's climb from the current path point to the next path point is also constrained to change only within a certain range, thus limiting the turning angle.

[0094] The formula for calculating the steering angle is:

[0095]

[0096] The formula for calculating the climb angle is:

[0097]

[0098] Among them, (x i ,y i ,z i ) represents the coordinates of the path point, i = 0, 1, 2, 3, ...; In this invention, the turning angle and the climb angle are limited to 15° to 45° per second and 15° to 35° per second, respectively.

[0099] Step 3: Establish the starting point q in the workspace determined in Step 2. start And the target point is q goalThe ellipsoidal sampling domain enclosed within the sampling points is used to limit the sampling points to the ellipsoidal sampling domain and determine the extent of the ellipsoidal sampling domain.

[0100] The ellipsoidal sampling domain strategy refers to a method where the entire sampling domain dynamically adjusts its range as the RRT algorithm expands towards the target point, generally existing within a continuously shrinking ellipsoidal sampling domain. Initially, the ellipsoidal sampling domain is relatively large, with relatively generous lengths for the major and minor axes to ensure sufficient global exploration capability for the RRT algorithm and avoid getting trapped in local minima. As the iteration progresses and the RRT algorithm gradually expands towards the target point and gets closer, it intelligently shrinks the ellipsoidal sampling domain, specifically by gradually reducing the lengths of the major and minor axes. This dynamic shrinkage mechanism greatly optimizes the sampling process: on the one hand, it shifts the sampling point from extensive exploration to refined exploration near the potential optimal path region, significantly reducing invalid sampling in already explored or irrelevant areas; on the other hand, shrinking the ellipsoidal sampling domain increases the convergence speed towards the target point and improves the planning accuracy and search efficiency in local areas. Through this adaptive adjustment strategy, not only is the completeness of global exploration guaranteed in the initial stage, but the targeting and convergence speed of local development are also improved in the later stages, thus achieving synergistic optimization of planning efficiency and path quality.

[0101] The method is as follows:

[0102] Step 3.1: Define the parameters of the ellipsoidal sampling domain: such as Figure 3 As shown, the straight-line distance from the starting point of the path to the target point is defined as C. min The cost of the path is C. best a is the major axis, b is the minor axis, and q c Let be the center of the ellipsoid, with major axis a, minor axis b, and center q of the ellipsoid. c The calculation formula is as follows:

[0103]

[0104] Step 3.2: Based on the major axis a and minor axis b, construct the starting point q as described above. start And the target point is q goal The ellipsoidal sampling domain is enclosed by a direction vector d = q. start -q goal Normalizing the direction vector d yields the normalized direction vector. This direction will be used as the major axis direction of the ellipsoid. Two unit vectors, u2 and u3, orthogonal to the normalized direction vector u1 will be constructed, such that u1, u2, and u3 form a set of orthonormal bases, serving as an orthogonal coordinate system within the ellipsoidal sampling domain. The unit vector u2 is constructed by first selecting an arbitrary vector v that is not parallel to the normalized direction vector u1, and then calculating the unit vector using the following formula. Obtain the unit vector perpendicular to the normalized direction vector u1; then calculate the unit vector u3 = u1 × u2 using the following formula, where the × operator is the cross product operation;

[0105] Step 3.3: In order to sample uniformly within the ellipsoidal sampling domain, based on the above-mentioned orthonormal basis, the unit sphere x ball The scaling mapping is applied to an ellipsoidal sampling domain; first, the scaling matrix L is defined:

[0106]

[0107] Then the ellipsoid is Lx ball Therefore, the ellipsoidal sampling domain x ellipsoid for:

[0108] x ellipsoid =Lx ball +q c

[0109] Where, q c It is the center of the ellipsoid.

[0110] Step 4: Set the sampling point density when sampling in the above ellipsoidal sampling domain using the Gaussian sampling method, and use the Artificial Potential Field (APF) algorithm to limit the expansion direction and step size of the sampling points, thereby determining the sampling strategy;

[0111] The Gaussian sampling method employs a sampling strategy that increases sampling points in areas of high obstacle density and decreases them in areas of low obstacle density. This adjusts the sampling point density, enabling more effective sampling in obstacle environments with varying densities. It increases the sampling density near obstacle boundaries, essentially constraining the sampling probability to follow a Gaussian distribution, making it easier to find the most efficient path in narrow, obstacle-rich spaces. The artificial potential field algorithm possesses strong heuristic properties, efficiently limiting the expansion direction and step size of sampling points when facing obstacles, making the sampling points more closely aligned with the target point q. goal The direction;

[0112] Step 5: Following the sampling strategy determined in Step 4, use the Gaussian sampling method in the aforementioned ellipsoidal sampling domain x. eipsoid From the starting point q start Sampling begins, a pair of sampling points is obtained and obstacle collision detection is performed, and new sampling points are determined based on the detection results;

[0113] The method is as follows:

[0114] Step 5.1: In the ellipsoidal sampling domain x ellipsoid Inside, from the starting point q start To begin, the first sampling point q is randomly generated using the randomsample function in Python. sample ;

[0115] Step 5.2: Using the first sampling point q sample Centered on a Gaussian distribution, a random offset vector Δq = randomnormal(0, σ, dimension) is generated from a Gaussian distribution with a mean of 0 and a standard deviation of σ, where randomnormal is the random normalization function in Python.

[0116] Step 5.3: Based on the first sampling point q mentioned above sample And a second sampling point q is generated using a random offset vector Δq. guass =q sample +Δq;

[0117] Step 5.4: Perform obstacle collision detection. If the first sampling point q sample and the second sampling point q guass If both are in free space, one of them is randomly selected as a new sampling point; if one falls in free space and the other falls in an obstacle, the sampling point in free space is selected; if both fall in an obstacle, this pair of sampling points is discarded and sampling is performed again; in addition, sampling points that are closer to obstacles are suitable for fine-grained searching, in which case a smaller standard deviation σ needs to be set, while sampling points that are more dispersed are suitable for large-scale exploration, in which case a larger standard deviation σ needs to be set. The standard deviation σ is usually set to 5%-15% of the environmental size.

[0118] Step 6: After obtaining the new sampling points, use the artificial potential field algorithm to adjust the starting point q. start Using the new sampling point as the direction, expand using an adaptive step size strategy to obtain the first path point q1 and put it into the queue;

[0119] In path planning algorithms, the step size has a crucial impact on search efficiency and the quality of the final path. While a fixed and excessively small step size can achieve high path accuracy, it leads to a large number of redundant nodes in vast areas with few or no obstacles, significantly increasing unnecessary computational overhead and reducing the algorithm's scalability. To address scenarios with high-density workspaces or high real-time requirements, this invention introduces an adaptive step size adjustment mechanism. This is an efficient scalability strategy whose core idea is to give the search process environmental awareness, dynamically adjusting the scalability step size based on the complexity of the local space. For example, in complex areas with dense obstacles and narrow passages, the algorithm automatically reduces the step size for more refined and cautious exploration. This refined scalability effectively avoids collisions with obstacles or missing narrow feasible paths due to excessively large step sizes, thus ensuring the feasibility and safety of the path. Conversely, in free areas with sparse obstacles or open spaces, the step size is significantly increased, allowing the algorithm to move towards the target direction more quickly, greatly reducing the number of path points generated in that area, thereby accelerating spatial coverage and improving overall search efficiency.

[0120] To better facilitate this adaptive adjustment, this invention introduces an artificial potential field algorithm for guiding the sampling direction and an adaptive step size expansion strategy. This algorithm constructs a virtual potential field, superimposed from a repulsive field (typically generated by obstacles) and a gravitational field (generated by the target point), providing a directional gradient for path planning. The adaptive direction guidance strategy can be combined with the potential field strength. In regions with strong repulsive fields (i.e., near obstacles), the potential field changes drastically, indicating a complex environment; in this case, the step size should be reduced to ensure safety. In regions with weak repulsive fields and strong gravitational fields, the potential field changes smoothly, indicating open space ahead; in this case, the step size can be safely increased. Applying the adaptive sampling direction expansion strategy of the artificial potential field algorithm to step size expansion forms a search method that indirectly senses the environment and intelligently adjusts. Instead of simply searching the workspace uniformly, it optimizes the allocation of computational resources, focusing more refined exploration on complex and dangerous areas while rapidly traversing simple and safe areas. This differentiated approach significantly improves the expansion efficiency and convergence speed of path planning while ensuring path quality, making the algorithm more flexible, efficient, and practical.

[0121] The method is as follows:

[0122] Step 6.1: Guide the expansion direction using the artificial potential field algorithm. During sampling, use the APF expansion strategy for iterative optimization. For each path point on the path, except for the starting and target points, calculate the gravitational field function U according to the following formula. att(q) is a vector pointing from the current sampling point to the target point, and its magnitude is proportional to the distance; gravity is responsible for pulling the path toward the target point; then the repulsive field function U is calculated according to the following formula. rep (q) Iterate through all obstacles and calculate the distance from the current sampling point to each obstacle. If the distance is less than the range ρ0 of the repulsive field, a repulsive vector is generated from the obstacle to the current sampling point, the magnitude of which is inversely proportional to the square of the distance. The repulsive force is responsible for pushing the path away from the obstacles. Then, the negative gradients of the obtained gravitational and repulsive field functions are calculated to obtain the gravitational and repulsive functions. Finally, all the gravitational and repulsive forces acting on the current sampling point are vectored together to obtain the total potential force U. total (q);

[0123] The formulas for calculating the gravitational field function and the repulsive field function are as follows:

[0124]

[0125] U total (q)=U att (q)+U rep (q)

[0126] Among them, U att (q) represents the gravitational potential energy at position q, ξ represents the gravitational coefficient gain, and ρ(q, q) represents the gravitational potential energy at position q. goal () represents the distance from position q to target point q. goal Euclidean distance; U rep (q) represents the repulsive potential energy at position q, η represents the repulsive coefficient gain, and ρ(q, q) represents the repulsive potential energy at position q. obs () represents the distance from position q to obstacle q. obs The closest distance, ρ0 is the effective range of the repulsive field, which defines the distance at which an obstacle will exert a repulsive force on the sampling point. An obstacle will only exert a repulsive force if it enters this range. U total (q) is the sum of gravitational potential energy and repulsive potential energy;

[0127] The formulas for calculating the gravitational and repulsive functions are as follows:

[0128]

[0129] F total (q)=F att (q)+F rep (q)

[0130] in, For gradient calculation, F att (q) is the gravitational function, F rep (q) is the repulsive force function, F total (q) represents the total potential force;

[0131] Step 6.2: Adjust the sampling point position: Based on the total potential force F mentioned above total The current sampling point is moved a small step along the direction of (q) to obtain the first path point q1. This process can be compared to sliding a point on the path to a position with lower potential energy, greater safety, and smoother flow. The line connecting the first path point q1 and the next sampling point is denoted as L1. The first path point q1 and the target point q goal Let L2 be the line connecting L1 and L2, and let ω be the angle between them. The step size after adaptive expansion is calculated using the following formula:

[0132]

[0133] Step 7: Repeat steps 5 and 6 to obtain the second sampling point q. sample1 Then the first path point q1 is used with the second sampling point q sample1 The direction is expanded to obtain the second path point q2 and placed in the queue; then sampling is performed in the same way to obtain the third and subsequent sampling points q. sample2 q sample3 ...and corresponding multiple path points q3, q4... until the last sampling point is the target point q. goal At this point, sampling is complete, and the queue contains a set of path points q. start ,q1,q2······q goal Finally, redundant sampling points are pruned.

[0134] This step typically requires multiple iterations, ensuring the path remains collision-free after each move. After APF optimization and adaptive step size optimization, the path expands towards the target point faster, but redundant sampling points may still exist. Finally, path pruning is needed to further improve efficiency, which involves removing redundant sampling points. Starting from the previous path point, attempts are made to connect the current path point with its multiple sampled points. If the connecting line segment does not collide with any obstacles, it means that all intermediate sampling points are redundant and are removed; this process is repeated until the path cannot be shortened further.

[0135] Step 8: Transfer the above set of path points q start ,q1,q2······q goal Connect them sequentially according to the order in which they are placed in the queue to obtain a broken line-shaped initial path; although the initial path obtained at this time can meet the basic requirements of the path, it is not smooth enough and does not conform to the kinematic and dynamic characteristics of the quadcopter UAV.

[0136] Step 9: Smooth the initial path obtained in Step 8 using cubic B-spline interpolation to conform to the dynamics and kinematics characteristics of the quadcopter UAV, and obtain the final smooth path.

[0137] The initial path generated so far is a set of broken lines connected by path points, which does not conform to the dynamics and kinematics of a quadcopter UAV. Therefore, the initial path needs to be smoothed. This is a post-processing step in UAV path planning, which transforms the initial path generated by the improved RRT algorithm, consisting of a series of discrete and tortuous path points, into a continuous, smooth final trajectory that conforms to the kinematic constraints of the quadcopter UAV. Since cubic uniform B-spline curves have excellent local controllability, continuity, and computational efficiency, the cubic B-spline interpolation method becomes an ideal tool for achieving this goal.

[0138] The method is as follows:

[0139] Step 9.1: Transfer the above set of path points q start ,q1,q2······q goal As input to the cubic B-spline interpolation method, although the initial path obtained in the previous step guarantees the collision-free characteristics of the path, its polygonal shape will cause the quadcopter UAV to frequently start, stop, and make sharp turns, which is not only inefficient but also cannot meet the needs of actual motion control. The task of the smoothing algorithm is to smooth these points while keeping the path points unchanged.

[0140] Step 9.2: Construct the node vector and control points, which determine the domain and shape basis of the cubic B-spline curve; define the node vector. For a cubic B-spline curve, its node vector U is a non-decreasing sequence of parameters; the node spacing of a uniform cubic B-spline curve is equal. A typical cubic uniform B-spline curve defined by m+1 control points can be represented by the node vector: U=[u0,u1,u2,......,u m+1 ] = [0, 0, 0, 0, 1, 2, 3, ..., m-3, m-2, m-1, m], control point L j Not the original path point q j Where j = 1, 2, 3..., the control point selection strategy directly affects the smoothing effect. This invention adopts a curve fitting method, treating the path points in the initial path as target points to be fitted by a cubic B-spline curve, and solving for a set of optimal control points using the least squares method. This method can generate a smooth curve that best fits all path points in the initial path in the least squares sense, effectively filtering out noise and unnecessary fluctuations, resulting in better smoothing.

[0141] Step 9.3: Calculate the basis functions of the cubic B-spline curve: The shape of the cubic B-spline curve is determined by the control points and a set of basis functions; the basis functions are piecewise polynomials that define the control points L. j The influence weights of a node on a cubic B-spline curve are calculated using the Cox-deBox recursive formula; for any node [u0, u1, u2, ..., u] on the cubic B-spline curve... m+1 The cubic B-spline curve is only affected by four adjacent control points. This characteristic gives it unparalleled local controllability. Modifying a control point will only affect a small segment of the cubic B-spline curve, without affecting the entire path.

[0142] Step 9.4: Calculate the path of the cubic B-spline curve: Uniformly sample within the effective interval formed by the node vector U. For each sampled node u... sample Calculate the corresponding points on the smoothed path according to the following formula. This formula means that the points on the curve are all control points relative to the current node u. sample The result of a weighted average of the corresponding basis function values; by calculating u of all nodes sample By finding the corresponding point S(u), a series of dense point sets defining smooth paths can be obtained;

[0143] Step 9.5: Connect the corresponding points S(u) obtained in the previous step in order to obtain the following... Figure 4 , Figure 5 The final smooth path shown is not only geometrically continuous but also conforms to the kinematics and dynamics of the quadcopter UAV, and is typically shorter and better than the initial path. This smooth path is then transmitted to the quadcopter UAV's trajectory tracking controller for execution, enabling efficient and smooth motion during flight missions.

Claims

1. A UAV path planning method based on an improved RRT algorithm, characterized in that: The UAV path planning method based on the improved RRT algorithm includes the following steps performed in sequence: Step 1: Based on the shape, kinematics, and dynamics of the quadrotor UAV, establish a mathematical model of the quadrotor UAV, including a dynamic model, an attitude model, and a position model; Step 2: Determine the 3D workspace for simulating the path planning of a quadcopter UAV using the improved RRT algorithm, and determine the starting point q. start And the target point is q goal Then, a queue is created to store the path points obtained by expanding along the sampling direction, and then the starting point q is... start Place it in the queue, and at the same time import the quadcopter drone mathematical model established in step 1 into the workspace; And set kinematic and dynamic constraints; Step 3: Establish the starting point q in the workspace determined in Step 2. start And the target point is q goal The ellipsoidal sampling domain enclosed within the sampling points is used to limit the sampling points to the ellipsoidal sampling domain and determine the extent of the ellipsoidal sampling domain. Step 4: Set the sampling point density when sampling in the above ellipsoidal sampling domain using the Gaussian sampling method, and use the Artificial Potential Field (APF) algorithm to limit the expansion direction and step size of the sampling points, thereby determining the sampling strategy; Step 5: Following the sampling strategy determined in Step 4, use the Gaussian sampling method in the aforementioned ellipsoidal sampling domain x. ellipsoid From the starting point q start Sampling begins, a pair of sampling points is obtained and obstacle collision detection is performed, and new sampling points are determined based on the detection results; Step 6: After obtaining the new sampling points, use the artificial potential field algorithm to adjust the starting point q. start Using the new sampling point as the direction, expand using an adaptive step size strategy to obtain the first path point q1 and put it into the queue; Step 7: Repeat steps 5 and 6 to obtain the second sampling point q. sample1 Then the first path point q1 is used with the second sampling point q sampke1 The direction is expanded to obtain the second path point q2 and placed in the queue; then sampling is performed in the same way to obtain the third and subsequent sampling points q. sample2 ,q sample3 ...and corresponding multiple path points q3, q4..., until the last sampling point is the target point q. goal At this point, sampling is complete, and the queue contains a set of path points q. start ,q1,q2……q goal Finally, redundant sampling points are trimmed. Starting from the previous path point, try to connect the current path point with the sampling points obtained from multiple samplings. If the connecting line segment does not collide with any obstacles, it means that all the sampling points in the middle are redundant and they are removed. Repeat this process until the path cannot be shortened further. Step 8: Transfer the above set of path points q start q1, q2...q goal Connect them sequentially according to the order in which they were placed in the queue to obtain an initial path in the shape of a broken line; Step 9: Smooth the initial path obtained in Step 8 using cubic B-spline interpolation to conform to the dynamics and kinematics characteristics of the quadcopter UAV, and obtain the final smooth path.

2. The UAV path planning method based on the improved RRT algorithm according to claim 1, characterized in that: In step 1, the dynamic model is as follows: t z =c M (ω1 2 -ω2 2 +ω3 2 -ω4 2 ); T=c T (ω1 2 +ω2 2 +ω3 2 +ω4 2 ); Among them, T m ω is the motor response time constant. m This is the steady-state speed of the motor. Let ω1(t), ω2(t), ω3(t), and ω4(t) be the derivatives of the rotational speeds of the four motors on the quadcopter drone, respectively; σ1(t), σ2(t), σ3(t), and σ4(t) be the throttle values ​​of the four motors, respectively; and τ be the derivatives of the rotational speeds of the four motors on the quadcopter drone. x ,τ y ,τ z The torques are the x, y, and z axes, respectively, and d is... The drone's wheelbase, T, is the sum of the lift of the four propeller blades, and c... T c is the thrust coefficient of a single blade. M For a single blade moment coefficient; The attitude model is established based on Euler's dynamic equations as follows: Among them, J xx J yy J zz Let x, y, and z be the moments of inertia of rotation along the x, y, and z axes, respectively. θ, These are the pitch angle, roll angle, and yaw angle of the quadcopter drone. Let be the derivatives of the angular velocities along the x, y, and z axes, respectively. These are the derivatives for pitch, roll, and yaw angles, respectively. The location model is as follows: Among them, v x v y v z The velocities p are the velocities along the x, y, and z axes, respectively. x p y p z The x, y, and z axes represent the positions, g is the acceleration due to gravity, and m is the mass of the quadcopter drone. Let be the derivatives of the velocities along the x, y, and z axes, respectively. These are the derivatives of the positions along the x, y, and z axes, respectively.

3. The UAV path planning method based on the improved RRT algorithm according to claim 1, characterized in that: In step 2, the three-dimensional workspace for simulating the path planning of the quadcopter UAV using the improved RRT algorithm is determined, and the starting point q is determined. start And the target point is q goal Then, a queue is created to store the path points obtained by expanding along the sampling direction, and then the starting point q is... start Place it in the queue, and simultaneously import the quadcopter UAV mathematical model established in step 1 into the workspace; and set the kinematic and dynamic constraints as follows: In Python, a 3D workspace is set up for path planning simulation of a quadrotor drone, which includes randomly generated obstacles. Then, based on the kinematic characteristics of the drone, kinematic and dynamic constraints, including the turning angle and climb angle, are applied to the quadrotor drone in the workspace. The turning angle of the quadrotor drone from the current path point to the next path point is constrained to change only within a certain range. At the same time, the angle of the quadrotor drone when climbing from the current path point to the next path point is also constrained to change only within a certain range. The formula for calculating the steering angle is: The formula for calculating the climb angle is: Among them, (x i ,y i ,z i Let be the coordinates of the path points, i = 0, 1, 2, 3....

4. The UAV path planning method based on the improved RRT algorithm according to claim 1, characterized in that: In step 3, the starting point q is established in the workspace determined in step 2. start And the target point is q goal The method for confining sampling points to the ellipsoidal sampling domain and determining the extent of the ellipsoidal sampling domain is as follows: Step 3.1: Define the parameters of the ellipsoidal sampling domain: Define the straight-line distance from the starting point of the path to the target point as C. min The cost of the path is C. best a is the major axis, b is the minor axis, and q is the minor axis. c Let be the center of the ellipsoid, with major axis a, minor axis b, and center q of the ellipsoid. c The calculation formula is as follows: Step 3.2: Based on the major axis a and minor axis b, construct the starting point q as described above. start And the target point is q goal The ellipsoidal sampling domain is enclosed by a direction vector d = q. start -q goal Normalizing the direction vector d yields the normalized direction vector. This direction will be used as the major axis direction of the ellipsoid. Two unit vectors u2 and u3, orthogonal to the normalized direction vector u1, will be constructed such that u1, u2, ... u 3. A set of orthonormal bases is constructed as an orthonormal coordinate system within the ellipsoidal sampling domain; the unit vector u2 is constructed by first selecting an arbitrary vector v that is not parallel to the normalized direction vector u1, and then calculating the unit vector using the following formula. Obtain the unit vector perpendicular to the normalized direction vector u1; then calculate the unit vector u3 = u1 × u2 using the following formula, where the × operator is the cross product operation; Step 3.3: Based on the above orthonormal basis, the unit sphere x ball The scaling mapping is applied to an ellipsoidal sampling domain; first, the scaling matrix L is defined: Then the ellipsoid is Lx ball Therefore, the ellipsoidal sampling domain x ellipsoid for: X ellipsoid =Lx ball +q c ; Where, q c It is the center of the ellipsoid.

5. The UAV path planning method based on the improved RRT algorithm according to claim 1, characterized in that: In step 4, the density of sampling points is set when sampling in the above-mentioned ellipsoidal sampling domain using the Gaussian sampling method. At the same time, an artificial potential field algorithm is used to limit the expansion direction and step size of the sampling points. The sampling strategy is determined as follows: The Gaussian sampling method employs a sampling strategy that increases sampling points in areas of high obstacle density and decreases them in areas of low obstacle density to adjust the sampling point density. The artificial potential field algorithm efficiently restricts the expansion direction and step size of sampling points when facing obstacles, making the sampling points more closely aligned with the target point q. goal The direction.

6. The UAV path planning method based on the improved RRT algorithm according to claim 1, characterized in that: In step 5, the sampling strategy determined in step 4 is followed, and the Gaussian sampling method is used in the aforementioned ellipsoidal sampling domain X. ellipsoid From the starting point q start Sampling begins, a pair of sampling points is obtained, and obstacle collision detection is performed. The method for determining new sampling points based on the detection results is as follows: Step 5.1: Sampling domain X on the ellipsoid ellipsoid Inside, from the starting point q start To begin, the first sampling point q is randomly generated using the randomsample function in Python. sample ; Step 5.2: Using the first sampling point q sample Centered on a Gaussian distribution, a random offset vector Δq = randomnormal(0,σ,dimension) is generated from a Gaussian distribution with a mean of 0 and a standard deviation of σ, where randomnormal is the random normalization function in Python. Step 5.3: Based on the first sampling point q mentioned above sample And a second sampling point q is generated using a random offset vector Δq. guass =q sample +Δq; Step 5.4: Perform obstacle collision detection. If the first sampling point q sample and the second sampling point q guass If both are in free space, one of them is randomly selected as the new sampling point; if one falls in free space and the other falls in an obstacle, the sampling point in free space is selected; if both fall in an obstacle, the pair of sampling points is discarded and sampling is performed again.

7. The UAV path planning method based on the improved RRT algorithm according to claim 1, characterized in that: In step 6, after obtaining the new sampling point, the artificial potential field algorithm is used to adjust the starting point q. start The method for expanding the path using the new sampling point as the direction and an adaptive step size strategy to obtain the first path point q1 and add it to the queue is as follows: Step 6.1: Guide the expansion direction using the artificial potential field algorithm. During sampling, use the APF expansion strategy for iterative optimization. For each path point on the path, except for the starting and target points, calculate the gravitational field function U according to the following formula. att (q); then the repulsive field function U is calculated according to the following formula. rep (q) Iterate through all obstacles and calculate the distance from the current sampling point to each obstacle; If the distance is less than the range ρ0 of the repulsive field, a repulsive vector is generated from the obstacle to the current sampling point. Then, the negative gradients of the obtained gravitational and repulsive field functions are calculated to obtain the gravitational and repulsive functions. Finally, all the gravitational and repulsive forces acting on the current sampling point are vector-synthesized to obtain the total potential force U. total (q); The formulas for calculating the gravitational field function and the repulsive field function are as follows: U total (q)=U att (q)+U rep (q); Among them, U att (q) represents the gravitational potential energy at position q, ξ represents the gravitational coefficient gain, and ρ(q, q) represents the gravitational potential energy at position q. goal () represents the distance from position q to target point q. goal Euclidean distance; U rep (q) represents the repulsive potential energy at position q, η represents the repulsive coefficient gain, and ρ(q, q) represents the repulsive potential energy at position q. obs () represents the distance from position q to obstacle q. obs The closest distance, ρ0 is the effective range of the repulsive field, which defines the distance at which an obstacle will exert a repulsive force on the sampling point. An obstacle will only exert a repulsive force if it enters this range. U total (q) is the sum of gravitational potential energy and repulsive potential energy; The formulas for calculating the gravitational and repulsive functions are as follows: F tota (q)=F att (q)+F rep (q); in, For gradient calculation, F att (q) is the gravitational function, F rep (q) is the repulsive force function, F total (q) represents the total potential force; Step 6.2: Adjust the sampling point position: Based on the total potential force F mentioned above total The current sampling point is moved a small step along the direction of (q) to obtain the first path point q1; the line connecting the first path point q1 and the next sampling point is denoted as L1, and the first path point q1 and the target point q goal Let L2 be the line connecting L1 and L2, and let ω be the angle between them. The step size after adaptive expansion is calculated using the following formula:

8. The UAV path planning method based on the improved RRT algorithm according to claim 1, characterized in that: In step 9, the initial path obtained in step 8 is smoothed using cubic B-spline interpolation to conform to the dynamics and kinematics characteristics of the quadcopter UAV. The method for obtaining the final smooth path is as follows: Step 9.1: Transfer the above set of path points q start ,q1,q2.......q goal As input for cubic B-spline interpolation; Step 9.2: Construct the node vector and control points, which determine the domain and shape basis of the cubic B-spline curve; define the node vector. For a cubic B-spline curve, its node vector U is a non-decreasing sequence of parameters; the node spacing of a uniform cubic B-spline curve is equal. The node vector of a typical cubic uniform B-spline curve defined by m+1 control points can be expressed as: U=[u0,u1,u2,......,u m+1 [0,0,0,0,1,2,3, ...,m-3,m-2,m-1,m], control point L j Not the original path point q j Where j = 1, 2, 3, ..., the curve fitting method is adopted, and the path points in the initial path are regarded as the target points to be fitted by the cubic B-spline curve. The least squares method is used to solve a set of optimal control points in reverse, and a smooth curve that is closest to all path points in the initial path in the least squares sense is generated. Step 9.3: Calculate the basis functions of the cubic B-spline curve: The shape of the cubic B-spline curve is determined by the control points and a set of basis functions; the basis functions are piecewise polynomials that define the control points L. j The influence weights of a node on a cubic B-spline curve are calculated using the Cox-deBox recursive formula for these basis functions; Step 9.4: Calculate the path of the cubic B-spline curve: Uniformly sample within the effective interval formed by the node vector U. For each sampled node u... sample The corresponding points on the smoothed path are calculated according to the following formula. By calculating all nodes u sample The corresponding point S(u) is used to obtain a series of dense point sets that define smooth paths; Step 9.5: Connect the corresponding points S(u) obtained in the previous step in order to obtain the final smooth path.

Images