Over-driven flight system teleoperation planning method based on MINCO trajectory

Abstract

The invention discloses an over-driven flight system teleoperation planning method based on a MINCO trajectory, and relates to the field of flight system trajectory planning. The problems that efficiency is reduced due to non-uniqueness, caused by redundancy characteristics, of inverse kinematics of a system during teleoperation of an overdrive vector rotor flight system in the prior art, and the process of seeking optimal solutions of multiple planned feasible solutions is complex in planning, long in consumed time and incapable of guaranteeing tracking precision are solved. According to the method, the waypoint of the trajectory and the time parameterization trajectory are directly utilized, the energy consumption of the system, the stability of the trajectory and the obstacle avoidance characteristic serve as optimization targets, the quasi-Newton method is used for accelerating convergence to the optimal solution, and the optimal trajectory is obtained to guarantee the tracking precision of the follow-up tracking process. The method is mainly used for trajectory planning of the teleoperation system.

Description

Technical Field

[0001] The present invention relates to the field of flight system trajectory planning. Background Art

[0002] Aerial rotorcraft systems combine the high maneuverability of drones with the complex operational capabilities of control devices, support active interaction with the environment, and have broad application prospects. Traditional rotorcraft exhibit coupled attitude and position control. Their underactuated nature makes spatial trajectory tracking of the low-degree-of-freedom end-of-arm manipulator difficult, hindering subsequent contact operations. Overactuated vector rotorcraft systems, however, decouple position and attitude control based on vectored rotor thrust, enabling independent control of the flight platform's posture and significantly expanding the operational scope for active interaction with the environment.

[0003] The integration of teleoperation technology with overdriven vector rotor flight systems further raises the upper limit of aerial operation systems. The operator remotely controls the contact operations of the aerial operation system within the control loop, combining human intelligence with the precision of the aerial operation device to efficiently complete the task. However, teleoperated overdriven vector rotor flight systems typically have more degrees of freedom than the task requires. This redundancy determines the non-uniqueness of the inverse kinematic solution while maintaining the trajectory of the end effector. This non-uniqueness reduces efficiency. Furthermore, during teleoperation, the dynamic obstacle avoidance requirements that may arise when the flight system executes the original trajectory require the optimal solution from the many feasible solutions planned. However, traditional planning methods require a large amount of computation to find the optimal solution from multiple sets of redundant solutions, which are high in dimensionality and may not necessarily pass through control points, resulting in an inability to guarantee the accuracy of tracking the end trajectory. Therefore, the above problems need to be solved urgently. Summary of the Invention

[0004] The purpose of the present invention is to solve the problems of traditional remote control of overdriven vector rotor flight systems, such as the non-uniqueness of the system kinematic inverse solution caused by redundant characteristics, which leads to reduced efficiency, and the complex and time-consuming process of seeking the optimal solution of the many planned feasible solutions, and the inability to guarantee tracking accuracy. The present invention provides a remote control planning method for overdriven flight systems based on MINCO trajectories.

[0005] A teleoperation planning method for an overdriven flight system based on MINCO trajectories is implemented based on a teleoperation system. The teleoperation system includes ground equipment and a flight system. The ground equipment is used to remotely operate the flight system. The flight system is a vector rotor flight system, including a 6-degree-of-freedom UAV and an n-degree-of-freedom robotic arm. The method includes the following steps:

[0006] S1. The ground equipment is used to calculate the position of the end effector of the flight system based on the currently collected operator posture; wherein the end effector of the flight system is the end effector of the robotic arm;

[0007] Generate a trajectory ξ of the end effector with a dimension of 6 according to the pose of the end effector of the flight system;

[0008] S2. Differential flat transformation: The onboard processor of the UAV solves the trajectory ξ using the Jacobian matrix and plans it in the null space of the flight system state. Based on the differential flatness property of the flight system, the flight system state variable x(t) is selected as the flat output trajectory with the dimension of the flight system state variable being n+6.

