Online optimal energy trajectory planning method based on maximum principle

Abstract

The present invention discloses an online optimal energy trajectory planning method based on the maximum principle, which belongs to the technical field of servo motion control and trajectory planning. First, a mechanical motion model of a servo motion control system is established, and jerk, acceleration constraint and speed constraint are introduced. With the system energy consumption as the optimization target, an optimization control problem is constructed. The optimal control quantity is derived by applying the Pontryagin maximum principle, and the original optimization control problem is finally transformed into a nonlinear two-point boundary value problem, and the optimal energy trajectory is obtained by solving the boundary value problem. For the optimization control problem with jerk, acceleration constraint and speed constraint, on the basis of solving the boundary value problem, a search algorithm is used to solve the optimal switching point according to the boundary value conditions. The present invention achieves the effect of online calculation of the optimal energy trajectory on a microcontroller platform with limited computing resources.

Description

Technical Field

[0001] The present invention relates to a speed trajectory planning technology, in particular to an online optimal energy trajectory planning method based on the maximum principle. Background Art

[0002] Velocity trajectory planning refers to the process of calculating the velocity trajectory under a given task time T and displacement θ. f Under the condition of , the displacement, velocity and acceleration at different time points within the planning time T are given. Velocity trajectory planning is widely used in the field of electric drive servo motion control. Commonly used velocity planning trajectories in industry include trapezoidal velocity trajectory, S-shaped velocity trajectory and polynomial velocity trajectory. None of the above velocity trajectories depends on the dynamic model of the system. In recent years, the optimal energy-speed trajectory based on optimization control theory has gradually attracted people's attention due to its excellent performance. The optimal energy-speed trajectory is obtained by establishing and solving the optimal control problem, which can minimize the energy consumption of the motor when performing point-to-point tasks.

[0003] There are usually numerical optimization methods, dynamic programming methods, and indirect methods for solving the optimal energy trajectory. Although numerical optimization methods and dynamic programming methods can handle complex constraint forms and performance indicators, they rely on a large number of computing resources and cannot complete online calculations. The indirect method based on the maximum principle has a fast calculation speed and is very suitable for online calculations. It can complete online calculations even on microcontrollers with limited computing resources. However, the current existing online calculation methods can only handle cases where the system dynamic model is linear, that is, its load torque usually only considers constant Coulomb friction. However, in actual servo motion control systems, in addition to Coulomb friction, its load torque usually also contains nonlinear terms related to the load position, resulting in nonlinearity in the system dynamic model. Existing methods make it difficult to calculate the optimal energy trajectory online. Summary of the Invention

[0004] The present invention aims to provide an online optimal energy trajectory planning method for nonlinear motion control systems. For a given point-to-point motion, this method can rapidly generate a corresponding optimal energy trajectory that is very smooth and minimizes the energy consumption of the electric drive system.

[0005] The technical solution for achieving the purpose of the present invention is: an online optimal energy trajectory planning method based on the maximum principle, comprising the following steps:

[0006] Step 1: Establish the mechanical motion equation of the system, take position and velocity as state variables, and rewrite the motion equation into a state space equation; take the energy consumption of the electric drive servo system as a performance indicator, and establish three types of optimal energy trajectory planning problems according to whether there is jerk, acceleration constraint or velocity constraint. The first type of optimal energy trajectory planning problem is an unconstrained optimization control problem consisting of a dynamic model and performance indicators; the second type of optimal energy trajectory planning problem is an optimization control problem with jerk and acceleration constraints, consisting of a dynamic model, performance indicators, jerk and acceleration constraints; the third type of optimal energy trajectory planning problem is an optimization control problem with jerk, acceleration constraint and velocity constraint, consisting of a dynamic model, performance indicators, jerk and acceleration constraints and velocity constraints;

[0007] Step 2: Based on the maximum principle, solve the three types of optimization control problems established in step 1 to obtain the corresponding optimal energy trajectory.

