A motion planning method for mobile robots using Archimedean spirals

Abstract

The present invention discloses a method for motion planning of a mobile robot using an Archimedean spiral. The motion planning method includes establishing a robot kinematic model in a polar coordinate system with a target position as the pole and a target posture as the polar axis; planning the robot path using the Archimedean spiral without considering obstacles; obtaining an optimized collision-free smooth path for the robot using an improved probabilistic roadmap path planning algorithm based on the Archimedean spiral; and obtaining an optimized desired robot speed using a speed planning algorithm based on the curvature of the Archimedean spiral and satisfying the robot's motion capability constraints, thereby ultimately achieving motion planning for the robot. The present invention can quickly achieve optimal path planning in complex environments, and the generated path satisfies kinematic constraints, is smooth, and intuitive. The planned speed takes into account the effect of curvature on motion smoothness and the robot's motion capability constraints, enabling the robot to reach the target position "quickly and steadily."

Description

Technical Field

[0001] The invention belongs to the technical field of mobile robot navigation, and in particular relates to a mobile robot motion planning method using an Archimedean spiral. Background Art

[0002] With the rapid advancement of artificial intelligence and robotics, autonomous mobile robots have penetrated various application scenarios in industries such as industry, scientific research, and the home. Their ability to replace humans in complex and arduous tasks is an inevitable trend. To achieve efficient and intelligent autonomous movement in complex environments, mobile robots must possess capabilities such as positioning, perception, motion planning, and motion control. The fundamental problem of motion planning is to find a collision-free path from a starting position to a target position in a complex environment with obstacles. This involves adding time series information, or velocity planning, to the path planning. Motion planning for mobile robots remains a challenging problem, particularly given the need to simultaneously consider the robot's kinematic constraints, motion capacity constraints, path smoothness, and time optimality.

[0003] Currently, widely used robot motion planning methods can be categorized into three main types: sampling-based, search-based, and optimization-based. However, each of these methods has its advantages and disadvantages. Traditional sampling-based and search-based planning methods, such as the rapidly expanding random tree algorithm (RRT), the probabilistic road map algorithm (PRM), A*, and Dijkstra, can quickly find collision-free paths from a starting point to a target point. However, they typically use Euclidean distance to measure the distance between the robot and the target. Obviously, for mobile robots with nonholonomic constraints (e.g., robots that cannot move sideways), the Euclidean distance does not reflect the true cost of moving from one position to another, resulting in these algorithms being unable to effectively explore the space. Furthermore, the broken-line paths obtained by these algorithms often fail to consider the robot's kinematic constraints, resulting in unrealistic and unsmooth motion along the path. While motion planning methods based on nonlinear optimization can optimize smooth and safe trajectories, they are only locally optimal, require a good initial guess for the trajectory and timeframe, and generally suffer from high computational complexity, low solution efficiency, and relatively poor real-time performance. Therefore, despite recent progress in the field of motion planning for mobile robots, most existing work focuses solely on ensuring that the robot can successfully reach the target pose. However, for truly successful autonomous navigation, the "quality" of motion planning is also very important. Summary of the Invention

[0004] In order to solve the problems existing in the background technology, the present invention provides a mobile robot motion planning method using Archimedean spiral.

[0005] The technical solution adopted by the present invention is as follows, comprising the following steps:

[0006] Step S1: first establish a polar coordinate system with the target position as the pole and the target posture as the polar axis, and then establish a robot kinematic model in the polar coordinate system;

[0007] Step S2: without considering obstacles, establish the robot's initial planned path based on the robot's kinematic model and Archimedean spiral;

[0008] Step S3: Based on the improved probabilistic roadmap algorithm, the initial planned path of the robot established in step S2 is optimized to obtain the optimized optimal collision-free smooth path of the robot;

[0009] Step S4: Based on the curvature of the Archimedean spiral path and the optimal robot collision-free smooth path of step S3, the optimized robot expected speed is obtained, and finally the motion planning of the robot is realized.

[0010] The step S1 is specifically as follows:

[0011] Step S1.1: First, the robot pose P0 and the target pose P are aligned in the global coordinate system O-XY. t Define the robot pose P0 and target pose P t The vector expression of is as follows:

[0012] P0=[x0,y0,θ0] T

[0013] P t =[x t ,y t ,θ t ] T

[0014] Among them, x0 and y0 are the current horizontal and vertical coordinates of the robot respectively; θ0 is the angle of the robot's current orientation relative to the X axis; t and y t are the horizontal and vertical coordinates of the target position, θ t The angle of the target direction relative to the X axis;

[0015] Step S1.2: Next, establish the kinematic model of the differential robot in the global coordinate system O-XY according to the following formula:

[0016]

[0017] Where v is the linear velocity of the robot; ω is the angular velocity of the robot; θ is the angle of the robot; is the component of the robot’s linear velocity on the X-axis, is the component of the robot's linear velocity on the Y axis; is the derivative of θ with respect to time, i.e. the angular velocity of the robot;

[0018] Step S1.3: Establish a polar coordinate system T with the target position as the pole and the target posture as the polar axis. The robot state s in the polar coordinate system T is expressed as follows:

[0019]

[0020] Where r is the distance between the robot's current position and the target position; α is the angle of the target heading relative to the robot's line of sight to the target position; β is the angle of the robot heading relative to the robot's line of sight to the target position; represents the set of positive real numbers;

[0021] Step S1.4: Based on the kinematic model of the robot in the global coordinate system O-XY, the kinematic model of the robot in the polar coordinate system T is obtained according to the following formula:

[0022]

[0023] in, is the time derivative of r, is the time derivative of α, is the time derivative of β;

[0024] The step S2 is specifically as follows:

[0025] Step S2.1: First, without considering any obstacles, set the robot's initial linear velocity v according to the following formula based on the distance r between the robot and the target position:

[0026]

[0027] Where k1 and k2 are given adjustment coefficients, k1>0, k2>0; r is the distance between the robot's current position and the target position;

[0028] Step S2.2: Next, without considering obstacles, according to the kinematic model of the Archimedean spiral and the robot in the polar coordinate system T, the angle α and the angle β satisfy the following relationship:

[0029] β=arctan(-k α α)

[0030] Among them, k α is the preset proportional coefficient, satisfying k α >0; α is the angle of the target direction relative to the robot's line of sight to the target position; β is the angle of the robot's direction relative to the robot's line of sight to the target position;

[0031] Step S2.3: Then, without considering obstacles, the robot's angular velocity ω is obtained according to the following formula:

[0032]

[0033] Where v is the initial linear velocity of the robot without considering obstacles; k α is the preset proportional coefficient; α is the angle of the target direction relative to the robot's line of sight to the target position; k β is the preset scale factor; the function NormalizeAngle(angle) means converting the angle angle to the interval (-π,π].

[0034] The step S3 is specifically as follows:

[0035] Step S3.1: First, construct a directed graph G(V,E) and initialize the directed graph, where V represents the pose set and E represents the line set;

[0036] Step S3.2: Add the robot pose and the target pose to the pose set V, then connect the robot pose and the target pose using an Archimedean spiral, use the Archimedean spiral as the current path, and perform collision detection on the current path:

[0037] If the current path is determined to be a safe path after collision detection, the current path will be used as the optimized optimal robot collision-free smooth path;

[0038] Otherwise, set the current iteration number iter to 1 and execute the next step S3.3;

[0039] Step S3.3: Sample new pose P in the prior map by heuristic sampling method i , and judge the new sampling pose P i Rationality:

[0040] If the newly sampled pose P i If it is not in the obstacle of the prior map, the newly sampled pose P i Add to the pose set V;

[0041] Otherwise, resample until the new sampled pose P i Not among the obstacles in the a priori map;

[0042] Step S3.4: Construct the neighborhood line set E' of the new sampling pose and obtain the new sampling pose P i The pose distance D from the pose P in the pose set V is the same as the pose P in the pose set V. iIf the pose distance D between them is less than the distance threshold d, the Archimedean spiral is used to connect the pose P in the pose set V and the new sampled pose P i , add the Archimedean spiral to the neighborhood line set E';

[0043] Step S3.5: Check the feasibility of the robot moving along the path in the neighborhood line set E':

[0044] If the robot does not collide with an obstacle while moving along the path in the neighborhood link set E', then the Archimedean spiral in the neighborhood link set E' is added to the link set E;

[0045] Otherwise, the Archimedean spiral in the neighborhood link set E' is not added to the link set E;

[0046] Step S3.6, repeat steps S3.3 to S3.5 until the number of poses in the pose set V reaches the preset number of sampling poses required iter*n, where n represents the preset number of sampling poses;

[0047] Step S3.7: Use the graph search algorithm to search the current directed graph G(V,E):

[0048] If a feasible path from the robot pose to the target pose exists, the current feasible path is used as the collision-free smooth path of the robot in the current iteration;

[0049] If a feasible path from the robot pose to the target pose does not exist, increase the value of the current iteration number iter by 1 and repeat steps S3.3 to S3.7 until a feasible path from the robot pose to the target pose exists, and then proceed to step S3.8;

[0050] Step S3.8: Determine whether the robot collision-free smooth path of the current iteration is the optimal robot collision-free smooth path:

[0051] If the collision-free smooth path of the robot obtained in the current iteration is not optimized after m consecutive iterations, the collision-free smooth path of the robot obtained in the current iteration is used as the optimal collision-free smooth path of the robot after optimization;

[0052] Otherwise, increase the value of the current iteration number iter by 1 and repeat steps S3.3 to S3.8 until the optimal robot collision-free smooth path is found.

[0053] The step S4 is specifically as follows:

[0054] Step S4.1: Based on the maximum linear velocity of the robot, construct the relationship between the desired linear velocity of the robot and the curvature of the Archimedean spiral path according to the following formula:

[0055]

[0056] Where k3 is the preset design parameter; κ is the path curvature, v max is the maximum linear speed of the robot;

[0057] Step S4.2, then every preset interval s on the optimal robot collision-free smooth path from the robot pose to the target pose L Take a point as the intermediate pose point to discretize the optimal robot collision-free smooth path into a smooth path vector L. The expression of the smooth path vector is as follows:

[0058] L=[P0,P1,…,P i ,P i+1 ,…,P I ,P t ]

[0059] Among them, the starting point P0 is the robot posture, and the end point P t is the target point pose;

[0060] Step S4.3: Then, smooth the path vector from the starting point P0 to the end point P t The forward velocity planning is used to obtain the robot moving forward along the optimal robot collision-free smooth path, and the robot reaches the i-th intermediate pose point P i The expected linear velocity v i ;

[0061] Step S4.4: Finally, smooth the path vector end point P t The reverse speed planning to the starting point P0 is obtained, and when the robot moves in the reverse direction along the optimal robot collision-free smooth path, the robot reaches the i-th intermediate posture point P i The expected linear velocity v i ;

[0062] Step S4.5: Based on the optimal robot collision-free smooth path and the expected linear speed of the robot when moving forward and backward along the optimal robot collision-free smooth path, the motion planning of the robot is finally completed.

[0063] In step S3.4, the new sampling pose P i The pose distance D from the pose P in the pose set V is obtained by the following formula:

[0064]

[0065] Among them, r d is the position distance between the two poses, α d is the new sampled pose P i Orientation relative to pose P to the new sample pose Pi Angle of sight direction; β d is the orientation of pose P relative to pose P to the new sample pose P i The angle of the viewing direction.

[0066] The step S4.3 is specifically as follows:

[0067] Step S4.3.1. First, determine the planned linear velocity and angular velocity of the robot at the starting point P0 as the current actual linear velocity v0 and actual angular velocity ω0 of the robot, and initialize the intermediate pose point number i = 1;

[0068] Step S4.3.2: Preliminarily determine the intermediate pose point P based on the relationship between the robot's expected linear velocity and the curvature of the Archimedean spiral path. i The expected linear velocity v i , and according to the intermediate pose point P i The expected linear velocity v i and path curvature κ i , preliminarily determine the intermediate pose point P according to the following formula i The expected angular velocity ω i :

[0069] ω i =v i ×κ i

[0070] Step S4.3.3, according to the intermediate pose point P i-1 The expected linear velocity v i-1 and angular velocity ω i-1 , according to the maximum linear acceleration av max and angular acceleration aω max For the intermediate pose point P i The expected linear velocity v i Perform constraint adjustment. The specific constraint adjustment methods are as follows:

[0071] If the intermediate pose point P i Expected linear speed Then order

[0072] If the intermediate pose point P i The expected angular velocity Then order v i =|ω i ×κ i |, where θ i is the intermediate pose point P i The current heading angle relative to the X axis;

[0073] Otherwise, the intermediate pose point P iThe expected linear velocity v i Make adjustments;

[0074] Step S4.3.4: Update the intermediate pose point sequence number i to i+1, and repeat steps S4.3.2 to S4.3.4 until the intermediate pose point sequence number i is greater than the total number of intermediate path points I, completing the forward velocity planning.

[0075] The step S4.4 is specifically as follows:

[0076] Step S4.4.1: First, define the robot at the end point P t The planned linear speed v t and the planned angular velocity ω t are all 0, and the intermediate pose point number i=I is initialized;

[0077] Step S4.4.2, according to the intermediate pose point P i+1 The expected linear velocity v i+1 and angular velocity ω i+1 , according to the maximum linear acceleration av max and angular acceleration aω max For the intermediate pose point P i The expected linear velocity v i Perform constraint adjustment. The specific constraint adjustment methods are as follows:

[0078] If the intermediate pose point P i Expected linear speed Then order

[0079] If the intermediate pose point P i The expected angular velocity Then order v i =ω i ×κ i |, where θ i is the intermediate pose point P i-1 The current heading angle relative to the X axis;

[0080] Otherwise, the intermediate pose point P i The expected linear velocity v i Make adjustments;

[0081] Step S4.4.3, update the intermediate pose point sequence number i to i-1, repeat steps S4.4.2 to S4.4.3 until the intermediate pose point sequence number i is 0, and complete the reverse velocity planning.

[0082] The goal of this invention is to rapidly and intuitively implement smooth and intuitive motion planning (including path and velocity planning) from any given initial position to any target position, avoiding obstacles in complex environments. This invention proposes a path planning algorithm that combines a traditional sampling-based probabilistic roadmap algorithm with an improved probabilistic roadmap algorithm based on the Archimedean spiral. It also proposes a velocity planning algorithm based on the curvature of the Archimedean spiral that satisfies the robot's motion constraints.

[0083] Compared with the prior art, the present invention has the following beneficial effects:

[0084] 1. The present invention proposes to use Archimedean spiral to realize the path planning of the robot from any initial posture to any target posture. The generated path not only satisfies the kinematic constraints but is also smooth and intuitive.

[0085] 2. The present invention proposes an improved probabilistic roadmap algorithm, which combines the traditional probabilistic roadmap algorithm and the path planning algorithm using Archimedean spiral, and can quickly achieve optimal path planning in complex environments.

[0086] 3. The present invention proposes a velocity planning method, designs the expected linear velocity based on the curvature of the Archimedean spiral path, and takes into account the robot's motion capability constraints, so that the robot can reach the target position "quickly and steadily". BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 This is a flow chart of robot motion planning using Archimedean spirals proposed by the present invention;

[0088] Figure 2 Flowchart of path planning using Archimedean spiral between two positions to avoid obstacles;

[0089] Figure 3 Schematic diagram of the robot state expressed in polar coordinate system;

[0090] Figure 4 The Archimedean spiral path diagram of the robot from the initial position to the target position;

[0091] Figure 5 It is the Archimedean spiral path diagram of the robot from the initial pose to the target pose;

[0092] Figure 6 It is a "maze" scene map;

[0093] Figure 7 It is the Archimedean spiral path diagram that directly connects the robot pose and the target pose;

[0094] Figure 8 A diagram showing the process of path planning using the improved probabilistic roadmap algorithm;

[0095] Figure 9 It is a graph showing the total “distance” change from the robot’s pose to the optimal feasible path for the target pose. DETAILED DESCRIPTION

[0096] The present invention is described in detail below with reference to specific implementation cases. The following implementation cases will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form.

[0097] The present invention comprises the following steps:

[0098] Step S1: first establish a polar coordinate system with the target position as the pole and the target posture as the polar axis, and then establish a robot kinematic model in the polar coordinate system;

[0099] Step S2: Without considering obstacles, establish the robot's initial planning path based on the robot's kinematic model and Archimedean spiral, such as Figure 2 As shown;

[0100] Step S3: Based on the improved probabilistic roadmap algorithm, the initial planned path of the robot established in step S2 is optimized to obtain the optimized optimal collision-free smooth path of the robot;

[0101] Step S4: Based on the curvature of the Archimedean spiral path and the optimal robot collision-free smooth path of step S3, the optimized robot expected speed is obtained, and finally the motion planning of the robot is realized.

[0102] Step S1 is specifically as follows:

[0103] Step S1.1: First, the robot pose P0 and the target pose P are aligned in the global coordinate system O-XY. t Define the robot pose P0 and target pose P t The vector expression of is as follows:

[0104] P0=[x0,y0,θ0] T

[0105] P t =[x t ,y t ,θ t ] T

[0106] Among them, x0 and y0 are the current horizontal and vertical coordinates of the robot respectively; θ0 is the angle of the robot's current orientation relative to the X axis; t and y t are the horizontal and vertical coordinates of the target position, θ t is the angle of the target relative to the X axis, such as Figure 3As shown, the target direction θ t is a negative value, and the robot is heading towards θ0 is a positive value;

[0107] The robot in the present invention adopts a differential robot. As common knowledge in the field, on the one hand, the rotational speeds of the two wheels of the differential robot are independent of each other, that is, the linear velocity and angular velocity of the robot can be independently controlled; on the other hand, the differential robot is underdriven, and the direction of the linear velocity is consistent with the direction of the robot, that is, it cannot move sideways.

[0108] Step S1.2: Next, establish the kinematic model of the differential robot in the global coordinate system O-XY according to the following formula:

[0109]

[0110] Among them, v is the linear velocity of the robot, along the x axis of the robot system b axis; ω is the angular velocity of the robot, perpendicular to the robot's motion plane; θ is the angle of the robot; is the component of the robot’s linear velocity on the X-axis, is the component of the robot's linear velocity on the Y axis; is the derivative of θ with respect to time, i.e. the angular velocity of the robot;

[0111] Step S1.3: Different coordinate systems are selected, and the kinematic model describing the robot's motion is also different. To facilitate the design of the Archimedean spiral from the robot's current position to the target position, a polar coordinate system T is established with the target position as the pole and the target posture as the polar axis. The robot state s in the polar coordinate system T is expressed as follows:

[0112]

[0113] Where r is the distance between the robot's current position and the target position; α is the angle of the target heading relative to the robot's line of sight to the target position; β is the angle of the robot's heading relative to the robot's line of sight to the target position; counterclockwise is negative and clockwise is positive, and (-π,π] represents the left-open and right-closed interval from negative π to positive π. r and α describe the robot's position, and β describes the robot's heading. Figure 3 The values ​​of α and β shown are both negative; represents the set of positive real numbers;

[0114] Step S1.4: Based on the kinematic model of the robot in the global coordinate system O-XY, the kinematic model of the robot in the polar coordinate system T is obtained according to the following formula:

[0115]

[0116] in, is the time derivative of r (velocity), is the time derivative of α (angular velocity), is the time derivative of β;

[0117] Obviously, the robot posture P0 expressed in the global coordinate system O-XY and the robot state s expressed in the polar coordinate system T can be converted to each other. The derivation process of the conversion is as follows:

[0118] 1) The target pose P expressed in the known global coordinate system O-XY t =[x t ,y t ,θ t ] T and the robot state s = [r, α, β] expressed in polar coordinate system T T , then, in the target pose coordinate system o t -x t y t The robot pose represented below is [-rcosα,rsinα,NormalizeAngle(β-α)] T , so the robot pose P0 = [x0, y0, θ0] expressed in the global coordinate system O-XY T for:

[0119]

[0120] θ=NormalizeAngle(θ t +β-α)

[0121] Among them, the function NormalizeAngle(angle) represents converting the angle angle to the interval (-π,π].

