Trajectory cooperative planning method for heterogeneous robot colony of asteroid exploration based on convex optimization

Abstract

The application discloses a kind of based on convex optimization asteroid exploration heterogeneous robot colony trajectory cooperative planning method.The application converts heterogeneous robot colony trajectory cooperative planning problem into heterogeneous robot colony cooperative control optimization problem, and the optimization problem includes: dynamics, state constraint, control constraint, boundary condition and objective function.The application carries out convex processing to the heterogeneous robot colony cooperative control optimization problem established.The application uses sequential convex programming to solve the heterogeneous robot colony cooperative control convex optimization problem.The application realizes solving asteroid exploration heterogeneous robot colony trajectory cooperative planning problem under convex optimization framework, and the calculation speed is faster, so it can meet the demand of on-board real-time calculation, i.e., using the control variable of the first few times as maneuvering control, and using the state information collected by current sensor to update trajectory in real time, and constantly rolling updating the control of robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of multi-target trajectory cooperative planning algorithm, and particularly relates to a small asteroid exploration heterogeneous robot group trajectory cooperative planning method based on convex optimization. BACKGROUND

[0002] For trajectory cooperative planning of the heterogeneous robot group, dynamics of each part in the heterogeneous robot group needs to be established, state constraints for avoiding collision between the robots and between the robots and the small asteroid are given, and control constraints for control ability of each robot are given, an overall observation range of the heterogeneous robot group on the small asteroid or fuel consumption is taken as an optimization index, a small asteroid exploration heterogeneous robot group cooperative control optimization problem is established, and then the optimization problem is solved to give motion trajectories and control amounts of each part in the heterogeneous robot group.

[0003] However, the small asteroid exploration heterogeneous robot group cooperative control optimization problem is a complex multi-target multi-constraint problem, there are equality constraints and inequality constraints, and each constraint can be highly nonlinear and non-convex, and an optimal solution can not be found for solving such an optimization problem, and there are problems of large calculation amount and slow solving speed by using existing solving methods such as sequential quadratic programming. Therefore, it is necessary to propose an autonomous control planning algorithm with strong universality, fast solving function and suitable for onboard real-time online calculation, so as to realize observation on the small asteroid by using the heterogeneous robot group. SUMMARY

[0004] The present application provides a small asteroid exploration heterogeneous robot group trajectory cooperative planning method based on convex optimization, which has good universality and effectively improves solving speed of the multi-target trajectory cooperative planning problem.

[0005] In a first aspect, a small asteroid exploration heterogeneous robot group trajectory cooperative planning method based on convex optimization is provided, characterized in that the method is applied to a small asteroid exploration heterogeneous robot system including one central robot and n observation robots, and the method includes:

[0006] Converting a dynamics equation of the small asteroid exploration heterogeneous robot system into a system state equation;

[0007] Carrying out convex processing on state constraints of the small asteroid exploration heterogeneous robot system;

[0008] Converting the control constraints into constraints of the control variables, converting the state constraints into constraints of the state variables, and converting an objective function of the small asteroid exploration heterogeneous robot system into a function of the control variables and the state variables, the objective function including height performance indexes and fuel performance indexes of all the observation robots;

[0009] According to the system state equation, the state constraint after convex processing, the constraint of the control variable and the constraint of the state variable, the target function is iteratively optimized to obtain the group trajectory of the asteroid exploration heterogeneous robot system.

[0010] In combination with the first aspect, in some implementations of the first aspect, the system state equation satisfies:

[0011] x i (t)=[r i (t),v i (t)] T u i (t)=T i (t) T

[0012]

[0013]

[0014]

[0015] wherein i represents the i th robot, i = 1, 2, 3,..., n, n is the total number of robots, and when i = 1, it is the central robot; Ω is the rotation speed of the asteroid, Ω = [Ω x ,Ω y ,Ω z ] T , r i (t) is the displacement of the i th robot, v i (t) is the speed of the i th robot, T i (t) is the thrust received by the i th robot, and ▽U(r i ) is the asteroid gravity received by the i th robot.