[0008] Compared with the existing technology, the present invention has the following significant advantages: based on the maximum principle, the present invention converts the online solution of the optimal energy trajectory planning problem into the solution of a two-point boundary value problem of a nonlinear ordinary differential equation system. For linear systems, the analytical solution of the boundary value problem can be directly obtained, while for nonlinear systems, the analytical solution of the boundary value problem cannot be obtained, so it is necessary to use numerical methods to quickly solve the nonlinear two-point boundary value problem. The present invention quickly solves the two-point boundary value problem based on the extreme value variation method, and successfully expands the online optimal energy trajectory planning method from linear systems to nonlinear systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] Figure 1 It is the structure of the solution to the second type of optimization control problem (optimization control problem with jerk and acceleration constraints).

[0010] Figure 2 It is the solution structure of the third type of optimization control problem (optimization control problem with jerk, acceleration constraints and velocity constraints).

[0011] Figure 3 These are the unconstrained optimal energy motion trajectory and trapezoidal speed trajectory of the embodiment of the present invention (load condition 1, task 1).

[0012] Figure 4 This is the optimal energy motion trajectory and trapezoidal velocity trajectory with jerk and acceleration constraint according to an embodiment of the present invention (load condition 1, task 2).

[0013] Figure 5 These are the unconstrained optimal energy motion trajectory and trapezoidal speed trajectory of the embodiment of the present invention (load case 2, task 1).

[0014] Figure 6 This is the optimal energy motion trajectory and trapezoidal velocity trajectory with jerk and acceleration constraint according to an embodiment of the present invention (load condition 2, task 2). DETAILED DESCRIPTION

[0015] like Figure 1 、 2 As shown in Figure 1, an online optimal energy trajectory planning method based on the maximum principle is shown in Figure 1. The specific steps are as follows:

[0016] Step 1: First, establish the system's mechanical equation of motion, using position and velocity as state variables and rewriting the equation of motion into a state-space equation. Minimizing energy consumption (energy consumption primarily considers mechanical work and motor copper loss) is then selected as the performance metric. The optimization control problem consists of a system dynamic model, performance metrics, and constraints. Three types of optimization control problems can be formulated, depending on whether jerk, acceleration constraints, or velocity constraints exist.

[0017] Specifically, the mechanical motion model is:

[0018]

[0019] Among them: J m is the system equivalent moment of inertia, θ is the motor rotor position, ω is the motor rotor speed, K t is the torque coefficient of the motor, u is the q-axis current of the motor, B m is the viscous damping coefficient, T L is the load torque.

[0020] The general expression for load torque is:

[0021]

[0022] Where c0 is the magnitude of the Coulomb friction force, and its sign is determined by the direction of the rotational speed. Since the direction of the rotational speed does not change in point-to-point motion, we can assume that ω ≥ 0 without loss of generality. f1(θ) can represent any load position-dependent load as long as it satisfies the following conditions in the interval [0, θ f ] can be continuously derived.

[0023] The motor position and speed are taken as state variables, recorded as X = [θ, ω], and the formula can be The dynamic equations shown are written in state-space form

[0024]

[0025] Where K = K t / Jm , F=(−1)*B m / J m ,C = (−1)*c0 / J m , D = (−1) / J m .

[0026] The performance index can be written as

[0027]

[0028] Among them, t f is the point-to-point task time, R s is the motor resistance, which is composed of mechanical work and copper loss of the motor.

[0029] When planning the optimal energy trajectory, the acceleration constraint and speed constraint of the servo motor are usually considered. In addition, in order to further improve the smoothness of the motion trajectory, the present invention also considers the jerk constraint.

[0030]

[0031] Among them, ω max is the velocity constraint, α max and α min are the upper and lower limits of the acceleration constraint respectively. lim is the acceleration constraint.

[0032] According to the formula The kinetic model shown in Eq. The performance indicators shown and the formula The constraints shown in the figure define the following three types of optimization control problems:

[0033] The first type of optimization control problem: based on the dynamic model and performance indicators Unconstrained optimization control problem composed of .

