Robot docking trajectory planning and path control method based on particle swarm optimization

Abstract

The invention discloses a robot docking trajectory planning and path control method based on particle swarm optimization. The method specifically comprises the following steps: determining robot docking task information; a B spline curve is used, and an initial monotonic track of docking motion of the mobile robot is generated through parametric modeling and a superposition control point sorting method; based on an improved multi-target particle swarm optimization algorithm, optimizing the initial monotonic trajectory from a plurality of targets by utilizing a self-adaptive grid, external memory updating and a variation mechanism until an optimal planning path is obtained; and according to an MPC tracking control technology of a linear error model, accurate tracking of a trajectory is realized through a discretized state equation and a quadratic objective function. The method provided by the invention can be applied to industrial automation large component docking and autonomous navigation of the mobile robot in a complex environment without a priori map, robot kinematics, computational geometry and a multi-objective optimization theory are fused, and a complete technical closed loop of track generation-optimization-tracking is realized.

Description

Technical Field

[0001] The present technology relates to the technical field of mobile robot path planning, and in particular to a robot docking trajectory planning and path control method based on particle swarm optimization. Background Art

[0002] Existing autonomous motion planning technologies for mobile robots generally rely on prior system models and have low versatility. Traditional methods require prior map information or precise modeling, and are not adaptable enough in complex scenarios such as unmapped environments and dynamic obstacles. In addition, the linkage between trajectory planning and tracking control is weak, making it difficult to achieve end-to-end integration of "perception-planning-control". For high-precision docking tasks, the trajectory must meet strict smoothness and local adjustability requirements, while traditional trajectory generation methods have limitations in computational efficiency or local controllability, and reciprocating motion may cause inertial impact and component damage. In addition, the docking process requires a balance between motion efficiency and smoothness, and is constrained by motor performance. Traditional single optimization algorithms are prone to falling into local optimality in multi-objective optimization, and the solution set lacks diversity, making it difficult to meet engineering needs. Summary of the Invention

[0003] In response to the problems existing in the above-mentioned existing technologies, the present invention proposes a robot docking trajectory planning and path control method based on particle swarm optimization to realize autonomous navigation of mobile robots used in the docking of large components in industrial automation (such as automobile and aerospace assembly) and complex environments without prior maps (such as warehousing logistics and emergency rescue). It integrates robot kinematics, computational geometry and multi-objective optimization theory to form a complete technical closed loop of "trajectory generation-optimization-tracking".

[0004] In order to achieve the above technical objectives, the present invention provides the following technical solutions:

[0005] The robot docking trajectory planning and path control method based on particle swarm optimization includes:

[0006] S1. Clarify the docking task information: Two robots complete the docking task. One robot is stationary, and its co-carrying component maintains its position unchanged. Under the condition that there are no path obstacles in the docking environment, the other robot is mobile and needs to continuously adjust its position until it docks with the component carried by the stationary mobile robot.

[0007] S2. Using B-spline curves, the initial monotonic trajectory of the mobile robot's docking motion is generated through parametric modeling and superposition control point sorting method;

[0008] S3, based on the improved multi-objective particle swarm optimization algorithm PDAG-MOPSO, uses adaptive grid, external memory update and mutation mechanism to optimize the initial monotonic trajectory from multiple objectives until the optimal planning path is obtained;

[0009] S4. Based on the MPC tracking control technology of the linear error model, the optimal control quantity is solved by discretizing the state equation and the quadratic objective function to achieve accurate trajectory tracking.

[0010] Furthermore, step S2 specifically includes:

[0011] S21. Generate the initial trajectory of the robot docking motion using a B-spline curve, set constraints and trajectory performance requirements, and specify the B-spline curve order, number of control points, node vector length, and the repetition of control points and node vector endpoints;

[0012] S22, using a superposition control point sorting method to perform monotonic planning on the initial trajectory of the robot docking motion to obtain an initial monotonic trajectory;

[0013] S23. Perform multi-order derivatives on the initial monotonic trajectory to derive the velocity curve, acceleration curve, emergency stop curve, and corresponding control point sequence of the robot's docking motion.

[0014] More specifically, step S22 is as follows:

[0015] First, the initial trajectory of each direction is planned separately, and then the planned trajectories in each direction are merged into a complete initial monotonic trajectory. For the initial docking trajectory in any direction, the repetition degree of the initial and terminal control points is set to 3. Except for the initial and terminal control points in the control point sequence, the remaining intermediate control points are rearranged by superposition and division. The formula is expressed as follows:

[0016]

[0017] in, p l is the initial value of the randomly initialized control point; p e 、p s is the position of the initial and terminal control points; is the control point sequence after planning.

[0018] Furthermore, step S3 specifically includes:

[0019] S31. Determine the evaluation objectives and constraints, and transform the trajectory optimization problem into a multi-objective optimization problem;

[0020] S32. Use the multi-objective particle swarm optimization algorithm PDAG-MOPSO based on Pareto-dominated adaptive grid to solve the multi-objective optimization problem and obtain the Pareto frontier solution set;

[0021] S33. Use the average evaluation method to select the optimal trajectory from the Pareto frontier solution set.

[0022] More specifically, step S31 includes:

[0023] S311, the maximum value of the absolute value of the jerk of the mobile robot in each direction of the docking trajectory is set as the primary evaluation target of the docking trajectory, and the objective function The formula is expressed as:

[0024]

[0025] Among them, |j x |、|j y |、|j z | are the absolute values ​​of jerk in the x-axis, y-axis, and z-axis directions respectively; is the parameter vector that comprehensively reflects the trajectory characteristics;

[0026] The total time consumed by the mobile robot during docking is set as the secondary evaluation target, and the formula of the target evaluation function is expressed as follows: The total time is obtained based on the velocity curve of the initial monotonic trajectory.

[0027] S312, set the mobile robot to remain stationary at the starting point and the end point, that is, the speed at the starting point and the end point and acceleration is zero; set the motor performance constraints, and during operation, the speed and output torque of each motor of the robot are within the rated range; the final multi-objective optimization problem is expressed as:

[0028]

[0029] in, Evaluation function; are the speeds at the starting point and the end point respectively; are the accelerations at the starting point and the end point respectively; τ, τ max are the motor speed and speed rating respectively; a, a max They are the motor output torque and output torque rated value respectively.

[0030] Furthermore, step S32 specifically includes:

[0031] S321, initializing the population size, external memory capacity, and number of grids;

[0032] S322, deriving the initial monotonic trajectory to obtain a velocity curve and a jerk curve, and calculating the evaluation function value of each particle; based on the kinematic and dynamic models of the mobile robot, determining the motor speed and output torque corresponding to the trajectory, and setting the evaluation function value of particles that exceed the constraint conditions to a maximum value; storing the initial non-dominated particles in an external memory using the Pareto dominance relationship, and recording them as a rep set;

[0033] Determine the initial number of grids for each dimension in the target space, and then determine the initial grid interval range based on the values ​​of each objective function in the first iteration; the formula is expressed as:

[0034]

[0035] Among them, d w is the width of each grid in the w-th dimension of the target space; K w is the number of grid divisions in the w-th dimension; are the maximum and minimum values ​​of the w-th objective function respectively;

[0036] Suppose that after one iteration, a new non-dominated particle sequence a=(a1,a2,a3,L,a m ), then the specific position of each non-dominated particle in the target space for:

[0037]

[0038] Among them, floor() is a rounding function; then each non-dominated particle is assigned to the corresponding grid;

[0039] S323, determine the global optimal particle; based on the grid density, use the global optimal particle update strategy to select the global optimal particle gbest from the rep set, specifically: calculate the fitness fit = exp(-β*n based on the grid density for each grid containing non-dominated particles g ), where β is the fitness coefficient, n g is the number of non-dominated particles in the grid; the roulette wheel selection method is applied to select the grid with the highest fitness, and a particle is randomly selected from it as the global optimal particle gbest of this iteration to guide the movement adjustment of the remaining particles;

[0040] S324. Adjust the speed and position of each particle and ensure that the particles meet the boundary conditions; at the same time, apply a small probability random mutation to the updated population particles; if the mutated trajectory does not meet the monotonicity condition, re-sort it by superimposing the control point sorting method;

[0041] After that, the evaluation function value of each particle is recalculated, and the local optimal particle pbest is updated according to the local optimal particle update rule; specifically, two particles are selected to determine the dominance relationship, and the non-dominated particle is used as the local optimal particle pbest. If the two particles do not dominate each other, one is randomly selected as the local optimal particle pbest.

[0042] S325, after obtaining the local optimal particle pbest, update the rep set and the adaptive grid; and detect whether the rep set overflows: when the number of non-dominated particles in the rep set is less than the set capacity n rep When the non-dominated particles generated by the current iteration are added to the memory; if the number of non-dominated particles in the external memory exceeds n rep , then delete the non-dominated particles in the grid with the highest grid density in turn, and re-compare the density after each deletion until the number of non-dominated particles in the rep set returns to n rep , complete the update of external memory;

[0043] S326. Determine whether the iteration stop condition is met. If not, repeat steps S323-325. If met, output the rep set stored in the external memory as the Pareto frontier solution set.

[0044] Furthermore, step S4 specifically includes:

[0045] S41. Consider the mobile robot as a rigid body model and construct a nonlinear motion model of the mobile robot; linearize the nonlinear motion model by Taylor expansion at each point in the trajectory, and obtain a linear error equation including the state error and the control input error;

[0046] S42. Use the forward Euler method to discretize the linear error equation and construct a discrete state space equation. Based on the discrete state space equation, combined with the current state and control increment, predict the model output for multiple steps in the future and make advance predictions about the robot's motion.

[0047] S43. Construct a quadratic function of the model output as the objective function, set speed and acceleration limits for the control quantity and control increment, combine online rolling optimization to suppress disturbances, solve the optimal control quantity, control the actual trajectory of the mobile robot to fit the optimal planned trajectory, and achieve accurate tracking of the trajectory.

[0048] Based on the above technical solution, the present invention has at least the following beneficial effects:

[0049] The method proposed in this invention forms a complete technical closed loop through the logical chain of "trajectory generation provides a structural basis → trajectory optimization achieves multi-objective balance → trajectory tracking ensures execution accuracy"; the PDAG-MOPSO algorithm is proposed and combined with the MPC predictive control model, which not only solves the global optimization problem of complex trajectories, but also realizes real-time adjustment in dynamic environments through model prediction, ultimately significantly improving the automation level and reliability of docking tasks, and realizing the application of large-scale industrial automation docking and autonomous navigation of mobile robots in complex environments without prior maps. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 This is the overall flow chart of the method proposed in the present invention;

[0051] Figure 2 This is a schematic diagram of the x-direction trajectory of the control points when they are not sorted in the initial monotonic trajectory;

[0052] Figure 3 Schematic diagram of the x-direction trajectory of the control points after sorting in the initial monotonic trajectory;

[0053] Figure 4 Schematic diagram of the Pareto dominance relationship between two particles in the target space;

[0054] Figure 5 Schematic diagram of the Pareto frontier solution set in the target space;

[0055] Figure 6 Process diagram for adaptive mesh construction and mesh density estimation;

[0056] Figure 7 This is the multi-objective optimization flow chart of the PDAG-MOPSO proposed in the present invention;

[0057] Figure 8 Schematic diagram of the motion model for a mobile robot performing the docking task. DETAILED DESCRIPTION

[0058] In order to make the above-mentioned objects, features and advantages of the present invention more clearly understood, the following Figure 1-8 The present invention is further described in detail with specific implementation methods, so that the application can fully understand how to use technical means to solve technical problems and achieve technical effects and implement them accordingly.

[0059] Those skilled in the art will appreciate that all or part of the steps in the above-mentioned embodiment methods can be accomplished by instructing the relevant hardware through a program. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0060] like Figure 1 As shown, the present invention proposes a robot docking trajectory planning and path control method based on particle swarm optimization, which specifically includes the following steps:

[0061] S1. Clarify the docking task information: Two robots complete the docking task. One robot is stationary, and its co-carrying component maintains its position unchanged. Under the condition that there are no path obstacles in the docking environment, the other robot is mobile and needs to continuously adjust its position until it docks with the component carried by the stationary mobile robot.

[0062] S2. Using B-spline curves, the initial monotonic trajectory of the mobile robot's docking motion is generated through parametric modeling and superposition control point sorting method;

[0063] As a preferred embodiment, step S2 specifically includes:

[0064] S21. Generate the initial trajectory of the robot docking motion using a B-spline curve, set constraints and trajectory performance requirements, and specify the B-spline curve order, number of control points, node vector length, and the repetition of control points and node vector endpoints;

[0065] In this application, in order to meet the requirement of trajectory time consistency, the motion trajectory of the mobile robot in each direction shares a set of node vectors; the time sequence t={t0,t1,...,t m}, m = n + k + 1 is the node vector of the B-spline curve. Therefore, the equation of the mobile robot docking trajectory P(t) described by the B-spline curve can be expressed as:

[0066]

[0067] in, N i.k (t) represents the i-th control point and the i-th k-th basis function of the docking trajectory, respectively; k is the degree of the B-spline curve, m is the length of the node vector, and n is the number of control points;

[0068] Since the large component is fixedly connected to the mobile robot, the pose control points in the three-dimensional space can be simplified to the two-dimensional space in the docking trajectory. The two-dimensional generalized coordinates of the i-th pose control point in the trajectory are:

[0069]

[0070] in, Respectively represent the coordinates of the robot on the x-axis and y-axis, as well as the orientation at the pose control point;

[0071] In this embodiment, in order to accurately evaluate the trajectory, ensure the smoothness and controllability of the trajectory, and accurately describe the changes in the mobile robot's posture, the B-spline curve has at least a third-order derivative, so its degree is set to at least k = 5. To ensure that the robot can achieve a smooth transition between the starting and ending states, the control points of the starting and ending postures have at least three repetitions to avoid sudden changes or unnatural movements. In addition, considering the local properties of the B-spline curve, in addition to the starting control point, at least 5 additional control points must be selected to adjust the shape and characteristics of the trajectory. This paper selects 5 control points, so the total number of B-spline control points in this embodiment is set to 11, that is, n = 10. According to m = n + k + 1, the number of time node vectors m + 1 = 17 can be derived.

[0072] After obtaining the setting parameters of the B-spline docking trajectory, each physical quantity is expressed in detail. First, the starting and ending point poses of the mobile robot are expressed as follows:

[0073]

[0074] Then, perform 3 repetitions of the pose control point boundary constraints on the initial and final poses:

[0075]

[0076] At the same time, in order to ensure that the endpoint of the B-spline curve passes through the starting control point, the starting and ending postures must have k+1 repetitions, that is, 6 repetitions are required, so the time node vector is:

[0077] (The starting node is set to time 0)

[0078] In addition, due to the irreversibility of time, the time node vector must satisfy the non-decreasing constraint, that is, 0<t1≤t2≤t3≤t4≤t5≤t6;