[0009] S3, MINCO trajectory parameterization: The flat output trajectory x(t) is parameterized by MINCO trajectory parameterization to obtain trajectory x(q,T′); x(q,T′) is represented by the intermediate waypoint vector q and the time vector T′ between waypoints, and q and T′ are both MINCO trajectory parameters; q = (q1,…,q M-1 ) T , T′=(T′1,…,T′ M ) T ,q a is the position information of the ath intermediate waypoint in the flight process except the first and last waypoints, T′ i is the time vector between the first and last waypoints corresponding to the i-th trajectory during the flight process, a=,1,2……M-1, i=1,2,3……M, M is the total number of segments;

[0010] S4. Construct an unconstrained nonlinear objective function: Design the energy cost function J based on the time vector T′ and the coefficient matrix c c (c,T′), continuity constraint cost penalty function J s (c,T′) and the obstacle avoidance constraint cost penalty function J g (c,T′), and J c (c,T′,j s (c,T′),J g (c, T′) and the total time cost penalty function J t The sum is minimized, and an unconstrained nonlinear objective function is constructed; c is the coefficient matrix of the trajectory x(q,T′);

[0011] S5, MINCO gradient conduction:

[0012] Solve the unconstrained nonlinear objective function J c (c,T′),J s (c,T′),J g (c,T′) and J t , respectively, with respect to the gradient of the coefficient matrix c and the gradient with respect to the time vector T′ J is a function variable, and J refers to Jc (c,T′),J s (c,T′),J g (c,T′) and J t Any function in

[0013] The gradient Conducted to the MINCO trajectory parameter q, the gradient is obtained

[0014] The gradient Conducted to the MINCO trajectory parameter T', the gradient is obtained

[0015] W represents the function of the function variable J parameterized by the MINCO trajectory;

[0016] S6. Diffeomorphism Exchange: Gradient Conducted to the gradient τ is a time-unconstrained proxy variable;

[0017] S7, rewriting the unconstrained nonlinear objective function represented by c and T′ to obtain the objective function to be optimized represented by q and τ;

[0018] The gradient and The objective function to be optimized is transmitted to the L-BFGS solver, and the quasi-Newton method is used to solve the objective function to be optimized, and the optimal trajectory x is output. * (t).

[0019] Preferably, the expression of the unconstrained nonlinear objective function is:

[0020]

[0021] Among them, λ is the weight vector.

[0022] Preferably,

[0023]

[0024] Among them, Q(T i ′) is an intermediate variable, Tr() represents the trace of the matrix, T is the transpose, c i The coefficient vector representing the i-th trajectory, β(t): = [1, t,…, t N ] T is the basis function, β (s) (t) is the s-order derivative of β(t), N is the highest order of the M-segment trajectory and N=2s-1.

[0025] Preferably,

[0026] Preferably,

[0027] Among them, k i represents the number of sampling points of the i-th trajectory, Indicates the trajectory position corresponding to the jth sampling point of the i-th trajectory, g τ () represents the value of the continuity constraint corresponding to the trajectory position.

[0028] Preferably,

[0029] Among them, k i Indicates the number of sampling points in the i-th trajectory, d s is the safety distance threshold, E() is the distance between the flight system and the nearest obstacle, represents the trajectory position corresponding to the jth sampling point of the i-th trajectory, and ‖‖1 represents the norm.

[0030] Preferably, in S7, the expression of the objective function to be optimized represented by q and τ is:

[0031]

[0032] Among them, J c (c(q,T′(τ)), T′(τ)) is the energy cost function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ), c(q,T′(τ)) is the coefficient matrix expression after parameterization and unconstraint by MINCO, T′(τ) is the time vector represented by τ when time is unconstrained, J t (T′(τ)) is the time cost penalty function represented by T′(τ), J s (c(q,T′(τ)),T′(τ)) is the continuity constraint cost penalty function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ), J g (c(q,T′(τ)),T′(τ)) is the obstacle avoidance constraint cost penalty function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ).

[0033] Preferably,

[0034]

[0035]

