A square-sum optimization based obstacle avoidance trajectory planning method

Abstract

The application relates to a square-sum optimization-based obstacle avoidance trajectory planning method, which is used for solving the technical problems that safety distance is difficult to quantitatively consider and an obstacle model is poor in adaptability in the prior art, and comprises the following steps: establishing an optimal control problem P0 without considering an obstacle avoidance condition; performing convexification and discrete processing on the problem P0 to obtain a convex optimization problem P1; describing a physical space occupied by an obstacle as a finite polynomial function inequality; describing an obstacle avoidance condition as a square-sum obstacle avoidance constraint; eliminating non-convexity in the square-sum obstacle avoidance constraint; adding the convexized square-sum obstacle avoidance constraint to the problem P1 and modifying an objective function of the problem P1 to construct a square-sum optimization problem P2; and iteratively solving the square-sum optimization problem P2 to obtain an obstacle avoidance trajectory. The application can quantitatively consider safety distance constraints, thereby reducing collision risks introduced by factors such as uncertainty and disturbance in an actual movement process, and is suitable for any obstacle model described by a finite polynomial.

Description

Technical Field

[0001] This invention belongs to the field of motion planning technology, specifically relating to an obstacle avoidance trajectory planning method based on square sum optimization. Background Technology

[0002] To enable motion systems such as aircraft and robots to complete predetermined tasks, a trajectory from the current position to the target position needs to be planned. Furthermore, if obstacles or terrain exist in the physical space where the system moves, the planned trajectory must not intersect with these obstacles. The trajectory planning problem is typically modeled as an optimal control problem with a performance index and multiple constraints on state and control variables. Common performance indices include minimizing fuel / energy consumption, travel time, or maximizing range and speed. Constraints generally include system state equations, process constraints, and boundary conditions. In recent years, numerical optimization methods, represented by convex programming and mixed-integer programming, have become classic methods for solving obstacle avoidance trajectory optimal control problems and have gained widespread attention.

[0003] For example, in 2020, Bai Chengchao et al. published "Optimal guidance for planetary landing in hazardous terrains" in Volume 56, Issue 4 of *IEEE Transactions on Aerospace and Electronic Systems*, disclosing a method for obstacle avoidance trajectory planning based on convex optimization. This method models obstacles as cones and approximates obstacle avoidance constraints as linear constraints. However, since the obstacle avoidance constraints are only applied to a finite number of discrete points, under the influence of optimal fuel performance indicators, the resulting trajectory is very close to the obstacle boundary, and there is a possibility of collision between trajectory segments between discrete points. Although this method can adjust the conservatism of the planned trajectory (relative to the distance to the obstacle) through the relaxation coefficient in the obstacle avoidance constraints, it cannot quantitatively constrain the distance between the planned trajectory and the obstacle. As another example, in 2015, Arthur Richards et al. published "Inter-sample avoidance in trajectory optimizers using mixed-integer linear programming" in Volume 25 of *International Journal of Robust and Nonlinear Control*, disclosing an interior-point obstacle avoidance trajectory planning method based on mixed-integer programming. This method models obstacles as polyhedra and proposes interior point obstacle avoidance conditions to prevent collisions between discrete point trajectory segments and obstacles. However, considering the system's overall size and the uncertainties and disturbances during actual motion, collision risks still exist when moving along the planned trajectory. If obstacle avoidance constraints satisfying a specified safe distance can be explicitly considered and solved, these potential collision risks can be significantly reduced.

[0004] Besides the difficulty in quantifying the safe distance, most existing obstacle avoidance trajectory planning methods are designed for simple obstacle models such as polyhedra, cones, and spheres, which have poor applicability to obstacle models. Summary of the Invention

[0005] The technical problem to be solved by this invention is:

[0006] To avoid the shortcomings of existing technologies, this invention provides an obstacle avoidance trajectory planning method based on sum of squares optimization, which addresses the problems of existing technologies having difficulty in quantitatively considering safety distances and insufficient applicability of obstacle models.

[0007] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0008] An obstacle avoidance trajectory planning method based on sum of squares optimization is characterized by the following steps:

[0009] Step 1: Establish the optimal control problem P0 without considering obstacle avoidance conditions:

[0010]

[0011]

[0012] ψ[x(t I ),t I ,x(t F ),t F ] = 0