[0079] At this point, an initial trajectory of the robot docking motion based on the B-spline curve can be obtained; and in order to ensure the consistency and smoothness of the motion and avoid oscillations caused by reciprocating motion, the planned trajectory must be processed for monotonicity. The trajectory of the B-spline curve approaches the successive connection of the control points. Therefore, in order to ensure the monotonicity of the trajectory, it is only necessary to ensure the monotonic orderliness of the components of the control points. The docking motion trajectory of the mobile robot involves three directions: x, y, and z. Therefore, the motion trajectory of each direction is planned separately, and finally the trajectories of each direction are merged into a complete motion trajectory, as shown in the following step S22. This application proposes a superimposed control point sorting method to achieve monotonic planning;

[0080] S22. Use the superposition control point sorting method to perform monotonic planning on the initial trajectory of the robot's docking motion to obtain an initial monotonic trajectory. More specifically:

[0081] First, the initial trajectory of each direction is planned separately, and then the planned trajectories in each direction are merged into a complete initial monotonic trajectory. For the initial docking trajectory in any direction, the repetition degree of the initial and terminal control points is set to 3. Except for the initial and terminal control points in the control point sequence, the remaining intermediate control points are rearranged by superposition and division. The formula is expressed as follows:

[0082]

[0083] in, p l is the initial value of the randomly initialized control point; p e 、p s is the position of the initial and terminal control points; is the control point sequence after planning; taking the x direction as an example, the sequence before and after planning is as follows Figure 2 、 Figure 3 Shown: From Figure 2 It can be observed that when the control points are not sorted, the robot shows a significant backoff phenomenon when reaching 50mm, retreating to around 40mm, and then continuing to move towards the end point from 40mm. This backoff phenomenon complicates the motion trajectory and leads to non-monotonicity. Although the parametric B-spline expression can generate a trajectory from the start point to the end point, the non-monotonicity of this trajectory causes reciprocating motion, which may cause wear and damage to the components during the docking process, which is not conducive to the smooth implementation of the docking process. Figure 3 The B-spline curve, after sorting the control points, shows a trajectory that monotonically increases from the starting point to the end point. This effectively avoids reciprocating motion and ensures the smoothness and monotonicity of the motion trajectory. By properly sorting the control points, the monotonicity of the trajectory is maintained, eliminating unnecessary inertial fallback and reducing potential damage to components. Furthermore, the monotonically increasing trajectory improves motion efficiency and reduces energy loss during movement.

[0084] S23. Perform multi-order derivatives on the initial monotonic trajectory to derive the velocity curve, acceleration curve, jerk curve, and corresponding control point sequence of the robot's docking motion. In this application, in order to more comprehensively evaluate the performance of the docking trajectory and provide evaluation indicators for the trajectory optimization later, it is necessary to obtain the velocity, acceleration, and jerk curves of the motion trajectory; by performing multi-order derivatives on the initial monotonic trajectory, the curve equations and node vectors of each physical quantity can be obtained, specifically:

[0085] Curve equations of various physical quantities:

[0086]

[0087] Among them, V(t), A(t), and J(t) represent the velocity curve, acceleration curve, and jerk curve, respectively; are the i-th control point on the velocity curve, acceleration curve, and jerk curve respectively; N i,4 (t), N i,3 (t), N i,2 (t) are the i-th basis functions of velocity curve, acceleration curve and jerk curve respectively;

[0088] The calculation method for each control point is:

[0089]

[0090] At the same time, the node vectors corresponding to the above physical quantity curves are:

[0091]

[0092] are the node vectors of velocity curve, acceleration curve and jerk curve respectively;

[0093] At this point, step S2 of this application has obtained the initial monotonic trajectory of the robot docking and the curves of various physical quantities, laying a solid foundation for subsequent trajectory optimization.

[0094] S3, based on the improved multi-objective particle swarm optimization algorithm PDAG-MOPSO, uses adaptive grid, external memory update and mutation mechanism to optimize the initial monotonic trajectory from multiple objectives until the optimal planning path is obtained;

[0095] As a preferred embodiment, step S3 specifically includes:

[0096] S31. Determine the evaluation objectives and constraints, and transform the trajectory optimization problem into a multi-objective optimization problem;

[0097] More specifically, step S31 includes:

[0098] S311, the maximum value of the absolute value of the jerk of the mobile robot in each direction of the docking trajectory is set as the primary evaluation target of the docking trajectory, and the objective function The formula is expressed as:

[0099]

[0100] Among them, |j x |、|j y |、|j z | are the absolute values ​​of jerk in the x-axis, y-axis, and z-axis directions respectively; is the parameter vector that comprehensively reflects the trajectory characteristics;

[0101] The total time consumed by the mobile robot during docking is set as the secondary evaluation target, and the formula of the target evaluation function is expressed as follows: The total time is obtained based on the velocity curve of the initial monotonic trajectory.

[0102] During the docking process of large components, ensuring the safety of the docking task is the top priority. The components must be protected from any form of damage during the docking process, which is not only related to the normal operation of the equipment, but also directly affects the subsequent production efficiency and safety. Therefore, the smoothness of the movement of the mobile robot has become a key factor. Jerk can effectively evaluate the acceleration change rate of the robot, thereby reflecting the smoothness of its movement. The control of jerk can help reduce sudden changes in the movement process, speed up the response time, and reduce the impact force that may be generated during docking. Under the premise of ensuring safety, docking efficiency is also an important issue that cannot be ignored. As a key link in the assembly of large components, the efficiency of component docking assembly is directly related to the progress and cost control of the overall project; therefore, this application chooses jerk and total time as evaluation targets.