[0034]

[0035] The second type of optimization control problem: based on the dynamic model , performance indicators And the optimization control problem with jerk and acceleration constraints.

[0036]

[0037] The third type of optimization control problem: based on the dynamic model , performance indicators , jerk and acceleration constraints and velocity constraints with jerk, acceleration constraints and velocity constraints.

[0038]

[0039] Step 2: Based on the maximum principle, solve the three types of optimization control problems established in step 1.

[0040] Based on the maximum principle, solve the first type of optimization control problem (unconstrained optimization control problem) established in step 1. The specific solution steps are as follows:

[0041] Based on the maximum principle, co-state variables are introduced, the Hamiltonian H is established, and the co-state equation is constructed according to the Hamiltonian; according to the optimality condition ∂H / ∂u=0, the optimal control quantity is obtained, and then the optimal control quantity is substituted into the co-state equation and the state equation. Then the co-state equation and the state equation can form a Hamiltonian equation system, which is a nonlinear ordinary differential equation system; this nonlinear ordinary differential equation system and the corresponding boundary value conditions form a nonlinear two-point boundary value problem.

[0042] In a further embodiment, the Hamiltonian variable is defined as:

[0043]

[0044] Among them, λ θ and λ ω is a covariate variable.

[0045] According to the defined Hamiltonian, the co-state equation can be written as

[0046]

[0047] According to the optimality condition ∂H / ∂u = 0, we can get

[0048]

[0049] From the above formula, the optimal control quantity can be obtained as

[0050]

[0051] Substituting the above formula into the state equation and co-state equation, we can form a Hamilton equation system, which is a nonlinear ordinary differential equation system:

[0052]

[0053] For this differential equation system, due to the initial value λ of the covariate variable θ (0) and λ ω (0) is unknown, so it cannot be solved directly. In addition, the optimal energy trajectory should also meet the following boundary conditions:

[0054]

[0055] The above Hamilton equations and boundary conditions form a nonlinear two-point boundary value problem for ordinary differential equations. Solving the optimal energy trajectory planning problem is now transformed into solving a nonlinear two-point boundary value problem.

[0056] In a further embodiment, for the Hamilton equations and its corresponding boundary conditions The two-point boundary value problem of the nonlinear ordinary differential equations is solved by the extreme value variation method. This method first guesses the unknown covariate variable λ θ ,λ ω The initial guess is then iterated using Newton's method until the corresponding boundary conditions are met.

[0057] The specific solution process starts with the unknown co-state variable λ θ and λ ω We start with an initial guess of , which can be chosen based on physical intuition, randomly chosen, or simply set to 0. Using this initial guess, we can numerically integrate the Hamiltonian equations over the entire time interval. Because the initial guess is often inaccurate, the resulting solution often fails to satisfy the specified terminal boundary conditions. Therefore, we can compute the residual at the final time as follows:

[0058]

[0059] Among them, res1 and res2 represent the difference between the actual end position and velocity and the given boundary conditions after integrating the Hamilton equations using the guessed initial values ​​of the co-state variables. In fact, res1 and res2 are both about λ θ and λ ω A binary function of .

[0060] After obtaining the residuals, the next key step is to determine how to update and adjust the initial guess so that the residuals are controlled within an acceptable error range after a finite number of iterations. To speed up its convergence, the Newton method is used to iteratively correct the guessed values ​​of the covariates based on the residuals. The update rule for the initial guess can be expressed as:

[0061]

[0062] Among them, λ θ (k+1), λ ω (k+1) represents the initial value guess of the covariate variable after k iterations, res1(k) and res2(k) represent the initial value λ θ (k) and λ ω (k) The residual calculated after numerical integration. Jacobian is the Jacobian matrix, which is used to reflect the sensitivity of the residual to the initial value.

[0063]

[0064] The elements of the Jacobian matrix can be calculated using the finite difference method, which involves applying a small perturbation δ to the initial guess of the covariate variable. Specifically, the elements of the Jacobian matrix can be calculated using the following formula:

[0065]

[0066] After calculating the Jacobian matrix, we can use the formula Update and adjust the initial guess values ​​of the covariates. Then, use the updated and adjusted initial guess values ​​to numerically integrate the Hamiltonian equations over the entire time interval until the solution to the initial value problem satisfies the following boundary conditions:

[0067]

[0068] Where τ1 and τ2 are the error tolerances of the end position and velocity, respectively.

[0069] After the above process, we can finally get the true initial value of the co-state variable, so the formula and formula The resulting boundary value problem for the system of differential equations can be transformed into an initial value problem. The initial value problem can be solved using the classic fourth-order Runge-Kutta method, ultimately yielding the solution to the first-kind optimal control problem, which is the optimal energy trajectory under unconstrained conditions.

[0070] Continue to solve the second type of optimization control problem established in step 1 (optimization control problem with jerk and acceleration constraints). The specific solution steps are as follows:

[0071] For the optimization control problem with jerk and acceleration constraints, the basic structure of the optimization control solution is as follows: Figure 1 As shown. Figure 1 It can be seen that the optimized trajectory with jerk constraint is mainly composed of five segments of motion trajectory. The first segment is the interval [0, t j ], the second segment is the acceleration constraint trajectory on the interval [t j , t1], the third segment is the unconstrained motion on the interval [t1, t2], and the fourth segment is the acceleration constraint trajectory on the interval [t2, t f −t j ] on the acceleration constraint trajectory, the fifth segment is the interval [t f −t j , t f ] on the jerk-constrained trajectory.

[0072] Solving the optimized trajectory with jerk and acceleration constraints mainly depends on determining the switching time points t1 and t2. The present invention uses a search algorithm to find the switching time points t1 and t2.

[0073] First assume that t1(1) = 0, t2(1)= t f .

[0074] The motion trajectory on the interval [0, t1] can be given as follows:

[0075]