[0013] P[x(t),u(t),t]≤0

[0014] in, For containing position vectors The state variables are: u (control variable), t (time), J (performance index function), f (state equation function), and ψ (boundary condition function). I At the initial time, t f The constraint function for process P at the terminal time;

[0015] Step 2: Perform convexification and discretization on problem P0 to obtain the convex optimization problem P1:

[0016] P1 min Z F(Z)

[0017] sth(Z)=0

[0018] g(Z)≤0

[0019] Where Z is the vector r containing the position at the discrete point. i The decision variables are i = 1, 2, ..., N; N is the number of discrete points, F is a linear function, h is an affine function, and g is a convex function.

[0020] Step 3: Describe the physical space occupied by the obstacle as a finite number of polynomial function inequalities:

[0021] O1(x)≤0,O2(x)≤0,…,O M (x)≤0

[0022] Where M is the number of polynomial functions used to describe the obstacle. For the corresponding position vector The undetermined elements;

[0023] Step 4: Describe the obstacle avoidance conditions as a sum of squares obstacle avoidance constraint:

[0024]

[0025] Where ||·|| is the L2 norm operator, d is the specified safety distance, and L i,m (x) is the multiplier polynomial whose coefficients are to be solved in the case of the indeterminate variable x, and S[x] is the set of polynomials of the sum of squares in the case of the indeterminate variable x;

[0026] Step 5: Eliminate non-convexity in the sum of squares obstacle avoidance constraints:

[0027] (5a) Introduce slack variable η i Constraints Equivalent replacement:

[0028]

[0029] (5b) Introducing slack variable ε i constrain ||r i || 2 -η i =0 can be replaced with:

[0030]

[0031] (5c) Apply the non-convex constraint ||r i || 2 -η i ≥0 is approximately:

[0032] 2r i-guess T r i -||r i-guess || 2 -η i ≥0i=1,2,…,N

[0033] Where, r i-guess r is the position vector at the discrete point. i The initial guess;

[0034] Step 6: Add the sum of squares obstacle avoidance constraint after eliminating non-convexity to problem P1 and modify the objective function of problem P1 to construct the sum of squares optimization problem P2:

[0035]

[0036] sth(Z)=0

[0037] g(Z)≤0

[0038]

[0039] Wherein, the penalty factor γ > 0;

[0040] Step 7: Iteratively solve the sum-of-squares optimization problem P2 to obtain the obstacle avoidance trajectory:

[0041] (7a) Specify the safety distance d > 0 and the tolerance e > 0, and give the initial guess r. i-guess (i = 1, 2, ..., N);

[0042] (7b) Solve the sum of squares optimization problem P2, and obtain the position vector r at the discrete points. i Let it be r i-sol (i = 1, 2, ..., N);

[0043] (7c) Let r i-guess =r i-sol Solve the sum of squares optimization problem P2, and obtain the position vector r at the discrete points. i Let it be r i sol (i = 1, 2, ..., N);

[0044] (7d) If max i ||r i-sol -r i-guess If ||≥e, proceed to step (7c); otherwise, output r. i-sol (i = 1, 2, ..., N) serve as the obstacle avoidance trajectory.

[0045] A computer system is characterized by comprising: one or more processors, and a computer-readable storage medium for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors cause the one or more processors to implement the method described above.

[0046] A computer-readable storage medium is characterized by storing computer-executable instructions, which, when executed, are used to implement the above-described method.

[0047] The beneficial effects of this invention are as follows:

[0048] The obstacle avoidance trajectory planning method based on sum of squares optimization provided by this invention has the following advantages compared with the prior art:

[0049] (1) The present invention models obstacle avoidance conditions as a sum of squares constraint, which can explicitly consider the safety distance and ensure that the minimum distance between the planned trajectory and the obstacle is not less than the limit, thereby reducing the potential collision risk introduced by uncertainties and disturbances in the actual movement process.

[0050] (2) The square sum obstacle avoidance constraint in this invention is applicable to any obstacle described by a finite number of polynomials, and is independent of the specific expression of the polynomial (reflecting the shape of the obstacle), thus having good applicability. Attached Figure Description

[0051] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0052] Figure 1 This is a flowchart illustrating the implementation of the present invention.

[0053] Figure 2 The example shows the obstacle avoidance trajectory for a conical obstacle with a safety distance d=0.