[0103] S312, set the mobile robot to remain stationary at the starting point and the end point, that is, the speed at the starting point and the end point and acceleration is zero; set the motor performance constraints, and during operation, the speed and output torque of each motor of the robot are within the rated range; the final multi-objective optimization problem is expressed as:

[0104]

[0105] in, Evaluation function; are the speeds at the starting point and the end point respectively; are the accelerations at the starting point and the end point respectively; τ, τ max are the motor speed and speed rating respectively; a, a max They are the motor output torque and output torque rated value respectively.

[0106] S32. Use the multi-objective particle swarm optimization algorithm PDAG-MOPSO based on Pareto-dominated adaptive grid to solve the multi-objective optimization problem and obtain the Pareto frontier solution set;

[0107] As a preferred embodiment, Figure 7 The specific process of the PDAG-MOPSO algorithm shown is:

[0108] S321, initializing the population size, external memory capacity, and number of grids;

[0109] S322, deriving the initial monotonic trajectory to obtain a velocity curve and a jerk curve, and calculating the evaluation function value of each particle; based on the kinematic and dynamic models of the mobile robot, determining the motor speed and output torque corresponding to the trajectory, and setting the evaluation function value of the particle that exceeds the constraint condition to a maximum value; by Figure 4 The Pareto dominance relationship shown, stores the initial non-dominated particles in the external memory, which is recorded as the rep set;

[0110] Determine the initial number of grids for each dimension in the target space, and then determine the initial grid interval range based on the values ​​of each objective function in the first iteration; the formula is expressed as:

[0111]

[0112] Among them, d w is the width of each grid in the w-th dimension of the target space; K w is the number of grid divisions in the wth dimension; are the maximum and minimum values ​​of the w-th objective function respectively;

[0113] Suppose that after one iteration, a new non-dominated particle sequence a=(a1,a2,a3,L,a m ), then the specific position of each non-dominated particle in the target space for:

[0114]

[0115] Among them, floor() is a floor rounding function; after each iteration, the particles in the external storage set are divided into grids, and the density of non-dominated particles in each grid can be determined;

[0116] Figure 6 The schematic diagram shows the process of building the adaptive grid and estimating the particle density when m=2, where is the minimum value of the target space, particles in the external storage set are represented by points, and the density of the grid without any particles is 0. By meshing the target space, we can better understand the distribution of solutions and provide a more accurate basis for particle selection in the subsequent optimization process.

[0117] S323, determine the global optimal particle; based on the grid density (i.e., the number of non-dominated particles in the grid), use the global optimal particle update strategy to select the global optimal particle gbest from the rep set, specifically: calculate the fitness fit = exp(-β*n g ), where β is the fitness coefficient, n gis the number of non-dominated particles in the grid; the roulette wheel selection method is applied to select the grid with the highest fitness, and a particle is randomly selected from it as the global optimal particle gbest of this iteration to guide the movement adjustment of the remaining particles;

[0118] S324. Adjust the speed and position of each particle and ensure that the particles meet the boundary conditions; at the same time, apply a small probability random mutation to the updated population particles; if the mutated trajectory does not meet the monotonicity condition, re-sort it by superimposing the control point sorting method;

[0119] In this embodiment, in the initial stage of the algorithm, all particles are affected by the mutation operator to cover the entire range of decision variables, promoting the diversity and exploratory behavior of the population. As the iteration proceeds, the number of particles affected by the mutation operator is gradually reduced so that more focus can be placed on local search in the later stages. In addition, the mutation operator also takes into account the range of decision variables of the problem to be solved, ensuring that all decision variables are effectively covered in the early stages of the search, thereby improving the algorithm's ability to explore the solution space. Therefore, this application uses a nonlinear function, which gradually reduces the covered range as the number of iterations increases, so that the influence of the mutation operator gradually weakens. This dynamic adjustment mechanism effectively adapts to changes in the solution space and improves the search efficiency of the algorithm at different stages;

[0120] After that, the evaluation function value of each particle is recalculated, and the local optimal particle pbest is updated according to the local optimal particle update rule; specifically, two particles are selected to determine the dominance relationship, and the non-dominated particle is used as the local optimal particle pbest. If the two particles do not dominate each other, one is randomly selected as the local optimal particle pbest.

[0121] The above-mentioned global and local optimal particle update strategies can effectively improve the efficiency of multi-objective optimization, enhance the quality and diversity of solutions, and provide strong support for solving complex multi-objective problems.

[0122] S325, after obtaining the local optimal particle pbest, update the rep set and the adaptive grid; and detect whether the rep set overflows: when the number of non-dominated particles in the rep set is less than the set capacity n rep When the non-dominated particles generated by the current iteration are added to the memory; if the number of non-dominated particles in the external memory exceeds n rep , then delete the non-dominated particles in the grid with the highest grid density in turn, and re-compare the density after each deletion until the number of non-dominated particles in the rep set returns to n rep , complete the update of external memory;