[0122] 2) The robot pose P0 = [x0, y0, θ0] expressed in the global coordinate system O-XY is known. T and target pose P t =[x t ,y t ,θ t ] T , the robot state s = [r, α, β] expressed in the polar coordinate system T T Satisfy the following formula:

[0123]

[0124] sinα=(-sinθ t (x0-x t )+cosθ t (y0-y t )) / r

[0125] cosα=-(cosθ t (x0-x t )+sinθ t (y0-y t )) / r

[0126] β=NormalizeAngle(θ-θ t +α)

[0127] Therefore, α can be easily calculated based on sinα and cosα.

[0128] According to the kinematic model of the differential robot, it is easy to obtain that the robot state s represented in the polar coordinate system T satisfies the kinematic model:

[0129]

[0130] The global coordinate system O-XY is generally a two-dimensional coordinate system established based on the robot's mapping. It is a plane coordinate system with the starting point of the mapping as the origin, the horizontal rightward direction as the X axis, and the horizontal upward direction as the Y axis. b -x b y b :Origin o b is the robot's rotation center, x b The y axis points directly in front of the robot. b The axis is in the horizontal plane with x b The axes form a right-handed coordinate system. Polar coordinate system T: The pole is the target position, and the polar axis points to the target direction. Target pose coordinate system o t -x t y t :Origin o t is the target position, x t Axis pointing towards the target, y t The axis is in the horizontal plane with x t The axes form a right-handed coordinate system.

[0131] Step S2 is specifically as follows:

[0132] Without considering obstacles, in the polar coordinate system T defined in the present invention, the robot is moved from any initial state s = [r, α, β] T To the target state [0,0,0] T This is equivalent to planning the path of the robot from any current pose to any target pose in the global coordinate system.

[0133] Step S2.1: To ensure that the robot can move to the target position, first, without considering obstacles, set the robot's initial linear velocity v according to the distance r between the robot and the target position according to the following formula:

[0134]

[0135] Where k1 and k2 are given adjustment coefficients, k1>0, k2>0, and the range of linear velocity v is [0, k1). When the robot is very far from the target position, that is, r→∞, v→k1; when the robot is very close to the target position, that is, r→0, v→0. The robot's motion constraints are not considered during the initial planning. A detailed velocity plan that takes motion constraints into account is given in step S4; r is the distance between the robot's current position and the target position;

[0136] Step S2.2: Next, without considering obstacles, according to the kinematic model of the Archimedean spiral and the robot in the polar coordinate system T, the angle α and the angle β must satisfy the relationship:

[0137] β=arctan(-k α α)

[0138] Among them, k α is the preset proportional coefficient, satisfying k α >0;

[0139] Step S2.3: Then, without considering obstacles, the robot's angular velocity ω is obtained according to the following formula:

[0140]

[0141] Where v is the initial linear velocity of the robot without considering obstacles; k α is the preset proportional coefficient; α is the angle of the target direction relative to the robot's line of sight to the target position; k β is the preset scale factor; the function NormalizeAngle(angle) means converting the angle angle to the interval (-π,π].

[0142] Specifically, the polar coordinate equation of the famous Archimedean spiral is:

[0143]

[0144] In the polar coordinate equation of the Archimedean spiral, r a is the radial distance to the pole, θ a is the polar angle, k is a constant that determines how tightly the spiral is wrapped, and a is the proportional factor. Taking the derivative of both sides of the equation with respect to time, we get:

[0145]

[0146] From the kinematic model of the differential robot, we can know that:

[0147]

[0148] Therefore, if α and β satisfy the relationship:

[0149] β=arctan(-k α α)(3)

[0150] Among them, the proportional coefficient k α Satisfy k α >0, is a given design parameter, so that the form of equation (1) and equation (2) is the same, and the path shape of the robot movement is an Archimedean spiral. Note: Although the polar angle θ in the Archimedean spiral is not exactly the same as the definition of the angle α in the present invention, there is a rotation and mirror relationship between them. After rotation and mirroring, the Archimedean spiral is still an Archimedean spiral. Figure 4 Given the robot's initial position [x=1,y=-1] T 、[x=1,y=0] T 、[x=1,y=1] T 、[x=0,y=1] T 、[x=-1,y=1] T 、[x=-1,y=0] T 、[x=-1,y=-1] T 、[x=0,y=-1] T Arrival target pose P t =[x t =0,y t =0,θ t =0] T Assuming that α and β always satisfy the relationship (3), we can see that the proportional coefficient k α Determines how tightly the spiral is "wrapped", k α → When k is 0, the robot's motion mode is simplified to "rotation in place - linear motion - rotation in place", α >>0, the convergence speed of the angle α in the robot state is much faster than that of the distance r.

[0151] Moreover, it can be proved that if the linear velocity v of the robot is greater than 0, and α and β satisfy the relationship (3), for any initial state of the robot in the polar coordinate system T, [0,0,0] T It is globally asymptotically stable, which is equivalent to that in the global coordinate system, for any initial and target poses of the robot, the target pose is globally asymptotically stable.

[0152] The proof process is as follows:

[0153] Construct a Lyapunov function:

[0154] V(r,α)=r 2 +(k α α) 2

[0155] Obviously, for any non-zero r, α, V(r, α)> 0. Taking the derivative of V(r, α), we can get the following expression:

[0156]

[0157] Since cos(arctan(-k α α))>0,sin(arctan(-k α α)) and (-α) have the same sign, r>0, v>0, so we have And, when ‖[r,α] T When ‖→∞, V(r,α)→∞. Therefore, the system is globally asymptotically stable.

[0158] The proof ends.

[0159] Step S2 gives the expected value of the robot's orientation β. However, since the actual orientation of the robot is inconsistent with the expected orientation, it is necessary to design a closed-loop control law for the robot's angular velocity so that the robot's actual orientation can quickly track the expected orientation.

[0160] Define the heading tracking error e β :

[0161] e β =β-arctan(-k α α)(4)

[0162] Taking the derivative of formula (4), we have

[0163]

[0164] The dot at the top of the symbol represents the time derivative. The feedback linearization method is used to eliminate the nonlinear terms of the system and design the following control law. Feedback linearization is common knowledge in this field. Its basic idea is to use feedback to compensate the nonlinear controlled object into a linear system, and then use linear system theory for design.

[0165]

[0166] The control law in the above formula consists of two parts. The purpose of the first half is to eliminate the nonlinear term. In order to make the shape of the robot's motion path independent of the linear velocity v, the second half u ω Designed to:

[0167]

[0168] Among them, k β >0, is a design parameter. The path shape is determined by the curvature κ. Substituting equations (6) and (7) into κ = ω / v, we can see that the curvature κ is independent of the linear velocity v. Substituting equations (6) and (7) into equation (5), the closed-loop system equation is:

[0169]

