Mobile robot motion planning method using Archimedes spiral

Abstract

The invention discloses a mobile robot motion planning method using an Archimedes spiral. The motion planning method comprises the steps that a robot kinematics model under a polar coordinate system with a target position as a pole and a target posture as a polar axis is established; under the condition that obstacles are not considered, robot path planning is conducted through the Archimedes spiral; an improved probability route map path planning algorithm of the Archimedes spiral is combined, and an optimized robot collision-free smooth path is obtained; and based on the Archimedes spiral curvature and a speed planning algorithm meeting robot motion capability constraints, the optimized expected speed of the robot is obtained, and finally motion planning of the robot is achieved. According to the method, the optimal path planning can be quickly realized in a complex environment, the generated path meets kinematics constraints and is smooth and visual, the planning speed considers the influence of curvature on motion stability, and the motion capability constraints of the robot are considered, so that the robot can quickly and stably reach the target pose.

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 into various application scenarios in the fields of industry, scientific research, and home. It is an inevitable trend for them to replace humans in completing complex and arduous tasks. In order to achieve efficient and intelligent autonomous movement in complex environments, mobile robots need to have functions such as positioning, perception, motion planning, and motion control. The basic problem of motion planning is to find a collision-free path from the starting position to the target position in a complex environment containing obstacles, and to add time series information on the basis of path planning, that is, speed planning. The motion planning problem of mobile robots is still very challenging, especially considering the robot's kinematic constraints, motion capacity constraints, path smoothness, time optimality, and other issues at the same time.

[0003] The widely used robot motion planning methods can be mainly divided into three categories: sampling-based, search-based, and optimization-based. However, these methods have their 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*, Dijkstra, etc., can quickly find a collision-free path from the starting point to the target point, but they usually use the Euclidean distance to measure the distance between the robot and the target. Obviously, for mobile robots with non-holonomic constraints (such as robots that cannot move sideways), the Euclidean distance cannot reflect the true cost of the robot moving from one posture to another, which leads to the inability of these algorithms to effectively explore the space. In addition, the broken line paths obtained by these algorithms often do not consider the kinematic constraints of the robot, resulting in the robot's movement along the path being unreasonable and not smooth. Although the motion planning method based on nonlinear optimization can optimize a smooth and safe trajectory, it is only a local optimum and requires a good initial guess for the trajectory and time range. In addition, there are generally problems such as large computational complexity, low solution efficiency, and relatively poor real-time performance. Therefore, despite recent progress in the field of mobile robot motion planning, most existing work focuses only on ensuring that the robot can successfully reach the target pose. However, to achieve 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, firstly 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 initial planning path of the robot according to the robot kinematic model and Archimedean spiral;

[0008] Step S3, based on the improved probabilistic roadmap algorithm, optimizing the initial planning path of the robot established in step S2 to obtain an optimized optimal robot collision-free smooth path;

[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 expected speed of the robot 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 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 current position of the robot and the target position; α is the angle of the target orientation relative to the line of sight from the robot to the target position; β is the angle of the robot orientation relative to the line of sight from the robot 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 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 current position of the robot 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 proportionality coefficient, satisfying k α >0; α is the angle of the target orientation relative to the robot's line of sight from the target position; β is the angle of the robot orientation relative to the robot's line of sight from 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 proportional coefficient; 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 a pose set and E represents a connection set;

[0036] Step S3.2, add the robot posture and the target posture to the posture set V, then use the Archimedean spiral to connect the robot posture and the target posture, 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 is 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 determine 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 in the obstacles of the a priori map;

[0042] Step S3.4: Construct a neighborhood line set E' of the new sampling pose to obtain the new sampling pose P i The pose distance D from the pose P in the pose set V is when the pose P in the pose set V is close to the newly sampled pose P 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 line set E', then the Archimedean spiral in the neighborhood line set E' is added to the line 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, repeating 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's posture to the target posture exists, the current feasible path is used as the robot's collision-free smooth path for the current iteration;

[0049] If a feasible path from the robot's position to the target position 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's position to the target position 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 robot collision-free smooth path obtained in the current iteration has not been optimized after m consecutive iterations, the robot collision-free smooth path obtained in the current iteration is used as the optimal robot collision-free smooth path 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 expression between the expected 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, on the optimal robot collision-free smooth path from the robot posture to the target posture, every preset interval s 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] The starting point P0 is the robot posture, and the end point P t is the target point pose;