[0016] In combination with the first aspect, in some implementations of the first aspect, the state constraint after convex processing satisfies:

[0017] or,

[0018]

[0019] wherein,

[0020]

[0021] Φ=[E3 03], is x ithe reference state of (t), k is the number of discrete time points, l represents the number of iterations of the sequence of convex programming, l = 1, 2, 3,..., and the center of the trust region at the kth discrete time point is located at is a virtual compensation term.

[0022] In combination with the first aspect, in some implementations of the first aspect, the state constraint after the convex processing satisfies:

[0023] or,

[0024]

[0025] wherein,

[0026]

[0027]

[0028] x(t) = [x1(t) T ,...,x i (t) T ,...,x j (t) T ,...,x n (t) T ] T , P ij is a coefficient matrix of n x n blocks, each block is a 6 x 6 square matrix, for the (i, i) block the (j, j) block the remaining blocks are zero matrices; is a reference trajectory of x(t), is a virtual compensation term, k is the number of discrete time points, l represents the number of iterations of the sequence of convex programming, l = 1, 2, 3,..., and the center of the trust region at the kth discrete time point is located at is a virtual compensation term, d is a safety sphere radius.

[0029] In combination with the first aspect, in some implementations of the first aspect, the state constraint after the convex processing satisfies:

[0030]

[0031] or,

[0032] wherein,

[0033]

[0034] β is a half-cone angle, Hi is a coefficient matrix of n x n blocks, each block is a 6 x 6 square matrix, for the (i, i) block The rest of the blocks are zero matrices, i = 1, 2,..., n, k is the number of discrete time points, l represents the number of iterations of the sequence of convex programming, l = 1, 2, 3,..., the center of the trust region at the kth discrete time point is located at is a virtual compensation term.

[0035] In combination with the first aspect, in some implementations of the first aspect, the constraint of the control variable satisfies:

[0036] ‖u i (t)‖ ∞ ≤T max ,i=1,2,3,...,n

[0037] where T max is the maximum thrust of the thruster.

[0038] In combination with the first aspect, in some implementations of the first aspect, the constraint of the state variable satisfies:

[0039] ||Φx i (t)||2≤R i ,i=1,2,3,...,n

[0040] where R i is the highest observation height of each robot at the observation time.

[0041] In combination with the first aspect, in some implementations of the first aspect, the objective function is and is a function of ω r and ω m are weight coefficients of the overall observation range term and the fuel consumption term of the heterogeneous robot group respectively, ω r > 0, ω m > 0, I sp is the specific impulse of the thruster in vacuum, g0 is the standard gravity constant of the earth, t f is the terminal time.

[0042] In combination with the first aspect, in some implementations of the first aspect, the objective function satisfies:

[0043]

[0044] l represents the number of iterations of the sequence of convex programming, l = 1, 2, 3,...; is the state deviation of two sequence iterations, The trust region center of each discrete point is located at The penalty function of the trust region is ω δ The trust region weight coefficient is ω The virtual compensation term is ω The virtual compensation term is ω b The virtual compensation term weight coefficient is ω e c The virtual compensation term weight coefficient is ω

[0045] In combination with the first aspect, in some implementations of the first aspect, the optimization iteration of the target function comprises:

[0046] According to the system state equation, the convex processed state constraint, the constraint of the control variable and the constraint of the state variable, an initial reference trajectory containing all robot state variables is given, the target function is solved, and the current solution is taken as a reference trajectory for the next solving, and the iteration is continuously performed until The trajectory containing the state variable and the control variable obtained in the last iteration is less than a given threshold value or reaches the number of iterations, and the trajectory containing the state variable and the control variable obtained in the last iteration is the group trajectory of the asteroid exploration heterogeneous robot system.