[0036] Among them, Tr() represents the trace of the matrix, c i (q,T′(τ) represents the coefficient matrix of the i-th trajectory represented by q and τ, T is the transpose, Q(Ti ′(τ)) is the sum of Q(T i The time vector of the i-th trajectory in the ′) function is mapped to the time-unconstrained proxy variable τ and represented by τ;

[0037] T i ′ is the time vector of the flight system in the i-th trajectory, T i ′(τ) represents mapping the time vector of the i-th trajectory to the time-unconstrained variable τ, k i Indicates the number of sampling points in the i-th trajectory, d s is the safety distance threshold, E() is the distance between the flight system and the nearest obstacle, The time vector representing the i-th trajectory is mapped to the trajectory position corresponding to the j-th sampling point under the time unconstrained variable τ, and ||||1 represents the norm.

[0038] Preferably, in step S6, during the differential homeomorphism exchange process, the mapping relationship between T′ and τ is:

[0039]

[0040] Preferably, the gradient The implementation is:

[0041]

[0042] Among them, G a It represents the intermediate variable generated when solving the gradient of the a-th intermediate waypoint, a=0,1,2……M-1, e1 represents the unit matrix I 2s The vector of the first column in , T is the transpose;

[0043] Get the gradient Medium gradient The implementation is:

[0044]

[0045] in, Represents the function W on the time vector T′ i Solve the gradient, H is a banded matrix, c i Represents the coefficient vector of the i-th trajectory, G i Represents the intermediate variable generated when solving the gradient of the remaining i-th waypoint besides the first waypoint.

[0046] The beneficial effects brought by the present invention are:

[0047] The present invention proposes a remote control planning method for an overdriven flight system based on the MINCO (Minimum Control) trajectory. This method directly uses the waypoints and time of the trajectory to parameterize the trajectory, takes the system's energy consumption, trajectory stability, and obstacle avoidance characteristics as optimization targets, and uses the quasi-Newton method to accelerate convergence to the optimal solution, obtaining the optimal trajectory to ensure the tracking accuracy of the subsequent tracking process. Compared with traditional planning methods, the planning method based on the MINCO trajectory achieves trajectory dimension reduction parameterization while having more flexible trajectory characterization characteristics. The planning process is simple and time-saving, and can be effectively used in trajectory optimization algorithms under various mission requirements, and the degree of freedom of planning can be adjusted as needed. The specific advantages are:

[0048] In specific applications, the backstepping method can be used at the control level to design the upper-level controller, and then combined with the linear extended state observer to estimate and compensate for system disturbances and uncertainties, achieve gradual convergence of system errors, and accurately track the terminal trajectory of the teleoperation input.

[0049] The specific advantages of the present invention are:

[0050] 1) The teleoperation planning method for an overdriven flight system based on MINCO trajectory proposed in this invention processes the problem with linear complexity and decouples the sampling frequency from the control parameters, thus solving the problem efficiently and perfectly.

[0051] Specifically, time cost and continuity constraints are used as penalty functions combined with weights and applied to the objective function, so that the flight system can accurately track the trajectory of the teleoperation input end effector in the shortest time and energy-optimal state while satisfying the actuator constraints and controller constraints throughout the process.

[0052] 2) The present invention provides a safer remote control method for flight systems. By adding an obstacle avoidance cost function to the objective function, the flight system can avoid obstacles based on the operational intention and the actual situation. This method is particularly suitable for emergencies such as obstacles in the blind spot of human vision or moving obstacles approaching the flight system, thereby improving the flexibility and practicality of the operation.

[0053] 3) The MINCO trajectory-based overactuated flight system teleoperation planning method provided by this invention is expected to replace traditional teleoperation flight system methods to complete high-altitude missions, meet operational requirements in different scenarios, and improve the accuracy and safety of high-altitude operations of teleoperation flight systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 is a schematic diagram of the principle of the teleoperation planning method of the overdriven flight system based on the MINCO trajectory of the present invention; wherein, x p is the state quantity of the vector rotor drone, x a is the state quantity of the n-free manipulator, x is the state quantity of the flight system, R6 is the spatial domain of the UAV state dimension of 6, R 6+n is the space domain of the flight system state quantity dimension 6+n, R 12+n The dimension of the flight system control quantity is 12+n in the spatial domain;