[0054] Figure 3 The example shows the obstacle avoidance trajectory for a conical obstacle with a safety distance of d = 100m.

[0055] Figure 4 The example shows the obstacle avoidance trajectory for three conical obstacles with a safety distance of d = 100m.

[0056] Figure 5 The example shows an obstacle avoidance trajectory for a non-convex obstacle with a safety distance of d = 100m. Detailed Implementation

[0057] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0058] like Figure 1 As shown, this embodiment of the invention provides an obstacle avoidance trajectory planning method based on sum of squares optimization, including the following steps:

[0059] Step 1: Establish the optimal control problem P0 without considering obstacle avoidance conditions:

[0060] Let the initial time t be I =0, given terminal time t F The Mars landing trajectory planning problem, without considering obstacle avoidance conditions, can be modeled as the following optimal control problem P0:

[0061] Performance indicators

[0062] Equations of state

[0063] Process constraints

[0064] Boundary conditions

[0065] Where t is time, and r = [rx ,r y ,r z ] T v, g, and T are the position vector, velocity vector, gravitational acceleration vector, and thrust vector, respectively; m is the lander mass; α is the flow coefficient; ρ2 and ρ1 are the upper and lower bounds of the thrust amplitude, respectively; r yz =[r y ,r z ] T θ alt Given a minimum altitude angle to avoid the lander colliding with the Martian surface; r I v I and m I Given the initial position, velocity, and mass, respectively, the state variables for this optimal control problem are x(t) = [r(t)]. T ,v(t) T [m(t)] T The control quantity u(t) = T(t).

[0066] Step 2: Perform convexification and discretization on problem P0 to obtain the convex optimization problem P1:

[0067] Applying the convexity method to problem P0, we introduce a slack variable Γ(t) and perform variable substitution: Set N discrete points t1, t2, ..., tt with uniform intervals Δt. N Where t1=0, t N =t F By using the trapezoidal method to discretize the state equations, the convex optimization problem P1 is obtained:

[0068]

[0069] str1 = r I v1=v I m1=m I ,r N =0,v N =0

[0070] for i = 2, 3, ..., N

[0071]

[0072]

[0073]

[0074] for i = 1, 2, 3, ..., N

[0075] ||r yzi ||≤rxi ·tan(π / 2-θ alt )

[0076] ln(m I -αρ2t i )≤z i ≤ln(m I -αρ1t i )

[0077] ||u i ||≤σ i

[0078]

[0079] σ i ≤μ 2i {1-[z i -z 0i ]}

[0080] The decision variable Z at the i-th discrete point i =[u i T ,σ i ] T The vector formed by all decision variables is Z = [Z1] T Z2 T ,…,Z N T ] T .

[0081] Step 3: Describe the physical space occupied by the obstacle as a finite number of polynomial function inequalities:

[0082] O1(x)≤0,O2(x)≤0,…,O M (x)≤0 (1)

[0083] Where M is the number of polynomial functions used to describe the obstacle. For the corresponding position vector The undetermined elements.

[0084] Step 4: Describe the obstacle avoidance conditions as a sum of squares obstacle avoidance constraint:

[0085]

[0086] Where ||·|| is the L2 norm operator, d is the specified safety distance, and L i,m (x) is a multiplier polynomial whose coefficients are to be solved in the case of an indeterminate variable x, and S[x] is the set of polynomials of the sum of squares in the case of an indeterminate variable x.

[0087] If a polynomial P(x) with respect to an indeterminate variable x can be expressed as a finite number of polynomials qi The sum of (x), that is

[0088]

[0089] Then P(x) is called a sum of squares (polynomial). Clearly, a polynomial being a sum of squares is a sufficient condition for its non-negativity. Equation (2) specifies L. i,m (x) is a sum of squares polynomial, and from equation (1) we know that Easy to understand, trajectory r i When (i=1,2,…,N) satisfies equation (2), ||xr|| must hold. i || 2 -d 2 ≥0 means maintaining a specified safe distance d from the obstacle.

[0090] The sum-of-squares optimization problem is a special type of convex optimization problem, and its general form is:

[0091] min y FF(y)

[0092] stP i (x; y)∈S[x],i=1,...,K