[0076] Similarly, the interval [t2, t f The motion trajectory on the interval [t1, t2] can be directly given. The key is to solve the unconstrained optimal energy motion trajectory on the interval [t1, t2]. According to the corresponding boundary conditions:

[0077]

[0078] The boundary conditions Substitute it into the first type of optimization control problem and solve it, and we can get the unconstrained motion trajectory on the interval [t1, t2]. Figure 1 It can be seen that the motion trajectory on the interval [t1, t2] must also meet the following conditions:

[0079]

[0080] Since t1 and t2 are randomly given at the beginning, according to the boundary conditions The obtained motion trajectory on the interval [t1, t2] must not satisfy the condition Therefore, the values ​​of t1 and t2 should be iteratively adjusted according to the error, so that after a finite number of iterations, the motion trajectory on the interval [t1, t2] satisfies the condition .

[0081] The specific method of iteratively adjusting t1 and t2 is as follows:

[0082]

[0083] Among them, γ k The acceleration trajectory on the interval [t1, t2] obtained by the k-th iteration is equal to α max The index at time T s is the integration step size.

[0084]

[0085] Among them, ε k The acceleration trajectory on the interval [t1, t2] obtained by the k-th iteration is equal to α min , length is the number of integration steps on the interval [t1, t2], and its size is , T s is the integration step size.

[0086] After adjusting the values ​​of t1 and t2, continue to solve the motion trajectory on the interval [t1, t2] until it meets the boundary conditions until.

[0087] Solve the third type of optimization control problem established in step 1 (optimization control problem with jerk, acceleration constraint and velocity constraint). The specific solution steps are as follows:

[0088] like Figure 2 As shown in the figure, the optimized trajectory with jerk, acceleration constraint and velocity constraint mainly consists of seven segments of motion trajectory. The first segment is the interval [0, t j ], the second segment is the acceleration constraint trajectory on the interval [t j , t1], the third segment is the unconstrained motion on the interval [t1, t3], the fourth segment is the velocity constraint trajectory on the interval [t3, t4], the fifth segment is the acceleration constraint trajectory on the interval [t4, t2], and the sixth segment is the velocity constraint trajectory on the interval [t2, t f −t j] on the acceleration constraint trajectory, the seventh segment is the interval [t f −t j , t f ] on the jerk-constrained trajectory.

[0089] Solving the optimized trajectory with jerk, acceleration constraints, and velocity constraints mainly involves determining the switching time points t3 and t4.

[0090] The optimized motion trajectory on the interval [0, t3] needs to meet the following boundary conditions:

[0091]

[0092] Substitute the above boundary conditions into the second type of optimization control problem, and then solve it to obtain the optimized motion trajectory on the interval [0, t3].

[0093] Similarly, the interval [t4, t f ], the following boundary conditions can be brought into the second type of optimization control problem to solve

[0094]

[0095] Since in the solution interval [0, t3] and [t4, t f ], the end displacement is not specified in the boundary conditions. Therefore, the solution does not meet the displacement conditions.

[0096]

[0097] Among them, Δθ1 and Δθ2 are the intervals [0, t3] and [t4, t f ], ρ represents the displacement error, which is related to the switching time points t3 and t4.

[0098] The next step is to iteratively adjust the values ​​of t3 and t4 so that after a finite number of iterations, the displacement error ρ approaches 0. , , transforming the problem of iteratively adjusting t3 and t4 into iteratively adjusting a single variable ζ. This allows the classic Newton bisection method to iteratively adjust the switching point ζ, so that the displacement error ρ converges to the allowable error range after a finite number of iterations.

[0099] Example

[0100] The present invention will be further described below with reference to the accompanying drawings. This embodiment is described by taking a typical dual-motor towing system as an example. Figure 1As shown in the figure, the system consists of two permanent magnet synchronous motors, one of which is used as the experimental motor to perform point-to-point tasks; the other motor is used as the load motor and uses current loop control to apply the corresponding load torque to the experimental motor. The system parameters are shown in the following table:

[0101] Table 1 System parameters

[0102]

[0103] Given the following task:

[0104] Table 2 Point-to-point location tasks

[0105]

[0106] In this embodiment, two typical load conditions are taken as examples. In the first condition, the load torque is T L = sin(g1θ), where ; The second load torque is T L = sin(g2θ), where For the first load case, Figure 3 Demonstrated the optimal energy trajectory for the unconstrained case (Task 1); Figure 4 The optimal energy trajectory with jerk and acceleration constraints (Task 2) is shown; for the second load case, Figure 5 Demonstrated the optimal energy trajectory for the unconstrained case (Task 1); Figure 6 Demonstrated the optimal energy trajectory with jerk and acceleration constraints (Task 2);

[0107] Throughout this specification, reference to terms such as "one embodiment," "some embodiments," "illustrative embodiments," "examples," "specific examples," or "some examples" means that a specific feature, structure, material, or characteristic described in conjunction with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, illustrative uses of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0108] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to the embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the claims and their equivalents.

Claims