[0170] Therefore, the system is globally asymptotically stable and the tracking error is guaranteed to converge to 0.

[0171] In summary, the angular velocity ω of the robot is designed to be

[0172]

[0173] Applying the above-mentioned robot path planning using Archimedean spiral, the robot's linear velocity v and angular velocity ω satisfy equations (1.1) and (9) respectively, and the design parameter k is α =1, k β =5, Figure 5 Given the robot's initial position P0 to the target position P t =[x t =0,y t =0,θ t =0] T The initial pose P0 is taken as [x0=1,y0=-1,θ0] T 、[x0=1,y0=0,θ0] T 、[x0=1,y0=1,θ0] T 、[x0=0,y0=1,θ0] T 、[x0=-1,y0=1,θ0] T 、[x0=-1,y0=0,θ0] T 、[x0=-1,y0=-1,θ0] T 、[x0=0,y0=-1,θ0] T , where the robot’s initial orientation is θ0 = -3π / 4, -π / 2, -π / 4, 0, π / 4, π / 2, 3π / 4, π.

[0174] The robot path planning method using Archimedean spirals in step S2 can only be applied to obstacle-free scenarios because it does not determine the feasibility of the planned path—that is, it does not determine whether the robot will collide with obstacles in the environment along the planned path. However, for scenarios where obstacles are prevalent, the robot must have obstacle avoidance capabilities. Therefore, based on step S2, an improved probabilistic roadmap algorithm is proposed. The probabilistic roadmap (PRM) algorithm is common knowledge in the field. It is a sampling-based path planning method that can find the shortest path given a known map. The basic steps of the probabilistic roadmap algorithm are to first construct a roadmap through sampling and collision detection, and then use a graph search algorithm to find the shortest feasible path on the constructed roadmap. The probabilistic roadmap algorithm has two major problems: first, it simply connects two points, and the planned broken line path cannot satisfy the robot's kinematic constraints; second, it measures the distance between two positions only by length, which does not truly reflect the "distance" between two postures. Unlike the basic probabilistic roadmap algorithm, the improved probabilistic roadmap algorithm: 1) does not sample points ([x, y]) in the map. T represents a point), but a pose ([x,y,θ] T represents a posture), 2) instead of directly connecting two points with a straight line, the method of step S2 is used to plan an Archimedean spiral to connect the two postures, and 3) instead of using length to represent the distance, the "distance" between the two postures is defined as follows based on the characteristics of the Archimedean spiral:

[0175]

[0176] Among them, r represents the position distance between two adjacent poses;

[0177] like Figure 1 As shown in Figure 2, the input, output and specific steps of the improved probabilistic roadmap algorithm are as follows:

[0178] Input: robot pose obtained by the localization algorithm; target pose specified by the actual task; prior global map (grid map) describing the scene environment.

[0179] Output: Based on the designed evaluation indicators, the optimal collision-free smooth path connecting the robot pose and the target pose and satisfying the robot's kinematic constraints.

[0180] Step S3 is specifically as follows:

[0181] 1. Build a roadmap:

[0182] Step S3.1. First, construct a directed graph G(V,E) and initialize it, where V represents a set of poses and E represents a set of links. Specifically, V is the set of collision-free vertices (poses) in the directed graph G(V,E), and E is the set of collision-free links (Archimedes spirals connecting two poses) in the directed graph G(V,E). The initial states of V and E are both empty sets.

[0183] Step S3.2: Add the robot pose and the target pose to the pose set V, then connect the robot pose and the target pose using an Archimedean spiral, use the Archimedean spiral as the current path, and perform collision detection on the current path using the collision detection algorithm:

[0184] If the current path is determined to be a safe path (i.e., no collision occurs) after collision detection, the current path is used as the optimized optimal robot collision-free smooth path, and the improved probabilistic roadmap algorithm ends;

[0185] Otherwise, set the current iteration number iter to 1 and execute the next step S3.3;

[0186] Step S3.3: Sample new pose P in the prior map by heuristic sampling method i , and judge the new sampling pose P i Rationality:

[0187] If the newly sampled pose P i If it is not in the obstacle of the prior map, the newly sampled pose P i Add to the pose set V;

[0188] Otherwise, resample until the new sampled pose P i Not among the obstacles in the prior map;

[0189] Among them, the prior map refers to the map of the robot posture, target posture, and obstacles.

[0190] Step S3.4: Construct a neighborhood line set E' of the new sampling pose. The initial state of the neighborhood line set E' is an empty set. Get the new sampling pose P i The pose distance D from the pose P in the pose set V is the same as the pose P in the pose set V. i If the pose distance D between them is less than the preset distance threshold d, the Archimedean spiral is used to connect the pose in the pose set V and the new sampled pose P i , add the Archimedean spiral to the neighborhood line set E';

[0191] In step S3.4, the new sampled pose P i The pose distance D from the pose P in the pose set V is obtained by the following formula:

[0192] Among them, r d is the position distance between the two poses, α d is the new sampled pose P i Orientation relative to pose P to P i Angle of sight direction; β d is the pose P relative to P to P i The angle of the viewing direction.

[0193] Step S3.5: Use the collision detection algorithm to detect the feasibility of the robot moving along the path in the neighborhood line set E':

[0194] If the robot does not collide with an obstacle while moving along the path in the neighborhood link set E', then the Archimedean spiral in the neighborhood link set E' is added to the link set E;

[0195] Otherwise, the Archimedean spiral in the neighborhood link set E' is not added to the link set E;

[0196] Step S3.6: Repeat steps S3.3 to S3.5 until the number of poses in the pose set V reaches the preset number of sampling poses required iter*n, where n represents the preset number of sampling poses. At this time, the roadmap construction is completed;

[0197] 2. Image Search:

[0198] Step S3.7: Use a graph search algorithm (such as A* or Dijkstra algorithm) to search the current directed graph G(V, E):

[0199] If a feasible path from the robot pose to the target pose exists, the current feasible path is used as the collision-free smooth path of the robot in this iteration, and then proceeds to step S3.8;

[0200] If a feasible path from the robot pose to the target pose does not exist, increase the value of the current iteration number iter by 1 and repeat steps S3.3 to S3.7 until a feasible path from the robot pose to the target pose exists, and then proceed to step S3.8;

[0201] Step S3.8: Determine whether the robot collision-free smooth path of the current iteration is the optimal robot collision-free smooth path:

[0202] If the robot collision-free smooth path obtained in the current iteration has not been optimized after m consecutive iterations (i.e., it has not been updated and the path distance has reached the minimum value), where m is the preset number of iterative optimizations, the robot collision-free smooth path obtained in the current iteration will be used as the optimal robot collision-free smooth path after optimization.

[0203] Otherwise, increase the value of the current iteration number iter by 1 and repeat steps S3.3 to S3.8 until the optimal robot collision-free smooth path is found.