[0047] Compared with the prior art, the scheme provided in the application has at least the following beneficial technical effects:

[0048] By constructing a heterogeneous robot group collaborative control convex optimization problem, the heterogeneous robot group trajectory collaborative planning problem is solved in a convex optimization framework, and the problem of poor generality of the existing solving method is solved. The framework gives a method for converting a non-convex problem into a convex problem that can guarantee convergence, and in the framework, constraint conditions can be inserted or increased or decreased according to the demand of the observation task and the number of the robot group, so that the solving of the trajectory collaborative planning problem is not increased when the heterogeneous robot group system changes.

[0049] The sequence convex programming method is used to solve the heterogeneous robot group trajectory collaborative planning problem, which has a faster solving speed and better convergence, and solves the problems of difficult solving of the multi-objective trajectory collaborative planning problem and slow calculation. The sequence convex programming is not sensitive to the initial reference trajectory, and the obtained solution has good local optimality even if it is not a global optimal solution. An optimal solution can be found in polynomial time using a custom solver, the movement trajectory and the corresponding control amount of each part of the heterogeneous robot group are given, the solving speed is fast, and the demand of on-board real-time calculation and trajectory planning can be met.​​​ Attached Figure Description

[0050] Figure 1 This is a schematic diagram of a collaborative trajectory planning method for heterogeneous robot groups in asteroid exploration based on convex optimization. Detailed Implementation

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

[0052] The proposed methods for collaborative trajectory planning of heterogeneous robot swarms currently lack versatility. The constraints designed to ensure flight safety and communication for heterogeneous robot swarms should be universal, minimizing changes caused by alterations in the configuration of the swarm. These constraints should be adjusted by adding or removing them from the constructed optimization problem, ensuring the integrity of the entire problem framework and avoiding increased solution difficulty.

[0053] Current proposed methods for collaborative trajectory planning of heterogeneous robot swarms are difficult to solve and slow. Multi-objective, multi-constraint planning problems, especially those considering communication and collision avoidance among different parts of a heterogeneous robot swarm, result in complex and redundant collaborative control optimization problems for heterogeneous robot swarms. Existing methods struggle to quickly find the optimal solution and guarantee convergence, and the computing power of current airborne computers cannot meet the demands of real-time online computation.

[0054] To solve the above problems, such as Figure 1 As shown, this application provides a collaborative trajectory planning method for a heterogeneous robot swarm in asteroid exploration based on convex optimization. This method can be applied to a heterogeneous robot system for asteroid exploration. Assume the heterogeneous robot system for asteroid exploration consists of one central robot and n observation robots. The dynamics of each robot are established in the asteroid-fixed coordinate system, and the dynamics can be expressed as...

[0055]

[0056] Where i represents the i-th robot, i = 1, 2, 3, ..., n, and n is the total number of robots. When i = 1, it is the central robot; Ω is the angular velocity of the asteroid's rotation, r i (t) represents the displacement of the i-th robot, v i (t) represents the speed of the i-th robot, T i (t) represents the magnitude of the thrust experienced by the i-th robot, ▽U(r) i Let be the gravitational force exerted by the asteroid on the i-th robot.

[0057] The thrust output of the thruster causes a change in the robot's mass. Assuming the rate of fuel consumption is proportional to the magnitude of the thrust vector, the mass change can be expressed as:

[0058]

[0059] Among them, I sp Let g be the vacuum specific impulse of the thruster, and g0 be the standard gravitational constant of the Earth.

[0060] Because asteroids are generally irregular in shape and have high spin speeds, ellipsoidal state constraints are used to ensure that the robots do not collide with the asteroid surface during observation. This requires all robots to remain outside the escape ellipsoid surrounding the asteroid. This ellipsoidal constraint can be described as follows:

[0061] 1-r i (t) T Θr i (t)≤0,i=1,2,3,...,n (3)

[0062] in, (a,b,c) represents the semi-major axis of the ellipsoid.