1. An online optimal energy trajectory planning method based on the maximum principle, characterized in that: The following steps are involved: Step 1: Establish the mechanical motion equation of the system, use position and velocity as state variables, and rewrite the motion equation into a state space equation. The specific process is as follows: The mechanical motion equation is: Among them, J m is the system equivalent moment of inertia, θ is the motor rotor position, ω is the motor rotor speed, K t is the torque coefficient of the motor, u is the q-axis current of the motor, B m is the viscous damping coefficient, T L is the load torque; The general expression for load torque is: ； Where c0 is the magnitude of the Coulomb friction force, ω ≥ 0, and f1(θ) represents any load torque related to the load position, satisfying the interval [0, θ f ] is continuously differentiable; The motor position and speed are taken as state variables, denoted as X = [θ, ω], and the mechanical motion model is written in state space form: ； Where K = K t / J m , F=(−1)*B m / J m ,C = (−1)*c0 / J m , D = (−1) / J m Taking the energy consumption of the electric drive servo system as the performance indicator, three types of optimal energy trajectory planning problems are established according to whether there is jerk, acceleration constraint or speed constraint. The first type of optimal energy trajectory planning problem is an unconstrained optimization control problem consisting of a dynamic model and performance indicators, specifically: ； Where, E is the performance index representing the electric energy consumed during the motion process, ω is the motor rotor speed, R s is the motor resistance, t f is the exercise time, K t is the torque coefficient of the motor, u is the control variable, X = [θ, ω] is the state variables θ(0) and θ(t f ) represent the initial position and the end position, ω(0) and ω(t f ) represent the initial velocity and terminal velocity respectively, and f represents the system state equation, which is rewritten from the system mechanical motion equation. The specific expression is: ； Where K = K t / J m , F=(−1)*B m / J m ,C = (−1)*c0 / J m , D = (−1) / J m . f1(θ) is the load torque The second type of optimal energy trajectory planning problem is an optimization control problem with jerk and acceleration constraints, which consists of a dynamic model, performance indicators, and jerk and acceleration constraints. Specifically: ； Where, E is the performance index representing the electric energy consumed during the motion process, ω is the motor rotor speed, R s is the motor resistance, t f is the task time, K t is the torque coefficient of the motor, u is the control variable, X = [θ, ω] is the state variable of the system; j lim is the acceleration constraint, α min and α max are the lower and upper limits of the acceleration constraint, θ(0) and θ(t f ) represent the initial position and the end position, ω(0) and ω(t f ) represent the initial velocity and terminal velocity respectively, and f represents the system state equation; The third type of optimal energy trajectory planning problem is an optimization control problem with jerk, acceleration and velocity constraints, which is composed of a dynamic model, performance indicators, jerk and acceleration constraints, and velocity constraints. Specifically: ； Step 2: Based on the maximum principle, solve the three types of optimization control problems established in step 1 and obtain the corresponding optimal energy trajectories, including: Based on the maximum principle, the first type of optimal energy trajectory planning problem in step 1 is solved as follows: Based on the maximum principle, co-state variables are introduced, Hamiltonian is established, and co-state equations are constructed based on Hamiltonian; The optimal control quantity is obtained according to the optimality condition, and the optimal control quantity is substituted into the co-state equation and the state space equation to form a Hamiltonian equation system; the Hamiltonian equation system and the boundary conditions given in the optimization control problem form a two-point boundary value problem of a nonlinear ordinary differential equation system; The extreme value variation method is used to solve the two-point boundary value problem, and the true initial value of the unknown co-state variable is obtained. The two-point boundary value problem is transformed into an initial value problem for solution. The fourth-order Runge-Kutta method is used to solve the initial value problem and obtain the unconstrained optimal energy motion trajectory. Based on the maximum principle, the second type of optimal energy trajectory planning problem in step 1 is solved as follows: The optimized trajectory with jerk constraint consists of five segments of motion trajectories. The first segment is the interval [0, t j ], the second segment is the acceleration constraint trajectory on the interval [t j , t1], the third segment is the unconstrained motion on the interval [t1, t2], and the fourth segment is the acceleration constraint trajectory on the interval [t2, t f −t j ] on the acceleration constraint trajectory, the fifth segment is the interval [t f −t j , t f ] on the jerk-constrained trajectory; Solve the optimal trajectory with jerk and acceleration constraints, that is, use the search algorithm to determine the switching time points t1 and t2, specifically: Assume t1(1) = 0, t2(1) = t f , solve the unconstrained motion trajectory on the interval [t1, t2], and the corresponding boundary conditions are: ； Substitute the above boundary conditions into the first type of optimization control problem and solve it to obtain the unconstrained motion trajectory on the interval [t1, t2]; The motion trajectory on the interval [t1, t2] also satisfies the following conditions: ； When the obtained motion trajectory on the interval [t1, t2] does not meet the above conditions, the values ​​of t1 and t2 are iteratively adjusted according to the error situation so that the motion trajectory on the interval [t1, t2] meets the above conditions after a finite number of iterations; The specific method of iteratively adjusting t1 and t2 is as follows: ； Among them, γ k The acceleration trajectory on the interval [t1, t2] obtained by the k-th iteration is equal to α max The index at time T s is the integration step length; ； Among them, ε k The acceleration trajectory on the interval [t1, t2] obtained by the k-th iteration is equal to α min , length is the number of integration steps on the interval [t1, t2], and its size is , T s is the integration step length; After adjusting the values ​​of t1 and t2, continue to solve the motion trajectory on the interval [t1, t2] until the boundary conditions are met; Based on the maximum principle, solve the third type of optimal energy trajectory planning problem in step 1. The specific method is: The optimized trajectory with jerk, acceleration constraint and velocity constraint consists of seven segments of motion trajectory. The first segment is the interval [0, t j ], the second segment is the acceleration constraint trajectory on the interval [t j , t1], the third segment is the unconstrained motion on the interval [t1, t3], the fourth segment is the velocity constraint trajectory on the interval [t3, t4], the fifth segment is the acceleration constraint trajectory on the interval [t4, t2], and the sixth segment is the velocity constraint trajectory on the interval [t2, t f −t j ] on the acceleration constraint trajectory, the seventh segment is the interval [t f −t j , t f ] on the jerk-constrained trajectory; Solve the optimized trajectory with jerk, acceleration constraint and velocity constraint, that is, determine the switching time points t3 and t4; The optimized motion trajectory on the interval [0, t3] is solved by bringing the following boundary conditions into the second-type optimization control problem: ; interval [t4, t f ], the following boundary conditions can be brought into the second type of optimization control problem to solve: ; Solve for the intervals [0, t3] and [t4, t f ], check the solved motion trajectory and whether the total displacement is equal to the given displacement; since in the solution interval [0, t3] and [t4,t f ], the end displacement is not specified in the boundary conditions, so the solution does not meet the displacement conditions, and the error is: ; Among them, Δθ1 and Δθ2 are the intervals [0, t3] and [t4,t f ], ρ represents the displacement error; When ρ is not 0, iteratively adjust the values ​​of t3 and t4 so that after a finite number of iterations, the displacement error ρ approaches 0; let , , the problem of iterative adjustment of t3 and t4 is transformed into iterative adjustment of a single variable ζ; the classical Newton bisection method is used to iteratively adjust the switching point ζ so that the displacement error ρ converges to the allowable error range after a finite number of iterations.