[0204] The purpose of the heuristic sampling method in the specific step S3.3 of the improved probabilistic roadmap algorithm is to improve the quality of the sampled poses and improve the efficiency of path planning. The heuristic sampling method is as follows: a) If the optimal robot collision-free smooth path connecting the robot pose and the target pose does not exist after the previous iteration, randomly sample poses in the entire prior map. b) If the optimal robot collision-free smooth path R connecting the robot pose and the target pose does exist after the previous iteration, first calculate the distance d of the optimal robot collision-free smooth path. R , and then randomly sample poses P in the entire prior map i , if the pose P i The sum of the “distances” to the robot pose and the target pose is less than d R , retain the sampled pose P i , otherwise resample.

[0205] The example is as follows Figure 6 In the "maze" map shown in (1), the map size is 10.0m*10.0m, the lower left corner of the map is the origin of the global coordinate system, the horizontal right is the X axis, and the horizontal upward is the Y axis. The black color in the map represents the obstacle area, and the white color represents the blank area. The robot can move freely in the blank area. It is known that the radius of the robot is 0.3m, the robot's posture is [1.0,2.0,-1.0], the target posture is [8.0,7.0,0.0], and the "distance" from the robot posture to the target posture is 9.06. When performing collision detection between the robot and the obstacle, if the shape of the robot can be simplified to a "point", the efficiency will be greatly improved. Therefore, the obstacles in the map are expanded, and the thickness of the expansion layer is 0.3m, as shown in Figure 6 shown in (2).

[0206] The improved probability roadmap algorithm based on Archimedean spiral designed by the present invention is used for path planning, and the design parameter k is taken as α =1, k β =5, n=20, m=3, d=5.0. First, use the Archimedean spiral to directly connect the robot pose and the target pose, such as Figure 7 As shown in the figure, it is obvious that this path is not a safe and feasible path. Next, heuristic sampling is performed and a feasible connection line between the sampled pose and the neighboring pose is generated. Figure 8 (1) Figure 8 (2) Figure 8 (3) Figure 8(4) are the results of the first, second, third and fourth sampling poses, respectively. Each sampling is n = 20 poses, and the sampling range is the entire map. The solid arrows in the figure represent the new sampling poses of the current iteration, the dotted arrows represent the historical sampling poses, and the thin dotted lines between the poses represent the feasible paths of the Archimedean spiral. After the construction of the directed roadmap of each iteration is completed, the A* algorithm is used to search the graph. After the first three iterations, the path from the robot pose to the target pose does not exist. After the fourth iteration, the optimal feasible path from the robot pose to the target pose of the current iteration is found, as shown in Figure 8 As shown by the thick solid line in (4). After finding the current optimal feasible path, the scope of heuristic sampling will be greatly reduced. Figure 8 (5) is the result of the fifth iteration of sampling poses (the Archimedean spiral feasible path between poses is no longer displayed). It can be seen that the new sampling poses are relatively concentrated in the vicinity of the current optimal feasible path. Figure 8 (6) Figure 8 (7), Figure 8 (8) are the results of the 10th, 20th, and 29th iteration sampling poses, respectively. It can be seen that the optimal feasible path of the current iteration is constantly improving. As the sampling progresses, the total "distance" of the optimal feasible path continues to decrease, as shown in Figure 9 As shown in the figure, after the 29th sampling, the optimal path has not been improved for m = 3 consecutive times, and the algorithm ends. The final optimal feasible path contains five poses: robot pose [1.0, 2.0, -1.0], sample pose [1.74, 2.11, 0.40], sample pose [4.00, 2.99, 0.31], sample pose [7.28, 6.37, 1.39], and target pose [8.0, 7.0, 0.0]. The poses are connected by Archimedean spirals.

[0207] Although the robot's linear velocity v is preliminarily designed in step S2, this velocity is only designed based on the distance from the target position and does not take into account the robot's motion constraints. Therefore, the robot cannot actually move at the planned speed. Moreover, the path obtained in step S3 generally includes the robot posture, the target posture, and several sample postures. If the expected linear velocity is designed according to equation (1.1), the linear velocity gradually decreases to 0 when the robot approaches the sample posture, and gradually increases when it leaves the sample posture. This is obviously unreasonable because the robot does not need to slow down at the sample posture in the middle of the path. Therefore, after completing the path planning of the improved probabilistic roadmap algorithm based on the Archimedean spiral, the robot's velocity needs to be replanned so that the robot can reach the target posture "quickly and steadily."

[0208] Robot velocity planning, given the robot's motion constraints, including the maximum linear velocity v max , maximum angular velocity ωmax , maximum linear acceleration av max , maximum angular acceleration aω max , as well as the robot's linear velocity v0 and angular velocity ω0 at the beginning of planning.

[0209] In a specific implementation, step S4 includes the following steps:

[0210] Step S4.1. First, in order to reduce the linear velocity to 0 when the path curvature is infinite, the robot rotates in place at the inflection point of the broken line. When the path curvature is 0, the expected linear velocity is the maximum linear velocity of the robot. When moving in a straight line, the linear velocity reaches the maximum value. The expected linear velocity of the robot based on the curvature of the Archimedean spiral path is designed. According to the maximum linear velocity of the robot, the relationship between the expected linear velocity of the robot and the curvature of the Archimedean spiral path is constructed according to the following formula:

[0211]

[0212] Where k3 is the preset design parameter, k3>0; κ is the path curvature, which is a known quantity, v max is the maximum linear speed of the robot;

[0213] Step S4.2, then every preset interval s on the optimal robot collision-free smooth path from the robot pose to the target pose L , s L =1cm and take a point as the intermediate pose point, so as to discretize the optimal robot collision-free smooth path into a smooth path vector L. The expression of the smooth path vector is as follows:

[0214] L=[P0,P1,…,P i ,P i+1 ,…,P I ,P t ]

[0215] Among them, the starting point P0 is the robot posture, and the end point P t is the target point pose, [P1,…,P i ,P i+1 ,…P I ] is the intermediate pose point, and there are I intermediate pose points in total;

[0216] Step S4.3: Then, smooth the path vector from the starting point P0 to the end point P t The forward velocity planning is used to obtain the robot moving forward along the optimal robot collision-free smooth path, and the robot reaches the i-th intermediate pose point P i The expected linear velocity v i ;

[0217] Step S4.4: Finally, smooth the path vector end point Pt The reverse speed planning to the starting point P0 is obtained, and when the robot moves in the reverse direction along the optimal robot collision-free smooth path, the robot reaches the i-th intermediate posture point P i The expected linear velocity v i ;

[0218] Step S4.5: Based on the optimal robot collision-free smooth path and the expected linear speed of the robot when moving forward and backward along the optimal robot collision-free smooth path, the motion planning of the robot is finally completed.

[0219] Step S4.3 includes the following steps:

[0220] Step S4.3.1. First, determine the planned linear velocity and angular velocity of the robot at the starting point P0 as the current actual linear velocity v0 and actual angular velocity ω0 of the robot, and initialize the intermediate pose point number i = 1;

[0221] Step S4.3.2: Preliminarily determine the intermediate pose point P based on the relationship between the robot's expected linear velocity and the curvature of the Archimedean spiral path. i The expected linear velocity v i , and according to the intermediate pose point P i The expected linear velocity v i and path curvature κ i , preliminarily determine the intermediate pose point P according to the following formula i The expected angular velocity ω i :

[0222] ω i =v i ×κ i

[0223] Step S4.3.3, according to the intermediate pose point P i-1 The expected linear velocity v i-1 and angular velocity ω i-1 , according to the maximum linear acceleration av max and angular acceleration aω max For the intermediate pose point P i The expected linear velocity v i Perform constraint adjustment. The specific constraint adjustment methods are as follows:

[0224] 1) If the intermediate pose point P i Expected linear speed Then order

[0225] 2) If the intermediate pose point P i The expected angular velocity Then order v i =|ωi ×κ i |, where θ i is the intermediate pose point P i The current heading angle relative to the X axis;

[0226] 3) Otherwise, the intermediate pose point P is incorrect i The expected linear velocity v i Make adjustments;

[0227] If the intermediate pose point P i Expected linear speed And the intermediate pose point P i The expected angular velocity Then the intermediate pose point P is not i The expected linear velocity v i Make adjustments;

[0228] Step S4.3.4: Update the intermediate pose point sequence number i to i+1, and repeat steps S4.3.2 to S4.3.4 until the intermediate pose point sequence number i is greater than the total number of intermediate path points I, completing the forward velocity planning.

[0229] Step S4.4 includes the following steps:

[0230] Step S4.4.1: First, define the robot at the end point P t The planned linear speed v t and the planned angular velocity ω t are all 0, and the intermediate pose point number i=I is initialized;

[0231] Step S4.4.2, according to the intermediate pose point P i+1 The expected linear velocity v i+1 and angular velocity ω i+1 , according to the maximum linear acceleration av max and angular acceleration aω max For the intermediate pose point P i The expected linear velocity v i Perform constraint adjustment. The specific constraint adjustment methods are as follows:

[0232] 1) If the intermediate pose point P i Expected linear speed Then order

[0233] 2) If the intermediate pose point P i The expected angular velocity Then order v i =|ω i ×κ i |, where θ i is the intermediate pose point Pi-1 The current heading angle relative to the X axis;

[0234] 3) Otherwise, the intermediate pose point P is incorrect i The expected linear velocity v i Make adjustments;

[0235] If the intermediate pose point P i Expected linear speed And the intermediate pose point P i The expected angular velocity Then the intermediate pose point P is not i The expected linear velocity v i Make adjustments;

[0236] Step S4.4.3, update the intermediate pose point sequence number i to i-1, repeat steps S4.4.2 to S4.4.3 until the intermediate pose point sequence number i is 0, and complete the reverse velocity planning.

[0237] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or make equivalent replacements for some or all of the technical features therein. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A mobile robot motion planning method using Archimedean spiral, characterized in that: The following steps are involved: Step S1: First, establish a polar coordinate system with the target position as the pole and the target posture as the polar axis, and then establish a robot kinematic model in the polar coordinate system; Step S2: without considering obstacles, establish the robot's initial planned path based on the robot's kinematic model and Archimedean spiral; The step S2 is specifically as follows: Step S2.1: First, without considering any obstacles, set the robot's initial linear velocity v according to the following formula based on the distance r between the robot and the target position: Where k1 and k2 are given adjustment coefficients, k1>0, k2>0; r is the distance between the robot's current position and the target position; Step S2.2: Next, without considering obstacles, according to the kinematic model of the Archimedean spiral and the robot in the polar coordinate system T, the angle α and the angle β satisfy the following relationship: β=arctan(-k α α) Among them, k α is the preset proportional coefficient, satisfying k α >0; α is the angle of the target orientation relative to the robot's line of sight to the target position; β is the angle of the robot orientation relative to the robot's line of sight to the target position; Step S2.3: Then, without considering obstacles, the robot's angular velocity ω is obtained according to the following formula: Where v is the initial linear velocity of the robot without considering obstacles; k α is the preset proportional coefficient; α is the angle of the target direction relative to the robot's line of sight to the target position; k β is the preset scale factor; the function NormalizeAngle(angle) converts the angle angle to the interval (-π,π]; Step S3: Based on the improved probabilistic roadmap algorithm, the initial planned path of the robot established in step S2 is optimized to obtain the optimized optimal collision-free smooth path of the robot; Step S4: Based on the curvature of the Archimedean spiral path and the optimal robot collision-free smooth path of step S3, the optimized robot expected speed is obtained, and finally the motion planning of the robot is realized.

2. The method for mobile robot motion planning using Archimedean spiral according to claim 1, characterized in that: The step S1 is specifically as follows: Step S1.1: First, the robot pose P0 and the target pose P are aligned in the global coordinate system O-XY. t Define the robot pose P0 and target pose P t The vector expression of is as follows: P0=[x0,y0,θ0] T P t [x t ,y t ,θ t ] T Among them, x0 and y0 are the current horizontal and vertical coordinates of the robot respectively; θ0 is the angle of the robot's current orientation relative to the X axis; t and y t are the horizontal and vertical coordinates of the target position, θ t The angle of the target direction relative to the X axis; Step S1.2: Next, establish the kinematic model of the robot in the global coordinate system O-XY according to the following formula: Where v is the linear velocity of the robot; ω is the angular velocity of the robot; θ is the angle of the robot; is the component of the robot’s linear velocity on the X-axis, is the component of the robot's linear velocity on the Y axis; is the time derivative of θ; Step S1.3: Establish a polar coordinate system T with the target position as the pole and the target posture as the polar axis. The robot state s in the polar coordinate system T is expressed as follows: Where r is the distance between the robot's current position and the target position; α is the angle of the target heading relative to the robot's line of sight to the target position; β is the angle of the robot heading relative to the robot's line of sight to the target position; represents the set of positive real numbers; Step S1.4: Based on the kinematic model of the robot in the global coordinate system O-XY, the kinematic model of the robot in the polar coordinate system T is obtained according to the following formula: in, is the time derivative of r, is the time derivative of α, is the time derivative of β.