[0093] Where FF is a linear function, P i (x; y) is a polynomial that is an affine expression for y, where y is the decision variable and is also a polynomial P. i The coefficients of each monomial component in (x; y). Based on existing literature, the sum of squares optimization problem can be transformed into a semidefinite programming problem (a common type of convex optimization problem), and solved using convex optimization algorithms such as the primal-dual interior point method. However, in equation (2), ||xr i || 2 An experiment introduced a quadratic term ||r| related to the decision variable. i || 2 The non-convexity must be eliminated first.

[0094] Step 5: Eliminate the non-convexity in the sum of squares obstacle avoidance constraint (2):

[0095] Step 5a: Introduce slack variable η i Constraints Equivalent replacement:

[0096]

[0097] The first equation of equation (3) already satisfies the specification of the sum of squares optimization problem, and the non-convexity is concentrated in the second equation.

[0098] Step 5b: Introduce slack variable ε i constrain ||ri || 2 -η i =0 can be replaced with:

[0099]

[0100] The non-convexity of equation (4) exists in the second equation.

[0101] Step 5c: Apply the non-convex constraint ||r i || 2 -η i ≥0 is approximately:

[0102] 2r i-guess T r i -||r i-guess || 2 -η i ≥0i=1,2,…,N

[0103] Where, r i-guess r is the position vector at the discrete point. i Based on the initial guess, the convexification of the non-convex square sum obstacle avoidance constraint (2) is now complete.

[0104] Step 6: Add the sum of squares obstacle avoidance constraint after eliminating non-convexity to problem P1 and modify the objective function of problem P1 to construct the sum of squares optimization problem P2:

[0105]

[0106] str1 = r I v1=v I m1=m I ,r N =0,v N =0

[0107] for i = 2, 3, ..., N

[0108]

[0109]

[0110]

[0111] for i = 1, 2, 3, ..., N

[0112] ||r yzi ||≤r xi ·tan(π / 2-θ alt )

[0113] ln(m I -αρ2ti )≤z i ≤ln(m I -αρ1t i )

[0114] ||u i ||≤σ i

[0115]