[0055] Figure 2 The constructed function ψ μ (x) Shape changes of the function, derivative, and second-order derivative at different polishing factors μ. DETAILED DESCRIPTION

[0056] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0057] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0058] The purpose of the present invention is to solve the problems of reduced efficiency due to the non-uniqueness of the system kinematic inverse solution caused by redundant characteristics when remotely operating an overdriven vector rotor flight system, as well as the complex and time-consuming process of seeking the optimal solution of the many planned feasible solutions, and the inability to guarantee tracking accuracy. A remote operation planning method and device for an overdriven flight system based on MINCO (Minimum Control) trajectory is proposed to seek the optimal solution among multiple sets of redundant solutions and achieve dynamic obstacle avoidance.

[0059] The present invention selects a rotor UAV based on a dual-axis tilt vector thrust as a flight platform, and on this basis connects an n-degree-of-freedom mechanical arm in series to form an overdriven vector rotor flight system.

[0060] Specific implementation method 1. Figure 1 This embodiment describes a method for teleoperation planning of an overdriven flight system based on a MINCO trajectory. The method is implemented based on a teleoperation system, which includes ground equipment and a flight system. The ground equipment is used to teleoperate the flight system. The flight system is a vector rotor flight system, including a 6-degree-of-freedom UAV and an n-degree-of-freedom robotic arm. The method includes the following steps:

[0061] S1. The ground equipment is used to calculate the position of the end effector of the flight system based on the currently collected operator posture; wherein the end effector of the flight system is the end effector of the robotic arm;

[0062] Generate a trajectory ξ of the end effector with a dimension of 6 according to the pose of the end effector of the flight system;

[0063] S2. Differential flat transformation: The onboard processor of the UAV solves the trajectory ξ using the Jacobian matrix and performs planning in the null space of the flight system state. Based on the differential flatness property of the flight system, x(t), the flight system state variable over time t, is selected as the flat output trajectory. The dimension of the flight system state is n+6, and the UAV has n degrees of freedom.

[0064] S3, MINCO trajectory parameterization: The flat output trajectory x(t) is parameterized by MINCO trajectory parameterization to obtain trajectory x(q,T′); x(q,T′) is represented by the intermediate waypoint vector q and the time vector T′ between waypoints, and q and T′ are both MINCO trajectory parameters; q = (q1,…,q M-1 ) T , T'=(T'1,…,T' M ) T ,q a is the position information of the ath intermediate waypoint in the flight process except the first and last waypoints, T' i is the time information between the first and last waypoints corresponding to the i-th trajectory during the flight process, a=,1,2……M-1, i=1,2,3……M, M is the total number of segments;

[0065] In this process, the MINCO trajectory class is used to generate the minimum control trajectory corresponding to the unconstrained trajectory optimization subproblem. This type of trajectory only takes q and T' as parameters, and the flat output trajectory can be expressed as x(q,T'); in specific applications, given q = (q1, ..., q M-1 ) T ∈R (6+n)×(M-1) , R (6+n)×(M-1) Represents a space of dimension (6+n)×(M-1), T′=(T′1,…,T′ M ) T Every element in satisfies T i ′∈R ＞0 . R ＞0 Indicates that all time vectors should be greater than 0.

[0066] S4. Construct an unconstrained nonlinear objective function: Design the energy cost function J based on the time vector T' and the coefficient matrix c c (c,T′), continuity constraint cost penalty function J s (c,T′) and the obstacle avoidance constraint cost penalty function J g (c,T′), and J c (c,T′、J s (c,T′),J g(c, T′) and the total time cost penalty function J t The sum is minimized, and an unconstrained nonlinear objective function is constructed; c is the coefficient matrix of the trajectory x(q,T′);

[0067] S5, MINCO gradient conduction:

[0068] Solve the unconstrained nonlinear objective function J c (c,T′),J s (c,T′),J g (c,T′) and J t , respectively, with respect to the gradient of the coefficient matrix c and the gradient with respect to the time vector T′ J is a function variable, and J refers to J c (c,T′),J s (c,T′),J g (c,T′) and J t Any function in

[0069] The gradient Conducted to the MINCO trajectory parameter q, the gradient is obtained

[0070] The gradient Conducted to the MINCO trajectory parameter T', the gradient is obtained

[0071] W represents the function of the function variable J parameterized by the MINCO trajectory;

[0072] S6. Diffeomorphism Exchange: Gradient Conducted to the gradient τ is a time-unconstrained proxy variable;

[0073] S7, rewriting the unconstrained nonlinear objective function represented by c and T′ to obtain the objective function to be optimized represented by q and τ;

[0074] The gradient and The objective function to be optimized is transmitted to the L-BFGS solver, and the quasi-Newton method is used to solve the objective function to be optimized, and the optimal trajectory x is output. * (t).

[0075] In this embodiment, the main task of the overdriven vector rotor flight system during flight is to accurately track the trajectory of the teleoperation input flight system end effector. 6+n Represents the state dimension of the flight system, ξ∈R 6It represents the trajectory of the six-degree-of-freedom end effector input by teleoperation. The redundant characteristics of the flight system can enable the system to optimize the performance indicators required for the secondary task in the null space of the system while meeting the main task.

[0076] In trajectory optimization, considering the differential flatness of overdriven vector rotor flight systems, dynamic differential constraints can be strictly enforced by ensuring sufficient continuity of the flat output space trajectory. Because the system is overdriven, its output trajectory x(t) is flat. The flight system's control input u can be directly solved based on the transient information of x(t). In specific applications, constraints defined on the control and state can also be expressed on the flat output trajectory x(t). By leveraging the system's differential flatness, differential constraints can be easily satisfied during state trajectory generation.

[0077] In the parameterization process of specific application: for M-segment polynomial trajectories of (n+6) dimensions, where the highest order of the trajectory is N=2s-1, s is 3 in the present invention, indicating the construction of a 3rd-order minimum control problem.

[0078] In specific applications, c=(c1 T ,…,c M T ) T ∈R 2Ms×(6+n) , represents the coefficient matrix of the flat output trajectory x(t);

[0079] Furthermore, the expression of the unconstrained nonlinear objective function is given as:

[0080]

[0081] Among them, λ is a weight vector used to balance various cost functions.

[0082] Specifically, for the energy cost function J c (c,T′), which is implemented as follows:

[0083]

[0084] Among them, Q(T i ′) is an intermediate variable, Tr() represents the trace of the matrix, T is the transpose, c i Represents the coefficient vector of the i-th segment trajectory, i=1,2,3……M, M is the total number of segments, β(t):=

[0085] [1,t,…,t N ] T is the basis function, β (s) (t) is the s-order derivative of β(t), N is the highest order of the M-segment trajectory and N=2s-1.

[0086] In order to make the task meet the time constraint and speed up the task process, the total time is minimized through the cost function, and the penalty function J for the total time cost is given. t ,

[0087] In specific applications, the continuity constraint cost penalty function J is given s The specific expression of (c, T′) is:

[0088]

[0089] Among them, k i represents the number of sampling points of the i-th trajectory, Indicates the trajectory position corresponding to the jth sampling point of the i-th trajectory, g τ () represents the value of the continuity constraint corresponding to the trajectory position. max[·,0] k The function represents the combination of the element-wise maximum operation and the element-wise power function. Indicates the location The value of the corresponding continuity constraint.

[0090] The above obtained continuity constraint cost penalty function J s The reasoning process of the expression (c, T′) is as follows:

[0091] First, the continuity constraint g of control and state is described in mathematical language as:

[0092]

[0093] Among them, V imax 、A imax 、J imax 、U imax are the upper limits of the velocity, acceleration, and jerk of each state variable and the upper limit of the actuator constraint. The continuity constraint g requires that the corresponding inequality is satisfied at infinite moments on the trajectory. In order to improve the solution efficiency, Due to the properties of the trajectories, we can directly decouple the resolution of the constraints from the dimensionality of the optimization variables.