3. The method for mobile robot motion planning using Archimedean spiral according to claim 1, wherein: The step S3 is specifically as follows: Step S3.1: First, construct a directed graph G(V,E) and initialize the directed graph, where V represents the pose set and E represents the line set; Step S3.2: Add the robot pose and the target pose to the pose set V, then connect the robot pose and the target pose using an Archimedean spiral, use the Archimedean spiral as the current path, and perform collision detection on the current path: If the current path is determined to be a safe path after collision detection, the current path will be used as the optimized optimal robot collision-free smooth path; Otherwise, set the current iteration number iter to 1 and execute the next step S3.3; Step S3.3: Sample new pose P in the prior map by heuristic sampling method i , and judge the new sampling pose P i Rationality: If the newly sampled pose P i If it is not in the obstacle of the prior map, the newly sampled pose P i Add to the pose set V; Otherwise, resample until the new sampled pose P i Not among the obstacles in the prior map; Step S3.4: Construct the neighborhood line set E' of the new sampling pose and obtain the new sampling pose P i The pose distance D from the pose P in the pose set V is the same as the pose P in the pose set V. i If the pose distance D between them is less than the distance threshold d, the Archimedean spiral is used to connect the pose P in the pose set V and the new sampled pose P i , add the Archimedean spiral to the neighborhood line set E'; Step S3.5: Check the feasibility of the robot moving along the path in the neighborhood line set E': If the robot does not collide with an obstacle while moving along the path in the neighborhood link set E', then the Archimedean spiral in the neighborhood link set E' is added to the link set E; Otherwise, the Archimedean spiral in the neighborhood link set E' is not added to the link set E; Step S3.6, repeat steps S3.3 to S3.5 until the number of poses in the pose set V reaches the preset number of sampling poses required iter*n, where n represents the preset number of sampling poses; Step S3.7: Use the graph search algorithm to search the current directed graph G(V,E): If a feasible path from the robot pose to the target pose exists, the current feasible path is used as the collision-free smooth path of the robot in the current iteration; If a feasible path from the robot pose to the target pose does not exist, increase the value of the current iteration number iter by 1 and repeat steps S3.3 to S3.7 until a feasible path from the robot pose to the target pose exists, and then proceed to step S3.8; Step S3.8: Determine whether the robot collision-free smooth path of the current iteration is the optimal robot collision-free smooth path: If the collision-free smooth path of the robot obtained in the current iteration is not optimized after m consecutive iterations, the collision-free smooth path of the robot obtained in the current iteration is used as the optimal collision-free smooth path of the robot after optimization; Otherwise, increase the value of the current iteration number iter by 1 and repeat steps S3.3 to S3.8 until the optimal robot collision-free smooth path is found.

4. The method for mobile robot motion planning using Archimedean spiral according to claim 1, wherein: The step S4 is specifically as follows: Step S4.1: Based on the maximum linear velocity of the robot, construct the relationship between the desired linear velocity of the robot and the curvature of the Archimedean spiral path according to the following formula: Where k3 is the preset design parameter; κ is the path curvature, v max is the maximum linear speed of the robot; Step S4.2, then every preset interval s on the optimal robot collision-free smooth path from the robot pose to the target pose L Take a point as the intermediate pose point to discretize the optimal robot collision-free smooth path into a smooth path vector L. The expression of the smooth path vector is as follows: L=[P0,P1,…,P i ,P i+1 ,…,P I ,P t ] Among them, the starting point P0 is the robot posture, and the end point P t is the target point pose; Step S4.3: Then, smooth the path vector from the starting point P0 to the end point P t The forward velocity planning is used to obtain the robot moving forward along the optimal robot collision-free smooth path, and the robot reaches the i-th intermediate pose point P i The expected linear velocity v i ; Step S4.4: Finally, smooth the path vector end point P t The reverse speed planning to the starting point P0 is obtained, and when the robot moves in the reverse direction along the optimal robot collision-free smooth path, the robot reaches the i-th intermediate posture point P i The expected linear velocity v i ; Step S4.5: Based on the optimal robot collision-free smooth path and the expected linear speed of the robot when moving forward and backward along the optimal robot collision-free smooth path, the motion planning of the robot is finally completed.

5. The method for mobile robot motion planning using Archimedean spiral according to claim 3, wherein: In step S3.4, the new sampling pose P i The pose distance D from the pose P in the pose set V is obtained by the following formula: Among them, r d is the position distance between the two poses, α d is the new sampled pose P i Orientation relative to pose P to the new sample pose P i Angle of sight direction; β d is the orientation of pose P relative to pose P to the new sample pose P i The angle of the viewing direction.

6. The method for mobile robot motion planning using Archimedean spiral according to claim 4, characterized in that: The step S4.3 is specifically as follows: Step S4.3.

1. First, determine the planned linear velocity and angular velocity of the robot at the starting point P0 as the current actual linear velocity v0 and actual angular velocity ω0 of the robot, and initialize the intermediate pose point number i = 1; Step S4.3.2: Preliminarily determine the intermediate pose point P based on the relationship between the robot's expected linear velocity and the curvature of the Archimedean spiral path. i The expected linear velocity v i , and according to the intermediate pose point P i The expected linear velocity v i and path curvature κ i , preliminarily determine the intermediate pose point P according to the following formula i The expected angular velocity ω i : oh i =v i ×k i Step S4.3.3, according to the intermediate pose point P i-1 The expected linear velocity v i-1 and angular velocity ω i-1 , according to the maximum linear acceleration av max and angular acceleration aω max For the intermediate pose point P i The expected linear velocity v i Perform constraint adjustment. The specific constraint adjustment methods are as follows: If the intermediate pose point P i Expected linear speed Then order If the intermediate pose point P i The expected angular velocity Then order v i =|ω i ×κ i |, where θ i is the intermediate pose point P i The current heading angle relative to the X axis; Otherwise, the intermediate pose point P i The expected linear velocity v i Make adjustments; Step S4.3.4: Update the intermediate pose point sequence number i to i+1, and repeat steps S4.3.2 to S4.3.4 until the intermediate pose point sequence number i is greater than the total number of intermediate path points I, completing the forward velocity planning.

7. The method for mobile robot motion planning using Archimedean spiral according to claim 4, characterized in that: The step S4.4 is specifically as follows: Step S4.4.1: First, define the robot at the end point P t The planned linear speed v t and the planned angular velocity ω t are all 0, and the intermediate pose point number i=I is initialized; Step S4.4.2, according to the intermediate pose point P i+1 The expected linear velocity v i+1 and angular velocity ω i+1 , according to the maximum linear acceleration av max and angular acceleration aω max For the intermediate pose point P i The expected linear velocity v i Perform constraint adjustment. The specific constraint adjustment methods are as follows: If the intermediate pose point P i Expected linear speed Then order If the intermediate pose point P i The expected angular velocity Then order v i =|ω i ×κ i |, where θ i is the intermediate pose point P i-1 The current heading angle relative to the X axis; Otherwise, the intermediate pose point P i The expected linear velocity v i Make adjustments; Step S4.4.3, update the intermediate pose point sequence number i to i-1, repeat steps S4.4.2 to S4.4.3 until the intermediate pose point sequence number i is 0, and complete the reverse velocity planning.

Images