[0063] To prevent collisions between robots, a safety sphere is set up for each robot with its center of mass as the center, and all other robots are required to be outside this safety sphere. This sphere constraint can be described as follows:

[0064] d 2 ≤[r i (t)-r j (t)] T [r i (t)-r j (t)],i≠j,i=1,2,3,...,n,j=1,2,3,...,n (4)

[0065] Where d is the radius of the safety sphere.

[0066] To ensure communication between all observation robots and the central robot, all observation robots must reside within a cone with the line connecting the central robot and the asteroid's center of mass as its axis and β as its semi-cone angle. This conical constraint can be expressed as:

[0067]

[0068] Furthermore, the robot's maneuverability is determined by the thrust output of the thruster, and the magnitude of the thrust is limited, which can be given as control constraints.

[0069] ||T i (t)‖ ∞ ≤T max ,i=1,2,3,...,n (6)

[0070] Among them, T maxThe maximum thrust of the thruster.

[0071] Assuming that the observation range angle of all observation robot cameras remains unchanged, the higher the observation position of the robot, the larger the observation range of the asteroid surface, so the height of all observation robots can be used as a performance indicator, and its expression is

[0072]

[0073] In addition, since the observation distance of the robot camera is limited by its working range, i.e. ||r i (t)||2 in equation (7) cannot be infinite, the observation distance of each robot needs to be limited, so the distance state constraint can be given as

[0074] ||r i (t)||2≤R i ,i=1,2,3,...,n (8)

[0075] where R i is the maximum observation height of each robot when observing.

[0076] In addition, the orbital transfer or maintenance of the robot in the observation of the asteroid will consume fuel, in order to minimize the fuel consumption of the robot while completing the task, so the fuel consumption of all robots can be used as a performance indicator, and its expression is

[0077]

[0078] where t f is the terminal time, indicating the end time of the entire flight process.

[0079] The objective function that simultaneously considers the overall observation range and fuel consumption composite index of the heterogeneous robot group can be given as

[0080] minJ=ω r J r +ω m J m (10)

[0081] where ω r and ω m are the weight coefficients of the overall observation range item and the fuel consumption item of the heterogeneous robot group, respectively, ω r > 0, ω m > 0.

[0082] In addition, for an optimization problem, the boundary conditions of all state variables also need to be given, and for the asteroid exploration heterogeneous robot group, the boundary conditions are

[0083] r i (0)=r i0 ,v i (0)=v i0 ,i=1,2,3,...,n (11)

[0084] r1(t f )=r 1tf (12)

[0085] Combining the dynamics of equations (1) and (2), the state constraints of equations (3)-(5) and (8), the control constraints of equation (6), the boundary conditions of equations (11) and (12), and the objective function of equation (10), the trajectory cooperative control optimization problem of the heterogeneous robot system is established.

[0086] Let the positions, velocities and masses of all robots be the state variables of the heterogeneous robot system, and let the thrust vectors of all robots be the control variables of the heterogeneous robot system, which are

[0087] x i (t)=[r i (t) T ,v i (t) T ] T u i (t)=T i (t) T (13)

[0088] In order to solve the optimization problem in the convex optimization framework, first, the dynamics of equation (1) is changed into the system state equation, which is expressed as

[0089]

[0090]

[0091] where Ω = [Ω x ,Ω y ,Ω z ] T .

[0092] In addition, since equations (3), (4) and (5) are non-convex state constraints, they need to be convexified to be converted into convex constraints, and the linearization method is used for convexification.

[0093] Linearizing equation (3), we can obtain

[0094]

[0095]

[0096] where Φ = [E303], is the reference state of x i (t).

[0097] Linearizing equation (4) gives

[0098]

[0099]

[0100] where P ij is the coefficient matrix of n x n blocks, each block is a 6 x 6 square matrix, for the (i, i) block the (j, j) block and the rest are zero matrices. is the reference trajectory of x(t).