2. The online optimal energy trajectory planning method based on the maximum principle according to claim 1 is characterized in that: The Hamiltonian is established as: ； Among them, λ θ and λ ω is a co-variable; The optimal control quantity is: ； The Hamiltonian equations are specifically: ； The corresponding boundary conditions are: 。 3. The online optimal energy trajectory planning method based on the maximum principle according to claim 2 is characterized in that: The Hamiltonian equations and the corresponding boundary conditions are solved using the extreme value variation method to obtain the initial values ​​of the unknown covariates. The specific method is as follows: Set the unknown covariate variable λ θ and λ ω Initial value λ θ (0) and λ ω (0); For the Hamiltonian equations in the entire time interval [0, t f ] and calculate the residual at the final moment, as follows: ； Where res1 and res2 represent the difference between the actual end position and velocity and the given boundary conditions after integrating the Hamilton equations using the guessed initial values ​​of the co-state variables; The Newton method is used to iteratively correct the initial co-state variables according to the residuals, specifically: ； Where Jacobian is the Jacobian matrix; ； After calculating the Jacobian matrix based on the residuals, the initial guesses of the covariates are updated, and the updated guesses are used to integrate the system of equations again to calculate the residuals of the terminal boundary conditions. After a finite number of updates and adjustments, the residuals of the terminal boundary conditions will eventually converge to meet the following boundary conditions: ； Where τ1 and τ2 are the error tolerances of the end position and velocity, respectively.

Images