[0123] This external memory update strategy effectively controls the size of the external memory, ensuring that only the optimal non-dominated solutions are retained in the memory, thereby improving storage efficiency and solution quality. Furthermore, this strategy effectively maintains the diversity of solutions, ensuring a uniform distribution of the Pareto frontier and effectively preventing the clustering of solutions, thus enabling the optimization process to conduct a more comprehensive exploration of the solution space.

[0124] S326, determine whether the iteration stop condition is met, if not, repeat steps S323-325; if met, output the rep set stored in the external memory as the Pareto frontier solution set, the Pareto frontier solution set is the boundary formed by all Pareto optimal solutions in the target space, such as Figure 5 shown.

[0125] S33. Use the average evaluation method to select the optimal trajectory from the Pareto frontier solution set; that is, select the solution that minimizes the average evaluation objective function value δ as the final optimal solution:

[0126]

[0127] Where m represents the number of objective functions; Represents the maximum and minimum values ​​of the i-th objective function.

[0128] So far, this application has solved the optimal planning trajectory for robot docking through PDAG-MOPSO. The following will discuss how to achieve the robot tracking the optimal planning trajectory.

[0129] S4. Based on the MPC tracking control technology of the linear error model, the optimal control variable is solved by discretizing the state equation and the quadratic objective function to achieve accurate trajectory tracking;

[0130] As a preferred embodiment, step S4 specifically includes:

[0131] S41. Consider the mobile robot as a rigid body model and construct a nonlinear motion model of the mobile robot; linearize the nonlinear motion model by Taylor expansion at each point in the trajectory, and obtain a linear error equation including the state error and the control input error;

[0132] In this embodiment, Figure 8 As shown:

[0133] The mobile robot is considered as a rigid body model, where X w O w Y w Represents the world coordinate system; (x, y, θ) represents the x-axis and y-axis coordinates of the mobile robot in the world coordinate system and the heading angle relative to the x-axis, describing the direction. Velocity vector It represents the translation speed of the mobile robot along the X-axis and Y-axis and the rotation speed around the Z-axis in the world coordinate system.

[0134] The motion state of the mobile robot can be expressed as:

[0135] in, is the position derivative of the mobile robot; for ease of analysis, let X = [x, y, θ] T 、u=[v x ,v y ,ω] T As the state quantity and control input of the mobile robot respectively; the state space description of the robot is:

[0136] This describes a nonlinear system. In practical applications, it needs to be linearized so that it can be analyzed and designed using linear control theory. The linearization process is achieved by performing Taylor expansion near the operating point of the system:

[0137] Assume that at any reference point r(x r ,y r ,θ r )satisfy where X r =[x r ,y r ,θ r ] T 、u r =[v xr ,v yr ,ω r ] T Performing Taylor expansion at the reference point r and ignoring higher-order terms yields the linearized state space equation:

[0138] Where A and B are the Jacobian matrices of the state and input, respectively, defined as:

[0139]

[0140] Subtract the linearized state space equation from the original equation to obtain the deviation between the actual system state and the reference state, that is, the linear error equation is:

[0141]

[0142] Among them, X e =XX r represents the state error, u e =uu r represents the control input error.

[0143] S42. Use the forward Euler method to discretize the linear error equation and construct a discrete state space equation. Based on the discrete state space equation, combined with the current state and control increment, predict the model output for multiple steps in the future and make advance predictions about the robot's motion.

[0144] In this embodiment, the optimal planning trajectory obtained in step S2 is used as the reference trajectory, the robot state is obtained through the docking positioning system, and the above linear error equation is used as the prediction model. Since the linear error equation belongs to a continuous system and does not meet the requirements of robot discrete control, the forward Euler method is first used to discretize it to obtain X e (τ+1)=a τ X e (τ)+b τ u e (τ);

[0145] in, T represents the sampling period, τ represents the discretized τth time step;

[0146] In order to unify the output form, the output equation is defined as:

[0147] In order to optimize efficiency and improve the control performance of MPC, the discrete state quantity X e (τ) and the discrete control input u at the previous time step e (τ-1) are combined to construct a new state variable ξ(τ)=[X e (τ)u e (τ-1)] T ;

[0148] Then the state space equation of the mobile robot trajectory tracking system can be expressed as:

[0149]

[0150] in, N X Indicates the dimension of state quantity; N u Indicates the control dimension; sets the prediction time domain N p Greater than the control time domain N c , then the state variables at each time step in the prediction domain are:

[0151]

[0152] The output of the model at each time step in the prediction domain can be expressed as:

[0153]

[0154] Thus, the general output of the prediction model is derived as: Y = ψξ(τ) + ΘΔU;

[0155] in,

[0156] That is, if the state quantity ξ(τ) at the current moment and the control input increment ΔU in the control time domain are known, the model output quantity Y in the predicted future time domain can be obtained.

[0157] S43. Construct a quadratic function output by the model as the objective function, set speed and acceleration limits for the control variable and control increment, combine online rolling optimization to suppress disturbances, solve the optimal control variable, control the actual trajectory of the mobile robot to fit the optimal planned trajectory, and achieve accurate trajectory tracking;

[0158] In this embodiment, the objective function is designed as follows:

[0159]