[0101] Linearizing equation (5) gives

[0102]

[0103]

[0104]

[0105] where H i is the coefficient matrix of n x n blocks, each block is a 6 x 6 square matrix, for the (i, i) block and the rest are zero matrices, i = 1, 2,..., n.

[0106] Rewriting equation (8) in terms of state variables gives

[0107] ||Φx i (t)||2≤R i , i = 1, 2, 3,..., n (22)

[0108] Rewriting equation (6) in terms of control variables gives

[0109] ‖u i (t)‖ ∞ ≤T max , i = 1, 2, 3,..., n (23)

[0110] In terms of state variables, the state equation can be expressed as

[0111] x i (0) = x i0 , x1(t f ) = x 1tfi = 1, 2, 3..., n (24)

[0112] Let the performance index be expressed by state variables and control variables, then

[0113]

[0114]

[0115] Then, combined with the objective function (10), the system state equation (14), the state constraint equations (16) (18) (20) (22) and the control constraint (23), and the boundary conditions (24), the trajectory cooperative control convex optimization problem of the asteroid exploration heterogeneous robots is constructed.

[0116] The trajectory cooperative control convex optimization problem of the asteroid exploration heterogeneous robots constructed by formula is solved by using sequential convex programming. First, the continuous-time infinite-dimensional time problem is converted into a discrete-time finite-dimensional optimization problem by discretization, then the trust region and virtual compensation are introduced, and the asteroid exploration heterogeneous robot group cooperative control convex constraint sub-problem is given.

[0117] The trajectory cooperative control convex constraint sub-problem of the asteroid exploration heterogeneous robots is

[0118]

[0119]

[0120]

[0121]

[0122]

[0123]

[0124]

[0125] ‖u i (k)‖ ∞ ≤T max i = 1, 2, 3..., n (34)

[0126]

[0127]

[0128] Wherein, l represents the number of iterations of sequential convex programming, l = 1, 2, 3,...; the is the state deviation of two sequential iterations, is the square of the trust region radius, and the trust region center at the kth discrete time point is located at is the square of the trust region radius, and the trust region center at the kth discrete time point is located at is the penalty function of the trust region, and ω δ is the trust region weight coefficient. and are the virtual compensation terms introduced to eliminate the artificial infeasibility caused by linearization of the non-convex state constraints, and these virtual compensation terms are also introduced into the objective function (21), and is the penalty function of the virtual compensation term, and ω b , ω e , and ω c are the virtual compensation term weight coefficients.

[0129] In combination with equations (27)-(36), and the initial reference trajectory containing all the robot state variables is given, the trajectory coordination control convex constraint sub-problem of the asteroid exploration heterogeneous robot group is solved, and the current solution is taken as the reference trajectory for the next time of solving, and iteration is continuously carried out until is less than a given threshold value or the number of iterations is reached, and the trajectory containing the state variables and the control variables obtained at the last time is the solution of the asteroid exploration heterogeneous robot group trajectory coordination planning.

[0130] The asteroid exploration heterogeneous robot group trajectory coordination planning problem is solved under the convex optimization framework, and the calculation speed is relatively fast, so that the demand of on-board real-time calculation can be met, that is, the control variables solved for the first few times are used as the maneuvering control, and the state information collected by the current sensor is used to continuously and real-timely calculate and update the trajectory, and the control of the robot is continuously and rolling updated.

[0131] The idea of the asteroid exploration heterogeneous robot group trajectory coordination planning method based on convex optimization proposed in the application is divided into three aspects: construction and convex processing of the heterogeneous robot group coordination control optimization problem, and sequential convex programming for solving.

[0132] 1. The heterogeneous robot group trajectory coordination planning problem is converted into a heterogeneous robot group coordination control optimization problem, and the optimization problem contains dynamics, state constraints, control constraints, boundary conditions, and an objective function.