[0094] With the help of the constraint transformation method, the constraint violation function of g is directly integrated in the time domain to obtain the finite-dimensional constraints.

[0095] For the trajectory x(t), define

[0096]

[0097] where k∈R ＞0 , max[·,0] krepresents the composite operation of the element-wise maximum operation and the element-wise power function. Using the above integral, the continuous-time constraint g is equivalent to Easy to know, is a function of the trajectory parameters q and T′, which is selected as the penalty functional to impose the continuity constraint. When k = 1, is a non-smooth but exact penalty functional for max[·,0] 1 Use a smoothing approximation constructed as follows:

[0098]

[0099] Among them, μ is called the polishing factor.

[0100] and Figure 2 The constructed function ψ is given in μ (x) The shape of the function, derivative, and second-order derivative at different polishing factors μ. It can be seen that when μ approaches 0, ψ μ (x) converges to max(x,0).

[0101] to I g Numerical integration of [x]J s It can be calculated by summing discrete samples:

[0102]

[0103] k i It represents the number of sampling points of the i-th trajectory and directly controls the accuracy of the integration.

[0104] Furthermore, to generate collision-free trajectories, the Euclidean distance field is used to obtain the distance and gradient information from the flight system surface to the obstacle. It is stipulated that E(·) is the distance between the flight system and the nearest obstacle, d s is the safety distance threshold, then the obstacle avoidance constraint cost penalty function J for collision avoidance g (c,T′) is defined as:

[0105]

[0106] Among them, k i Indicates the number of sampling points in the i-th trajectory, d s is the safety distance threshold, e() is the distance between the flight system and the nearest obstacle, represents the trajectory position corresponding to the jth sampling point of the i-th trajectory, and || ||1 represents the norm.

[0107] In MINCO gradient conduction, the gradient is obtained The implementation is:

[0108]

[0109] Among them, G a It represents the intermediate variable generated when solving the gradient of the a-th intermediate waypoint, a=0,1,2……M-1, e1 represents the unit matrix I 2s The vector of the first column in , T is the transpose;

[0110] Get the gradient Medium gradient The implementation is:

[0111]

[0112] in, Represents the function W on the time vector T′ i Solve the gradient, H is a banded matrix, c i Represents the coefficient vector of the i-th trajectory, G i Represents the intermediate variable generated when solving the gradient of the remaining i-th waypoint besides the first waypoint.

[0113] During the differential homeomorphism exchange process, the vector T′ is not in the Euclidean space. It is restricted by the time manifold R>0. It is necessary to convert the constrained variable T′ into the unconstrained proxy variable τ and transfer the gradient to τ to reduce the density of the optimizer. M-1 ) represents R M-1 Any element in R M-1 is a vector space of dimension M-1 over the real number field, then for all mappings 1≤i≤M, C is given. ∞ The diffeomorphism exchange, C ∞ This transformation satisfies the smoothness property of the function, so the mapping relationship between T′ and τ is:

[0114]

[0115] By directly defining the domain R of τ M-1 By optimizing the cost function (i.e., energy cost function) and penalty functions (i.e., continuity constraint cost penalty function, obstacle avoidance constraint cost penalty function, and total time cost penalty function), simple manifold constraints on the time vector can be implicitly satisfied. Furthermore, T′(τ) does not introduce new local minima in the space of τ.

[0116] Furthermore, in S7, the expression of the objective function to be optimized represented by q and τ is:

[0117]

[0118] Among them, J c(c(q,T′(τ)), T′(τ)) is the energy cost function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ), c(q,T′(τ)) is the coefficient matrix expression after parameterization and unconstraint by MINCO, T′(τ) is the time vector represented by τ when time is unconstrained, J t (T′(τ)) is the time cost penalty function represented by T′(τ), J s (c(q,T′(τ)),T′(τ)) is the continuity constraint cost penalty function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ), J g (c(q,T′(τ)),T′(τ)) is the obstacle avoidance constraint cost penalty function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ).