[0060] Step S4.3: Next, 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 opposite 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 reverse along the optimal robot collision-free smooth path, finally complete the motion planning of the robot.

[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 sampled pose P i The angle of the sight 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 sequence number i=1;

[0068] Step S4.3.2: Preliminarily determine the intermediate position point P based on the relationship expression between the robot's expected linear velocity and the Archimedean spiral path curvature. 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 Make constraint adjustments. The specific constraint adjustment methods are as follows:

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

[0072] If the intermediate pose point P i The expected angular velocity Then 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 is not iThe expected linear velocity v i Make adjustments;

[0074] Step S4.3.4, update the intermediate pose point sequence number i to i+1, 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, and complete 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 All are 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 Make constraint adjustments. The specific constraint adjustment methods are as follows:

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

[0079] If the intermediate pose point P i The expected angular velocity Then 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 is not 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 purpose of the present invention is to quickly complete smooth and intuitive motion planning (including path planning and speed planning) from any given initial posture to any target posture in a complex environment, bypassing obstacles. The present invention proposes an improved probabilistic roadmap algorithm that combines the traditional sampling-based probabilistic roadmap algorithm and the Archimedean spiral to achieve path planning; and proposes a speed planning algorithm based on the curvature of the Archimedean spiral that meets the robot's motion capability 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 also is 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 the Archimedean spiral, and can quickly achieve optimal path planning in a complex environment.

[0086] 3. The present invention proposes a speed planning method, designs the expected linear speed 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 A flow chart of robot motion planning using Archimedean spiral proposed by the present invention;

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

[0089] Figure 3 It is a schematic diagram of the robot state expressed in the polar coordinate system;

[0090] Figure 4 It is 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 position to the target position;

[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 of the process of path planning using the improved probabilistic roadmap algorithm;

[0095] Fig. 9 It is a graph of the total “distance” change from the robot’s posture to the optimal feasible path of the target posture. DETAILED DESCRIPTION

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

[0097] The present invention comprises the following steps:

[0098] Step S1, firstly 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 initial planning path of the robot according to the robot kinematic model and Archimedean spiral, such as Figure 2 As shown;

[0100] Step S3, based on the improved probabilistic roadmap algorithm, optimizing the initial planning path of the robot established in step S2 to obtain an optimized optimal robot collision-free smooth path;

[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 expected speed of the robot is obtained, and finally the motion planning of the robot is realized.

[0102] The specific step S1 is:

[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 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 is heading towards θ t is a negative value, and the robot is heading toward θ0 is a positive value;

[0107] The robot in the present invention adopts a differential robot. As is 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 under-driven, 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] Where 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 selected coordinate systems will result in different kinematic models describing the robot's motion. To facilitate the design of the Archimedean spiral from the current position to the target position of the robot, 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 current position of the robot and the target position; α is the angle of the target orientation relative to the line of sight from the robot to the target position; β is the angle of the robot orientation relative to the line of sight from the robot to the target position; counterclockwise is negative and clockwise is positive, (-π,π] represents the left-open and right-closed interval from negative π to positive π, r, α describe the position of the robot, and β describes the orientation of the robot. Figure 3 The α 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 represented by the global coordinate system O-XY and the robot state s represented by 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 the 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 , thus, the robot posture P0 = [x0, y0, θ0] expressed in the global coordinate system O-XY T for:

[0119]

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

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

[0122] 2) The robot pose P0 = [x0, y0, θ0] expressed in the known global coordinate system O-XY T and the target pose P t =[x t ,y t ,θ t ] T , the robot state s = [r, α, β] represented 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 right as the X axis, and the horizontal upward 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 set 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 posture to any target posture in the global coordinate system.

[0133] Step S2.1: In order to ensure that the robot can move to the target position, first, without considering obstacles, the robot's initial linear velocity v is set 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 the 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 capability constraints are not considered in the preliminary planning. In step S4, a detailed speed plan considering the motion capability constraints is given; 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 proportionality 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 proportional coefficient; 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 from 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 proportionality coefficient k α Satisfy k α >0, is a given design parameter, so that the form of equation (1) is the same as that of equation (2), 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. The Archimedean spiral is still an Archimedean spiral after rotation and mirroring. 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 proportionality coefficient k α Determines how tightly the spiral is “wrapped”, k α → When k is 0, the robot's motion mode is simplified to "rotate in place - linear motion - rotate in place", α >>0, the convergence speed of the angle α in the robot state is much faster than the distance r.

[0151] Moreover, it can be proved that if the robot's linear velocity v>0, and α and β satisfy equation (3), for any robot initial state 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 actual orientation of the robot can quickly track the expected orientation.

[0160] Define the tracking error e β :

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

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

[0163]

[0164] Among them, the dot on 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 the feedback method to compensate the nonlinear controlled object into a linear system, and then use the 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 for:

[0167]

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

[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 robot path planning using Archimedean spiral, the robot's linear velocity v and angular velocity ω satisfy equations (1.1) and (9) respectively. The design parameter k is α =1, k β =5, Figure 5 Given the robot's transition from initial position P0 to target position P t =[x t =0,y t =0,θ t =0] T The initial position 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 spiral in step S2 can only be applied to scenes without obstacles because the feasibility of the planned path is not judged, that is, whether the robot will collide with obstacles in the environment along the planned path. However, for scenes with widespread obstacles, the robot needs to have obstacle avoidance capabilities. Therefore, based on step S2, an improved probabilistic roadmap algorithm is proposed. The probabilistic roadmap (PRM: Probabilistic Road Map) algorithm is common knowledge in this field. It is a sampling-based path planning method that can find the shortest path when a map is known. The basic steps of the probabilistic roadmap algorithm are to first construct a roadmap through sampling and collision detection, and then find the shortest feasible path on the constructed roadmap through a graph search algorithm. There are two main problems with the probabilistic roadmap algorithm: first, it simply connects two points, and the planned broken line path cannot meet the robot's kinematic constraints; second, it only measures the distance between two position points based on length, which cannot truly reflect the "distance" between two postures. Different from 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 Archimedean spiral is planned to connect the two postures using the method of step S2, 3) instead of using length to represent the distance, the "distance" between the two postures is defined as:

[0175]

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

[0177] like Figure 1 As shown, the input, output and specific steps of the improved probability 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] Wherein step S3 is specifically:

[0181] 1. Build a roadmap:

[0182] Step S3.1, first construct a directed graph G(V,E) and initialize the directed graph, where V represents a set of postures and E represents a set of lines; specifically, V is a set of collision-free vertices (postures) in the directed graph G(V,E), and E is a set of collision-free lines (Archimedes spirals connecting two postures) in the directed graph G(V,E), and the initial states of V and E are both empty sets;

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

[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 a new pose P in the prior map by a heuristic sampling method i , and determine 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 in the obstacles of the a priori map;

[0189] Among them, the prior map refers to the map of the robot's 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, and obtain the new sampling pose P i The pose distance D from the pose P in the pose set V is 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 sight 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 line set E', then the Archimedean spiral in the neighborhood line set E' is added to the line 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, repeating 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 which point the roadmap construction is completed;

[0197] 2. Graph Search:

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

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

[0200] If a feasible path from the robot's position to the target position 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's position to the target position 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 continuous optimization times, the robot collision-free smooth path obtained in the current iteration is 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 above improved probabilistic roadmap algorithm is to improve the quality of the sampled poses and improve the efficiency of path planning. The specific 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 last 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 last 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 , keep 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's posture to the target posture is 9.06. When performing collision detection between the robot and obstacles, 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, such as Figure 6 shown in (2).

[0206] The improved probabilistic route map 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 posture and the target posture, 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 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 time, n = 20 poses are sampled, 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 perform graph search. 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 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 sampled 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 proceeds, the total "distance" of the optimal feasible path continues to decrease, as shown in Fig. 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 5 postures, namely robot posture [1.0, 2.0, -1.0], sampling posture [1.74, 2.11, 0.40], sampling posture [4.00, 2.99, 0.31], sampling posture [7.28, 6.37, 1.39], and target posture [8.0, 7.0, 0.0]. The postures 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, without considering the robot's motion constraints, so 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 sampling postures. If the expected linear velocity is designed according to formula (1.1), the linear velocity gradually decreases to 0 when the robot approaches the sampling posture, and gradually increases when it leaves the sampling posture. This is obviously unreasonable because the robot does not need to slow down at the sampling 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 "fast and stable".

[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 start 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, rotate in situ 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 Archimedean spiral path curvature is designed. According to the maximum linear velocity of the robot, the relationship expression between the expected linear velocity of the robot and the Archimedean spiral path curvature 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, and v max is the maximum linear speed of the robot;

[0213] Step S4.2: Then, on the optimal robot collision-free smooth path from the robot posture to the target posture, every preset interval s L ,s L =1cm, a point is taken 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] 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 middle pose point, there are I middle pose points in total;

[0216] Step S4.3: Next, 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 opposite 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 reverse along the optimal robot collision-free smooth path, finally complete the motion planning of the robot.

[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 sequence number i=1;

[0221] Step S4.3.2: Preliminarily determine the intermediate position point P based on the relationship expression between the robot's expected linear velocity and the Archimedean spiral path curvature. 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 Make constraint adjustments. The specific constraint adjustment methods are as follows:

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

[0225] 2) If the intermediate pose point P i The expected angular velocity Then 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 line 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, 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, and complete 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 All are 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 Make constraint adjustments. The specific constraint adjustment methods are as follows:

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

[0233] 2) If the intermediate pose point P i The expected angular velocity Then 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 line 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 aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. However, these modifications or replacements do not make the essence of the corresponding technical solutions 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 initial planning path of the robot according to the robot kinematic model and Archimedean spiral; Step S3, based on the improved probabilistic roadmap algorithm, optimizing the initial planning path of the robot established in step S2 to obtain an optimized optimal robot collision-free smooth path; Step S4: Based on the curvature of the Archimedean spiral path and the optimal robot collision-free smooth path of step S3, the optimized expected speed of the robot 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 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 current position of the robot and the target position; α is the angle of the target orientation relative to the line of sight from the robot to the target position; β is the angle of the robot orientation relative to the line of sight from the robot 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, characterized in that: The step S2 is specifically as follows: Step S2.1: First, without considering 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 current position of the robot 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 proportionality coefficient, satisfying k α >0; α is the angle of the target orientation relative to the robot's line of sight from the target position; β is the angle of the robot orientation relative to the robot's line of sight from 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 proportional coefficient; the function NormalizeAngle(angle) means converting the angle angle to the interval (-π,π].

4. The method for mobile robot motion planning using Archimedean spiral according to claim 1, characterized in that: 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 a pose set and E represents a connection set; Step S3.2, add the robot posture and the target posture to the posture set V, then use the Archimedean spiral to connect the robot posture and the target posture, 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 is 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 a new pose P in the prior map by a heuristic sampling method i , and determine 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 in the obstacles of the a priori map; Step S3.4: Construct a neighborhood line set E' of the new sampling pose to obtain the new sampling pose P i The pose distance D from the pose P in the pose set V is 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 line set E', then the Archimedean spiral in the neighborhood line set E' is added to the line set E; Otherwise, the Archimedean spiral in the neighborhood link set E' is not added to the link set E; Step S3.6, repeating 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's posture to the target posture exists, the current feasible path is used as the robot's collision-free smooth path for the current iteration; If a feasible path from the robot's position to the target position 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's position to the target position 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 robot collision-free smooth path obtained in the current iteration has not been optimized after m consecutive iterations, the robot collision-free smooth path obtained in the current iteration is used as the optimal robot collision-free smooth path 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.

5. The method for mobile robot motion planning using Archimedean spiral according to claim 1, characterized in that: The step S4 is specifically as follows: Step S4.1, based on the maximum linear velocity of the robot, construct the relationship expression between the expected 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, on the optimal robot collision-free smooth path from the robot posture to the target posture, every preset interval s 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 ] The starting point P0 is the robot posture, and the end point P t is the target point pose; Step S4.3: Next, 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 opposite 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 reverse along the optimal robot collision-free smooth path, finally complete the motion planning of the robot.

6. The method for mobile robot motion planning using Archimedean spiral according to claim 4, characterized in that: 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 sampled pose P i The angle of the sight direction.

7. The method for mobile robot motion planning using Archimedean spiral according to claim 5, 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 posture point sequence number i=1; Step S4.3.2: Preliminarily determine the intermediate position point P based on the relationship expression between the robot's expected linear velocity and the Archimedean spiral path curvature. i The expected linear velocity v i , and according to the intermediate pose point P i The expected linear velocity v i and the 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 Make constraint adjustments. The specific constraint adjustment methods are as follows: If the intermediate pose point P i Expected line speed Then If the intermediate pose point P i The expected angular velocity Then 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 is not i The expected linear velocity v i Make adjustments; Step S4.3.4, update the intermediate pose point sequence number i to i+1, 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, and complete the forward velocity planning.

8. The method for mobile robot motion planning using Archimedean spiral according to claim 5, 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 All are 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 Make constraint adjustments. The specific constraint adjustment methods are as follows: If the intermediate pose point P i Expected line speed Then If the intermediate pose point P i The expected angular velocity Then 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 is not 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