[0160] Among them, Y r is the reference output of the model, given in the form of error. In this embodiment, Y r =0;Q Q is the tracking error weight positive definite matrix, when Q Q When it is larger, the trajectory tracking error of the mobile robot will decrease; R R is the input weight positive definite matrix, when R R When it is larger, the calculated control quantity changes more smoothly;

[0161] Let E = ψξ(k) and simplify it to get the following form:

[0162]

[0163] In order to ensure the solvability of the quadratic form, the relaxation factor r is introduced when the objective function is transformed into a quadratic form. slack , and ignoring its constant term, the final quadratic objective function is:

[0164]

[0165] in, g=[E T Q Q Θ0];

[0166] In addition, in actual trajectory tracking control, the velocity trajectory and acceleration trajectory obtained from the docking trajectory are used to constrain the control amount and control increment to prevent excessive control amount from causing significant inertia, which may damage the components. According to the relationship between the control amount and the control increment, we can get:

[0167]

[0168] in, N c -dimensional identity matrix;

[0169] Since each time step control amount and control increment has a maximum and minimum value, we have:

[0170]

[0171] Combining the above two equations, we get:

[0172]

[0173] Among them, U max 、U min are the maximum and minimum values ​​of the controlled quantity, ΔU min , ΔU max are the maximum and minimum values ​​of the control increment respectively; U t , ΔU t are the control amount and control increment of the previous cycle respectively;

[0174] Finally, the quadratic objective function is optimized and solved under the constraints of the control quantity and control increment. The optimal control input is obtained by minimizing the objective function. The first element value in the solved control sequence is applied to the mobile robot to make the actual trajectory of the mobile robot as close as possible to the optimal planned trajectory.

[0175] In summary, the method proposed in the present invention solves the global optimization problem of complex trajectories, realizes real-time adjustment in dynamic environments, significantly improves the automation level and reliability of docking tasks, and ensures the autonomous navigation of mobile robots in complex environments such as industrial automation docking of large components and without prior maps.

[0176] In the description of this specification, the reference terms "one embodiment," "some embodiments," "example," "specific example," or "some examples" mean that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. Moreover, the specific features, structures, materials, or characteristics described may be combined in any appropriate manner in any one or more embodiments or examples. In addition, those skilled in the art may combine and combine different embodiments or examples described in this specification, as well as features of different embodiments or examples, unless they are mutually inconsistent.

[0177] The logic and / or steps represented in the flowchart or otherwise described herein may be considered, for example, as an ordered list of executable instructions for implementing logical functions, and may be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device).

[0178] The above embodiments provide a detailed introduction to the present invention. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core ideas. At the same time, for those skilled in the art, according to the ideas of the present invention, there may be changes in the specific implementation methods and application scopes. In summary, the contents of this specification should not be understood as limiting the present invention.

Claims

1. A robot docking trajectory planning and path control method based on particle swarm optimization, characterized in that: The specific steps include: S1. Clarify the docking task information: Two robots complete the docking task. One robot is stationary, and its co-carrying component maintains its position unchanged. Under the condition that there are no path obstacles in the docking environment, the other robot is mobile and needs to continuously adjust its position until it docks with the component carried by the stationary mobile robot. S2. Using B-spline curves, the initial monotonic trajectory of the mobile robot's docking motion is generated through parametric modeling and superposition control point sorting method; S3, based on the improved multi-objective particle swarm optimization algorithm PDAG-MOPSO, uses adaptive grid, external memory update and mutation mechanism to optimize the initial monotonic trajectory from multiple objectives until the optimal planning path is obtained; S4. Based on the MPC tracking control technology of the linear error model, the optimal control quantity is solved by discretizing the state equation and the quadratic objective function to achieve accurate trajectory tracking.

2. The robot docking trajectory planning and path control method based on particle swarm optimization according to claim 1 is characterized in that: Step S2 specifically includes: S21. Generate the initial trajectory of the robot docking motion using a B-spline curve, set constraints and trajectory performance requirements, and specify the B-spline curve order, number of control points, node vector length, and the repetition of control points and node vector endpoints; S22, using a superposition control point sorting method to perform monotonic planning on the initial trajectory of the robot docking motion to obtain an initial monotonic trajectory; S23. Perform multi-order derivatives on the initial monotonic trajectory to derive the velocity curve, acceleration curve, emergency stop curve, and corresponding control point sequence of the robot docking motion.

3. The robot docking trajectory planning and path control method based on particle swarm optimization according to claim 2 is characterized in that: Step S22 is specifically as follows: First, the initial trajectory of each direction is planned separately, and then the planned trajectories in each direction are merged into a complete initial monotonic trajectory. For the initial docking trajectory in any direction, the repetition degree of the initial and terminal control points is set to 3. Except for the initial and terminal control points in the control point sequence, the remaining intermediate control points are rearranged by superposition and division. The formula is expressed as follows: in, p l is the initial value of the randomly initialized control point; p e 、p s is the position of the initial and terminal control points; is the control point sequence after planning.

4. The robot docking trajectory planning and path control method based on particle swarm optimization according to claim 1 is characterized in that: Step S3 specifically includes: S31. Determine the evaluation objectives and constraints, and transform the trajectory optimization problem into a multi-objective optimization problem; S32. Use the multi-objective particle swarm optimization algorithm PDAG-MOPSO based on Pareto-dominated adaptive grid to solve the multi-objective optimization problem and obtain the Pareto frontier solution set; S33. Use the average evaluation method to select the optimal planning trajectory from the Pareto frontier solution set.