[0133] 1.1. The dynamics part contains the dynamics of all the robots in the heterogeneous robot group, and since the exploration object is an asteroid, the established dynamics is expressed in the asteroid fixed coordinate system, that is, the rotation of the asteroid and the gravity are considered. In addition, considering the fuel consumption, the mass changes of each part of the robot group are also included in the dynamics.

[0134] 1.2. State constraints are constraints on the flight path of the robot swarm, and are also conditions to ensure communication and flight safety between parts, including: constraints to ensure that each part of the heterogeneous robot swarm does not collide with an asteroid, constraints to ensure that each robot does not collide with each other, constraints to ensure that robots can communicate with each other, etc.

[0135] 1.3. The maneuvering capability of the robot is determined by the output control force of the thruster, and the control constraint mainly refers to the constraint on the size of the thrust of each part of the robot swarm.

[0136] 1.4. The boundary condition is the prerequisite for the optimization problem to have a definite solution, and in this case, the initial and terminal states of each part of the heterogeneous robot swarm are given as boundary conditions.

[0137] 1.5. The objective function represents the goal of the optimization model, and maximizing or minimizing a given objective function can obtain the optimal solution of the optimization problem. In this application, the observation range of all observation robots in the robot swarm on the asteroid and the fuel consumption of the robot swarm together form a composite index as the objective function.

[0138] 2. The established heterogeneous robot swarm cooperative control optimization problem is convex. First, the state variables including the positions, velocities and masses of all robots in the asteroid fixed coordinate system, and the control variables including the thrust vectors of all robots are constructed; the dynamics is rewritten into the form of the system state equation; the control constraint is rewritten into the convex function constraint form represented by the control variables; for some nonlinear and non-convex state constraints, linearization is performed, and then the linearized constraints and other state constraints are rewritten into the convex function constraint form represented by the state variables, so that the feasible region set is a convex set; similarly, the non-convex function part in the objective function can also be linearized to obtain a new objective function in the form of a convex function. Finally, the asteroid exploration heterogeneous robot swarm cooperative control convex optimization problem is given.

[0139] 3. The heterogeneous robot swarm cooperative control convex optimization problem is solved by using sequential convex programming. Sequential convex programming is a sequential iteration process for solving a series of convex constraint sub-problems. First, the heterogeneous robot swarm cooperative control convex optimization problem is discretized, and then for the discretized problem, a virtual compensation is introduced in the state constraint to eliminate the artificial infeasibility caused by linearization, and a trust region constraint composed of the state deviation of two iteration results is introduced to construct the heterogeneous robot swarm cooperative control convex constraint sub-problem. An initial reference trajectory is given, and the trajectory solved at present is taken as the reference trajectory for the next iteration to perform sequential iteration until the state variable difference between the two solved trajectories is less than the allowable error or the iteration number is reached. The last solved trajectory is the optimal solution of the asteroid exploration heterogeneous robot swarm trajectory cooperative planning problem.

[0140] Although the present application is disclosed with reference to the preferred embodiments above, it is not intended to limit the present application, and any person skilled in the art can make possible variations and modifications without departing from the spirit and scope of the present application. Therefore, the scope of protection of the present application should be defined by the scope of the claims.

Claims