[0119] It also specifically gives:

[0120]

[0121] Among them, Tr() represents the trace of the matrix, c i (q,T′(τ) represents the coefficient matrix of the i-th trajectory represented by q and τ, T is the transpose, Q(T i ′(τ)) is the sum of Q(T i The time vector of the i-th trajectory in the ′) function is mapped to the time-unconstrained proxy variable τ and represented by τ;

[0122] T i ′ is the time vector of the flight system in the i-th trajectory, T i ′(τ) represents mapping the time vector of the i-th trajectory to the time-unconstrained variable τ, k i Indicates the number of sampling points in the i-th trajectory, d s is the safety distance threshold, E() is the distance between the flight system and the nearest obstacle, The time vector representing the i-th trajectory is mapped to the trajectory position corresponding to the j-th sampling point under the time unconstrained variable τ, and ||||1 represents the norm.

[0123] In this preferred embodiment, the lightweight unconstrained objective function to be optimized and its gradients with respect to the unconstrained variables are passed into the L-BFGS solver, and the efficient quasi-Newton method is used to solve the unconstrained optimization using the second-order information of the optimization problem, and the optimal trajectory x is output. * (t) serves as the reference input of the trajectory tracking controller, and generates corresponding control signals through control allocation to control the flight system and achieve precise tracking teleoperation.

[0124] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely illustrative of the principles and applications of the invention. It should be understood that many modifications may be made to the illustrative embodiments, and that other arrangements may be devised, without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in ways other than those described in the original claims. It should also be understood that features described in conjunction with individual embodiments may be employed in conjunction with other described embodiments.

Claims

1. A teleoperation planning method for an overdriven flight system based on MINCO trajectory, characterized in that: The method is implemented based on a teleoperation system, which includes ground equipment and a flight system. The ground equipment is used to remotely operate the flight system, which is a vector rotor flight system including a 6-degree-of-freedom UAV and an n-degree-of-freedom robotic arm. The method includes the following steps: S1. The ground equipment is used to calculate the position of the end effector of the flight system based on the currently collected operator posture; wherein the end effector of the flight system is the end effector of the robotic arm; Generate a trajectory ξ of the end effector with a dimension of 6 according to the pose of the end effector of the flight system; S2. Differential flat transformation: The onboard processor of the UAV solves the trajectory ξ using the Jacobian matrix and plans it in the null space of the flight system state. Based on the differential flatness property of the flight system, the flight system state variable x(t) is selected as the flat output trajectory with the dimension of the flight system state variable being n+6. S3, MINCO trajectory parameterization: The flat output trajectory x(t) is parameterized by MINCO trajectory parameterization to obtain trajectory x(q,T′); x(q,T′) is represented by the intermediate waypoint vector q and the time vector T′ between waypoints, and q and T′ are both MINCO trajectory parameters; q = (q1,…,q M-1 ) T , T′=(T′1,…,T′ M ) T ,q a is the position information of the ath intermediate waypoint in the flight process except the first and last waypoints, T′ i is the time vector between the first and last waypoints corresponding to the i-th trajectory during the flight process, a=,1,2……M-1, i=1,2,3……M, M is the total number of segments; S4. Construct an unconstrained nonlinear objective function: Design the energy cost function J based on the time vector T' and the coefficient matrix c c (c,T′), continuity constraint cost penalty function J s (c,T′) and the obstacle avoidance constraint cost penalty function J g (c,T′), and J c (c,T′、J s (c,T′),J g (c, T′) and the total time cost penalty function J t The sum is minimized, and an unconstrained nonlinear objective function is constructed; c is the coefficient matrix of the trajectory x(q,T′); S5, MINCO gradient conduction: Solve the unconstrained nonlinear objective function J c (c,T′),J s (c,T′),J g (c,T′) and J t , respectively, with respect to the gradient of the coefficient matrix c and the gradient with respect to the time vector T′ J is a function variable, and J refers to J c (c,T′),J s (c,T′),J g (c,T′) and J t Any function in The gradient Conducted to the MINCO trajectory parameter q, the gradient is obtained The gradient Conducted to the MINCO trajectory parameter T', the gradient is obtained W represents the function of the function variable J parameterized by the MINCO trajectory; S6. Diffeomorphism Exchange: Gradient Conducted to the gradient τ is a time-unconstrained proxy variable; S7, rewriting the unconstrained nonlinear objective function represented by c and T′ to obtain the objective function to be optimized represented by q and τ; The gradient and The objective function to be optimized is transmitted to the L-BFGS solver, and the quasi-Newton method is used to solve the objective function to be optimized, and the optimal trajectory x is output. * (t).

2. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 1, characterized in that: The expression of the unconstrained nonlinear objective function is: Among them, λ is the weight vector.

3. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 1, characterized in that: Among them, Q(T′ i ) is an intermediate variable, Tr() represents the trace of the matrix, T is the transpose, c i The coefficient vector representing the i-th trajectory, β(t): = [1, t,…, t N ] T is the basis function, β (s) (t) is the s-order derivative of β(t), N is the highest order of the M-segment trajectory and N=2s-1.

4. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 1, characterized in that:

5. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 1, characterized in that: Among them, k I represents the number of sampling points of the i-th trajectory, Indicates the trajectory position corresponding to the jth sampling point of the i-th trajectory, g τ () represents the value of the continuity constraint corresponding to the trajectory position.

6. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 1, characterized in that: Among them, k i Indicates the number of sampling points in the i-th trajectory, d s is the safety distance threshold, E() is the distance between the flight system and the nearest obstacle, It represents the trajectory position corresponding to the jth sampling point of the i-th trajectory, and ||||1 represents the norm.

7. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 1, characterized in that: In S7, the expression of the objective function to be optimized represented by q and τ is: Among them, J c (c(q,T′(τ)), T′(τ)) is the energy cost function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ), c(q,T′(τ)) is the coefficient matrix expression after parameterization and unconstraint by MINCO, T′(τ) is the time vector represented by τ when time is unconstrained, J t (T′(τ)) is the time cost penalty function represented by T′(τ), J s (c(q,T′(τ)),T′(τ)) is the continuity constraint cost penalty function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ), J g (c(q,T′(τ)),T′(τ)) is the obstacle avoidance constraint cost penalty function represented by the coefficient matrix c(q,T′(τ)) and the time vector T′(τ).

8. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 7, characterized in that: Among them, Tr() represents the trace of the matrix, c i (q,T′(τ) represents the coefficient matrix of the i-th trajectory represented by q and τ, T is the transpose, Q(T′ i (τ)) is the value of Q(T′ i ) function maps the time vector of the i-th trajectory to the time-unconstrained proxy variable τ, and uses τ to represent the intermediate function; T′ i is the time vector of the flight system in the i-th trajectory, T′ i (τ) represents mapping the time vector of the i-th trajectory to the time-unconstrained variable τ, k i Indicates the number of sampling points in the i-th trajectory, d s is the safety distance threshold, E() is the distance between the flight system and the nearest obstacle, The time vector representing the i-th trajectory is mapped to the trajectory position corresponding to the j-th sampling point under the time unconstrained variable τ, and || ||1 represents the norm.

9. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 1, characterized in that: Step S6: During the differential homeomorphism exchange process, the mapping relationship between T′ and τ is:

10. The teleoperation planning method for an overdriven flight system based on MINCO trajectory according to claim 1, characterized in that: Get the gradient The implementation is: Among them, G a It represents the intermediate variable generated when solving the gradient of the a-th intermediate waypoint, a=0,1,2……M-1, e1 represents the unit matrix I 2s The vector of the first column in , T is the transpose; Get the gradient Medium gradient The implementation is: in, Represents the function W on the time vector T′ i Solve the gradient, H is a banded matrix, c i Represents the coefficient vector of the i-th trajectory, G i Represents the intermediate variable generated when solving the gradient of the remaining i-th waypoint besides the first waypoint.

Images