5. The robot docking trajectory planning and path control method based on particle swarm optimization according to claim 4 is characterized in that: Step S31 specifically includes: S311, the maximum value of the absolute value of the jerk of the mobile robot in each direction of the docking trajectory is set as the primary evaluation target of the docking trajectory, and the objective function The formula is expressed as: Among them, |j x |、|j y |、|j z | are the absolute values ​​of jerk in the x-axis, y-axis, and z-axis directions respectively; is the parameter vector that comprehensively reflects the trajectory characteristics; The total time consumed by the mobile robot during docking is set as the secondary evaluation target, and the formula of the target evaluation function is expressed as follows: The total time is obtained based on the velocity curve of the initial monotonic trajectory; S312, set the mobile robot to remain stationary at the starting point and the end point, that is, the speed at the starting point and the end point and acceleration is zero; set the motor performance constraints, and during operation, the speed and output torque of each motor of the robot are within the rated range; the final multi-objective optimization problem is expressed as: in, Evaluation function; are the speeds at the starting point and the end point respectively; are the accelerations at the starting point and the end point respectively; τ, τ max are the motor speed and speed rating respectively; a, a max They are the motor output torque and output torque rated value respectively.

6. The robot docking trajectory planning and path control method based on particle swarm optimization according to claim 5 is characterized in that: Step S32 specifically includes: S321, initializing the population size, external memory capacity, and number of grids; S322, deriving the initial monotonic trajectory to obtain a velocity curve and a jerk curve, and calculating the evaluation function value of each particle; based on the kinematic and dynamic models of the mobile robot, determining the motor speed and output torque corresponding to the trajectory, and setting the evaluation function value of particles that exceed the constraint conditions to a maximum value; storing the initial non-dominated particles in an external memory using the Pareto dominance relationship, and recording them as a rep set; Determine the initial number of grids for each dimension in the target space, and then determine the initial grid interval range based on the values ​​of each objective function in the first iteration; the formula is expressed as: Among them, d w is the width of each grid in the w-th dimension of the target space; K w is the number of grid divisions in the w-th dimension; are the maximum and minimum values ​​of the w-th objective function respectively; Suppose that after one iteration, a new non-dominated particle sequence a=(a1,a2,a3,L,a m ), then the specific position of each non-dominated particle in the target space for: Among them, floor() is a rounding function; then each non-dominated particle is assigned to the corresponding grid; S323, determine the global optimal particle; based on the grid density, use the global optimal particle update strategy to select the global optimal particle gbest from the rep set, specifically: calculate the fitness fit = exp(-β*n based on the grid density for each grid containing non-dominated particles g ), where β is the fitness coefficient, n g is the number of non-dominated particles in the grid; the roulette wheel selection method is applied to select the grid with the highest fitness, and a particle is randomly selected from it as the global optimal particle gbest of this iteration to guide the movement adjustment of the remaining particles; S324. Adjust the speed and position of each particle and ensure that the particles meet the boundary conditions; at the same time, apply a small probability random mutation to the updated population particles; if the mutated trajectory does not meet the monotonicity condition, re-sort it by superimposing the control point sorting method; After that, the evaluation function value of each particle is recalculated, and the local optimal particle pbest is updated according to the local optimal particle update rule; specifically, two particles are selected to determine the dominance relationship, and the non-dominated particle is used as the local optimal particle pbest. If the two particles do not dominate each other, one is randomly selected as the local optimal particle pbest. S325, after obtaining the local optimal particle pbest, update the rep set and the adaptive grid; and detect whether the rep set overflows: when the number of non-dominated particles in the rep set is less than the set capacity n rep When the non-dominated particles generated by the current iteration are added to the memory; if the number of non-dominated particles in the external memory exceeds n rep , then delete the non-dominated particles in the grid with the highest grid density in turn, and re-compare the density after each deletion until the number of non-dominated particles in the rep set returns to n rep , complete the update of external memory; S326. Determine whether the iteration stop condition is met. If not, repeat steps S323-325. If met, output the rep set stored in the external memory as the Pareto frontier solution set.

7. The robot docking trajectory planning and path control method based on particle swarm optimization according to claim 1, characterized in that: Step S4 specifically includes: S41. Consider the mobile robot as a rigid body model and construct a nonlinear motion model of the mobile robot; linearize the nonlinear motion model by Taylor expansion at each point in the trajectory, and obtain a linear error equation including the state error and the control input error; S42. Use the forward Euler method to discretize the linear error equation and construct a discrete state space equation. Based on the discrete state space equation, combined with the current state and control increment, predict the model output for multiple steps in the future and make advance predictions about the robot's motion. S43. Construct a quadratic function of the model output as the objective function, set speed and acceleration limits for the control quantity and control increment, combine online rolling optimization to suppress disturbances, solve the optimal control quantity, control the actual trajectory of the mobile robot to fit the optimal planned trajectory, and achieve accurate tracking of the trajectory.

Images