1. A collaborative trajectory planning method for heterogeneous robot groups in asteroid exploration based on convex optimization, characterized in that, The method is applied to a heterogeneous robotic system for asteroid exploration, which includes one central robot and n observation robots. The method includes: The dynamic equations of the heterogeneous robot system for asteroid exploration are converted into system state equations. The state constraints of the heterogeneous robotic system for asteroid exploration are convexized. The control constraints are converted into constraints of control variables, the state constraints are converted into constraints of state variables, and the objective function of the asteroid exploration heterogeneous robot system is converted into a function of control variables and state variables. The objective function includes the altitude performance index and fuel performance index of all observation robots. Based on the system state equation, the convexized state constraints, the constraints of the control variables, and the constraints of the state variables, the objective function is optimized iteratively to obtain the group trajectory of the heterogeneous robot system for asteroid exploration. The system state equations satisfy: x i (t)=[r i (t),v i (t)] T u i (t)=T i (t) T Where i represents the i-th robot, i = 1, 2, 3, ..., n, and n is the total number of robots. When i = 1, it is the central robot; Ω is the angular velocity of the asteroid's rotation, Ω = [Ω x ,Ω y ,Ω z ] T r i (t) represents the displacement of the i-th robot, v i (t) represents the speed of the i-th robot, T i (t) represents the magnitude of the thrust experienced by the i-th robot, ▽U(r) i Let be the gravitational force exerted by the asteroid on the i-th robot; The objective function is: and The function, ω r and ω m ω represents the weighting coefficients for the overall observation range term and the fuel consumption term of the heterogeneous robot swarm, respectively. r >0, ω m >0, I sp Let g be the vacuum specific impulse of the thruster, g0 be the standard gravitational constant of the Earth, and t be the velocity of the thruster. f For terminal time; The objective function satisfies: l represents the number of iterations in the sequential convex programming, l = 1, 2, 3, ...; The state deviation between two sequence iterations. The value is the square of the trust region radius, and the center of the trust region at each discrete point is located at... Let ω be the penalty function for the trust region. δ These are the trust region weight coefficients; and These are virtual compensation items. and Let ω be the penalty function for the virtual compensation term. b ω e and ω c For virtual compensation term weighting coefficients, The optimization iteration of the objective function includes: Based on the system state equations, the convexized state constraints, the constraints of the control variables, and the constraints of the state variables, and given an initial reference trajectory containing all robot state variables, the objective function is solved. The current solution is used as the reference trajectory for the next solution, and this process is iterated until... If the value is less than a given threshold or the number of iterations is reached, the last obtained trajectory, which includes state variables and control variables, is the group trajectory of the asteroid exploration heterogeneous robot system.

2. The method according to claim 1, characterized in that, The state constraints after convexification satisfy: or, in, Φ=[E3 03], For x i The reference state of (t), k is the number of discrete time points, l represents the number of iterations of the sequential convex programming, l = 1, 2, 3, ..., and the center of the trust region at the kth discrete time point is located at This is a virtual compensation item.

3. The method according to claim 1, characterized in that, The state constraints after convexification satisfy: or, in, x(t)=[x1(t) T ,...,x i (t) T ,...,x j (t) T ,...,x n (t) T ] T , P ij For n×n blocks, each block is a 6×6 square matrix of coefficients. For the (i,i)th block... The (j,j)th block The remaining blocks are all zero square matrices; Let x(t) be the reference trajectory. The virtual compensation term is represented by k, which is the number of discrete time points, and l represents the iteration number of the sequential convex programming, where l = 1, 2, 3, ..., and the center of the trust region at the k-th discrete time point is located at... d is the radius of the safety sphere, which is a virtual compensation term.

4. The method according to claim 1, characterized in that, The state constraints after convexification satisfy: or, in, β is the semi-cone angle, H i For n×n blocks, each block is a 6×6 square matrix of coefficients. For the (i,i)th block... The remaining blocks are all zero-matrixes, i = 1, 2, ..., n, k is the number of discrete time points, l represents the iteration number of the sequential convex programming, l = 1, 2, 3, ..., and the trust region center of the k-th discrete time point is located at... This is a virtual compensation item.

5. The method according to claim 1, characterized in that, The constraints of the control variables satisfy: ‖u i (t)‖ ∞ ≤T max ,i=1,2,3,...,n Among them, T max This represents the maximum thrust of the thruster.

6. The method according to claim 1, characterized in that, The constraints on the state variables satisfy: ||Φx i (t)||2≤R i ,i=1,2,3,...,n Where Φ=[E303], R i The highest observation height for each robot during observation.

Images