[0116] σ i ≤μ 2i {1-[z i -z 0i Among them, punishment

[0117] Obstacle avoidance constraints

[0118] Factor γ > 0. The multiplier polynomial L in the decision variables of problem P2. i,m (x), more precisely, L i,m (x) The coefficients of each monomial component to be solved. If the polynomial O describing the obstacle... m If (x)(m=1,2,…,M) does not exceed the second order, then L i,m (x) can be set as a zero-order polynomial, that is, it degenerates into a constant to be solved.

[0119] Step 7: Iteratively solve the sum-of-squares optimization problem P2 to obtain the obstacle avoidance trajectory:

[0120] Step 7a: Specify the safety distance d > 0 and the tolerance e > 0, and give the initial guess r. i-guess (i = 1, 2, ..., N);

[0121] Step 7b: Solve the sum of squares optimization problem P2, and obtain the position vector r at the discrete points. i Let it be r i-sol (i = 1, 2, ..., N);

[0122] Step 7c: Let r i-guess =r i-sol Solve the sum of squares optimization problem P2, and obtain the position vector r at the discrete points. i Let it be r i sol (i = 1, 2, ..., N);

[0123] Step 7d: If max i ||r i-sol -r i-guess If ||≥e, proceed to step 7c; otherwise, output r. i-sol (i = 1, 2, ..., N) serve as the obstacle avoidance trajectory.

[0124] The technical effects of this invention are further illustrated below with numerical simulation experiments. All simulations were performed using the YALMIP toolbox within the MATLAB environment, with MOSEK as the underlying optimization solver. The lander body parameters and mission parameters used in the simulation are shown in Table 1.

[0125] Table 1 Lander Body Parameters and Mission Parameters

[0126]

[0127] Simulation Experiment A. Consider a single conical obstacle, which can be described by the following polynomial:

[0128]

[0129] Let its semi-cone angle and vertex position vector be...

[0130] β = 40°, H = [774, 100, 1000] T m

[0131] The safety distance d = 0 was set, and the simulation results are attached. Figure 2 As shown in the figure. The solid line represents the trajectory planned by this invention (solved under square sum obstacle avoidance constraints), and the dashed line represents the trajectory planned by the method in the first comparative document "Optimal Guidance for Planetary Landing in Hazardous Terrains" in the background section (solved under linear obstacle avoidance constraints). The two are basically consistent, demonstrating the effectiveness of this invention. A safety distance of d = 100m is set, and the planning results are attached. Figure 3 As shown, the pipe-like area attached to the trajectory is a sphere with a radius of 100m centered on the trajectory (discrete point). It can be seen that the trajectory maintains a given safe distance when avoiding obstacles, which is something that the method in the first comparative document in the background section cannot achieve.

[0132] Simulation Experiment B. Three conical obstacles were investigated simultaneously, and their semi-cone angles and vertex positions are shown in Table 2. A safety distance of d = 100m was set, and the planning results are attached. Figure 4 As shown, the pipe-like area attached to the trajectory is a sphere with a radius of 100m centered on the trajectory (discrete point). It can be seen that the trajectory maintains a given safe distance when avoiding three obstacles, indicating that the present invention is applicable to multi-obstacle scenarios.

[0133] Table 2 Parameters of the three conical obstacles

[0134]

[0135] Simulation Experiment C. Consider a non-convex obstacle. The obstacle is modeled as the following polynomial:

[0136]

[0137] The planning results are attached. Figure 5 As shown, the pipe-like area attached to the trajectory is a sphere with a radius of 100m centered on the trajectory (discrete point). It can be seen that the trajectory maintains a given safe distance when avoiding obstacles, indicating that the present invention is applicable to non-convex obstacle models.

[0138] In summary, compared with the two prior art documents in the background section, the present invention can quantitatively consider the safety distance, thereby reducing the potential collision risk introduced by uncertainties and disturbances during actual movement; and it is applicable to obstacles described by a finite number of polynomials, regardless of the specific expression of the polynomial (reflecting the shape of the obstacle).

[0139] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the scope of the technology disclosed in the present invention, and such modifications or substitutions should all be covered within the scope of protection of the present invention.

Claims

1. A method for obstacle avoidance trajectory planning based on sum of squares optimization, characterized in that... The steps are as follows: Step 1: Establish the optimal control problem P0 without considering obstacle avoidance conditions: in, For containing position vectors The state variables, To control the quantity, For time, For performance index functions, The state equation function, For boundary condition functions, At the initial moment, For the terminal time, Process constraint functions; Step 2: Perform convexification and discretization on problem P0 to obtain the convex optimization problem P1: in, For the position vectors at discrete points Decision variables, ; The number of discrete points. It is a linear function. For affine functions, It is a convex function; Step 3: Describe the physical space occupied by the obstacle as a finite number of polynomial function inequalities: in, The number of polynomial functions used to describe the obstacle. For the corresponding position vector The undetermined elements; Step 4: Describe the obstacle avoidance conditions as a sum of squares obstacle avoidance constraint: in, For L2 norm operators, For the specified safe distance, For the undetermined elements The coefficients of the multiplier polynomial to be solved are... For the undetermined elements The set of sums of squares and polynomials; Step 5: Eliminate non-convexity in the sum of squares obstacle avoidance constraints: (5a) Introducing slack variables Constraints Equivalent replacement: (5b) Introducing slack variables Constraints Replace with: (5c) Non-convex constraints Approximately: in, Position vector at discrete point The initial guess; Step 6: Add the sum of squares obstacle avoidance constraint after eliminating non-convexity to problem P1 and modify the objective function of problem P1 to construct the sum of squares optimization problem P2: Among them, the penalty factor ; Step 7: Iteratively solve the sum-of-squares optimization problem P2 to obtain the obstacle avoidance trajectory: (7a) Specifying a safe distance and tolerance Given an initial guess ; (7b) Solve the sum of squares optimization problem P2, and obtain the position vectors at the discrete points. Recorded as ; (7c) Let Solve the sum of squares optimization problem P2, and obtain the position vectors at the discrete points. Recorded as ; (7d) If Proceed to step (7c); otherwise, output... As an obstacle avoidance trajectory.

2. A computer system, characterized in that... include: One or more processors, a computer-readable storage medium for storing one or more programs, wherein, when the one or more programs are executed by the one or more processors, the one or more processors cause the one or more processors to implement the method of claim 1.

3. A computer-readable storage medium, characterized in that... The device stores computer-executable instructions, which, when executed, are used to implement the method of claim 1.

Images