Space non-cooperative target optimal envelope trajectory tracking control method under multi-target constraint

Abstract

The invention discloses a space non-cooperative target optimal envelope trajectory tracking control method under multi-target constraint, and the method mainly comprises the following steps: designing an all-drive multi-finger space capture configuration with adaptive configuration adjustment and anti-escape capabilities, an effective multi-finger envelope configuration is generated by adopting a bidirectional Hausdorff distance based on finger features and target edge point features, and a multi-finger dynamic capture domain is constructed according to target motion features and joint feature lengths; performing space expected trajectory parameterization by adopting a continuous high-order Bezier curve, and planning a smooth envelope trajectory under minimum base disturbance based on multi-target constraint and a multi-modal particle collaborative learning mechanism; and designing a multi-fingered collaborative hierarchical tubular model predictive control strategy based on a disturbance observer through the multi-fingered nonlinear coupling characteristic of an inverse dynamic compensation system to realize envelope trajectory tracking control. According to the method, interference caused by shape complexity, motion uncertainty and nonlinear dynamic coupling of the target can be effectively overcome, and accurate tracking control over the space non-cooperative target trajectory is achieved.

Description

Technical Field

[0001] The present invention relates to the field of space target capture control, and in particular to an optimal envelope trajectory tracking control method for space non-cooperative targets under multi-target constraints. Background Art

[0002] With the growing demand for space exploration and activities, the number of successfully launched satellites around the world has increased year by year. Due to the lack of effective means to recover failed satellites, the number of non-cooperative targets in Earth orbit has increased significantly. At the same time, the targets usually lack their own power control and communication capabilities and cannot effectively interact with the ground. Their on-orbit disintegration and collision further exacerbate the complexity of the space environment, posing a serious threat to the safety of on-orbit spacecraft, space stations and astronauts.

[0003] Compared with non-cooperative target capture methods such as docking recovery, flying net capture, rope claw traction, and laser propulsion, the rigid capture method based on the manipulator and the end mechanism has the advantages of high connection stiffness, reusability, and precise force control, and has been widely studied and applied in the industry. However, in the actual on-orbit capture of non-cooperative targets, affected by external disturbance forces such as the earth's gravity, air resistance, and solar radiation pressure, the targets usually have large size spans, diverse shapes, and motion uncertainty, accompanied by rolling, spinning, or nutating motion; at the same time, the multi-finger system will face constraints such as bounded actuator output, target motion boundaries, and joint speed limits during trajectory tracking. The complex spatial kinematic relationship and nonlinear dynamic characteristics lead to dynamic coupling of the system, resulting in poor trajectory tracking control effects of existing methods in the capture of non-cooperative targets in space.

[0004] In response to the above problems, the present invention focuses on the design of all-wheel drive multi-finger capture configuration, dynamic capture domain positioning under escape constraints, minimum base disturbance envelope trajectory planning and disturbance compensation multi-finger coordinated control strategy design, and discloses an optimal envelope trajectory tracking control method for spatial non-cooperative targets under multi-target constraints. Summary of the invention

[0005] In view of the problems and challenges existing in the trajectory tracking control of the above-mentioned space non-cooperative target, the present invention provides an optimal envelope trajectory tracking control method for a space non-cooperative target under multi-objective constraints. The technical scheme and steps of the present invention are as follows:

[0006] Step 1: Considering the target's overall dimensions, mass inertia and motion characteristics, a full-drive multi-finger spatial capture configuration with adaptive configuration adjustment and anti-escape capabilities is constructed, and a multi-finger envelope capture coupling dynamics model based on the Lagrange method and the assumed modal method is established;

[0007] Step 2: Generate candidate multi-finger configurations based on the shape characteristics and joint thresholds of the target, use the bidirectional Hausdorff distance to select valid multi-finger envelope configurations similar to the target contour, and construct a multi-finger dynamic capture domain based on the target motion characteristics and joint characteristic lengths, as follows:

[0008] (1) Determine the near joint angle of the multi-finger mechanism according to the shape characteristics of the rolling target, and generate a series of multi-finger candidate configurations of finger joints using joint thresholds and sampling steps;

[0009] (2) The position coordinates of the finger joints, fingertips and connecting rod center points are used as feature point sequences, and the bidirectional Hausdorff distance is calculated based on the finger feature set and the target edge feature point set;

[0010] (3) According to the projection relationship between the multi-finger end effector and the bottom plane of the palm, the constraint conditions for the inter-finger escape of the rolling target are established, and the configuration with the smallest sum of the motion ranges of the finger joints is selected as the optimal configuration of the multi-finger mechanism.

[0011] Step 3: Determine the continuous high-order Bezier curve through time interval mapping, integrate multi-objective constraints to establish the optimal trajectory optimization cost function, and use the particle collaborative learning mechanism to solve the multi-finger smooth envelope trajectory that minimizes the base disturbance, as follows:

[0012] (1) The trajectory planning time interval is mapped to the standard interval by normalization, and a fifth-order Bezier curve of the desired trajectory in the joint space is established according to the starting point, end point, and shape control point;

[0013] (2) The repulsive potential field function is constructed based on the minimum distance between the links to avoid collision during the enveloping process. The multi-objective optimization cost function of the optimal joint trajectory is constructed by integrating the motion restrictions of the multi-finger joints, the disturbance of the palm posture, and the constraints of the multi-finger link self-collision avoidance.

[0014] (3) A dynamic collaborative learning mechanism is designed based on fitness, search progress and particle distance. The speed update strategy of different regions is adjusted by integrating multimodal characteristics to establish an update mechanism for the enhanced particle swarm optimization algorithm.

[0015] Step 4: Separate the dynamic equation of a single finger according to the distributed dynamic equation of the multi-finger system and the coupling relationship between fingers, decouple the nonlinear tracking error system into linear subsystems based on inverse dynamics, and design a multi-finger collaborative hierarchical tubular model predictive control strategy to achieve precise tracking control of the multi-finger envelope trajectory, as follows:

[0016] (1) The linearized feedforward torque is used to represent the inertial coupling and velocity coupling between fingers, and the dynamic equation of a single finger is separated according to the distributed dynamic equation of the multi-finger system and the coupling relationship between fingers;

[0017] (2) Based on the coupling torque and inverse dynamics control to compensate for the nonlinearity and coupling effects of the system, the multi-input and multi-output nonlinear tracking error system is reduced to a single-input and single-output linear system;

[0018] (3) Design a disturbance observation controller to estimate the uncertainty in the system and actively compensate for external disturbances based on the disturbance observations;

[0019] (4) A multi-finger collaborative optimization cost function is established based on the energy, state tracking error, terminal state constraint and single-finger inertial force change of the nominal model in the prediction time domain. The tube model predictive controller is designed based on the deviation feedback between the optimal control input trajectory and the actual state trajectory to suppress residual interference.

[0020] The advantages of the present invention are:

[0021] 1. The present invention designs a dynamic capture domain positioning method that integrates escape constraints and bidirectional Hausdorff distance, which can adaptively determine the optimal multi-finger envelope configuration.

[0022] 2. The present invention designs a trajectory planning method based on continuous high-order Bezier curves and particle collaborative learning, which can reasonably plan multi-finger envelope trajectories under minimal disturbance of the base.

[0023] 3. The present invention proposes a multi-finger collaborative hierarchical tubular model predictive control method with disturbance observer compensation, which can realize multi-degree-of-freedom finger collaborative enveloping tasks under nonlinear dynamic coupling. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 It is a schematic diagram of the overall process in the specific implementation of the present invention;

[0025] Figure 2 It is a flow chart of the dynamic capture domain positioning method in the specific implementation of the present invention;

[0026] Figure 3 It is a flow chart of the trajectory planning method in the specific implementation of the present invention;

[0027] Figure 4 It is a flow chart of the multi-finger coordinated control method in the specific implementation of the present invention;

[0028] Figure 5 The following is an abstract of the present invention. Implementation

[0030] To make the purpose, technical solution and advantages of the present invention clearer, the embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings. The following embodiments are only used to illustrate the present invention and cannot be used to limit the scope of the present invention.

[0031] The optimal envelope trajectory tracking control method for spatial non-cooperative targets under multi-objective constraints, such as Figure 1 As shown, the specific steps are as follows:

[0032] S1. Based on the human hand gesture analysis method, a full-drive 16-DOF spatial capture mechanism with adaptive configuration adjustment and anti-escape capability is designed. The mechanism adopts a four-finger configuration, each finger has independent movement capability, each finger has four degrees of freedom and the near joint has two degrees of freedom. The four fingers are symmetrically distributed in a cross to simulate the embracing grasping gesture. When grasping irregular objects, multi-point contact can be achieved through kinematic adjustment, which can ensure the grasping stability and adaptability to the target shape, and solve the problems of small tolerance range, poor shape adaptability and excessive contact collision force during operation of the end effector;

[0033] By comprehensively utilizing Lagrange and assumed modal methods, the multi-finger envelope proximity coupling dynamic equation is established:

[0034]

[0035] Among them, H p and H f represents the inertia matrix of the palm and fingers, H pf represents the coupling matrix between palm and fingers, J p and J f The Jacobian matrix of the palm and fingers, c p and c f represents the nonlinear term related to the palm and finger velocities, c pf represents the coupling matrix related to palm and finger velocities, F p represents the force and torque acting on the palm, θ represents the joint angle, represents the palm speed, τ f represents the finger joint torque vector, σ e represents the unknown disturbance force and disturbance moment.

[0036] S2. Generate multi-finger candidate configurations based on the target's shape characteristics and joint thresholds, use the bidirectional Hausdorff distance to select valid multi-finger envelope configurations similar to the target contour, and construct a multi-finger dynamic capture domain based on the target's motion characteristics and joint characteristic lengths. Figure 2 , as follows:

[0037] (1) Determine the near joint angle of the multi-finger mechanism according to the shape characteristics of the rolling target, and generate a series of multi-finger candidate configurations of finger joints using joint thresholds and sampling steps;

[0038] (2) The position coordinates of the finger joints, fingertips and connecting rod center points are used as feature point sequences, and the bidirectional Hausdorff distance is calculated based on the finger feature set and the target edge feature point set:

[0039]

[0040] in, represents the feature set of the kth finger, t e represents the target edge feature point set, and the function max(·) represents the maximum value. and Both represent one-way Hausdorff distance:

[0041]

[0042] Among them, a i Representing a collection Point b j Represents the set t e Point in, d(a i ,b j ) represents point a i With b j The Euclidean distance between

[0043] (3) Based on the projection relationship between the multi-finger end effector and the bottom plane of the palm, the constraint conditions for the inter-finger escape of the rolling target are established:

[0044]

[0045] in, represents the finger proximal joint angle that satisfies the constraint conditions, Represents the initial multi-finger configuration projection and vertical The angle between Indicates joints and The distance between Indicates joints and The distance between the fingers is 1 / 4, and the configuration with the smallest sum of the motion ranges of the finger joints is selected as the optimal configuration of the multi-finger mechanism:

[0046]

[0047] in, represents the optimal registration of multiple fingers, represents a multi-finger candidate configuration, Δθ i represents the range of motion of the i-th joint.

[0048] S3. Use time interval mapping to establish a continuous high-order Bezier curve, establish the optimal trajectory optimization cost function that integrates joint motion restrictions, palm posture disturbances, and link self-collision avoidance constraints, and solve the multi-finger smooth envelope trajectory that minimizes base disturbances based on the particle collaborative learning mechanism under multimodal characteristics. Please refer to Figure 3 , as follows:

[0049] (1) The trajectory planning time interval is mapped to the standard interval by normalization, and the fifth-order Bezier curve of the desired trajectory in the joint space is established according to the starting point, end point, and shape control point:

[0050]

[0051] Among them, θ i (t) represents the joint angle, p i,j represents the control point of the constructed curve, t represents the current time, and t 0 and t f Indicates the starting and ending points of the trajectory planning time interval;

[0052] (2) The link position relationship between any two fingers during the enveloping process is expressed as a spatial geometric vector, and the repulsive potential field function is constructed based on the minimum distance between the links to avoid collision:

[0053]

[0054] Among them, f re represents the virtual repulsion, k r represents the repulsion coefficient, d safe Represents the effective range of repulsion, d min represents the minimum distance between connecting rods, κ is a constant;

[0055] Considering the motion restrictions of multi-finger joints, the disturbance of palm posture and the constraints of multi-finger linkage self-collision avoidance, the multi-objective optimization cost function of the optimal joint trajectory is constructed:

[0056]

[0057] Where θ(t) represents the joint angle, λ t represents the weight matrix of the multi-finger terminal, λ p represents the penalty factor for palm posture disturbance, Indicates the terminal position of multiple fingers, represents the drift disturbance of the palm posture, ||·|| represents the Euclidean norm;

[0058] (3) Design a dynamic collaborative learning mechanism based on fitness, search progress, and particle distance, integrate multimodal characteristics to adjust the speed update strategy of different regions, and design an update mechanism to enhance the particle swarm optimization algorithm:

[0059]

[0060] in, represents the non-negative inertia weight, x k (s) and v k(s) represent the position and velocity of particle k at the sth iteration, p k (s) represents the individual optimal position of particle k, g k (s) represents the optimal position of particle k for collaborative learning, Δv k (s) represents the speed adjustment of particles based on multimodal recognition, and γ(s) represents the dynamic synergy weight:

[0061]

[0062] Among them, Γ k (s) and Γ g (s) represents the fitness of particle k and the global optimal solution, S max represents the maximum number of iterations, α represents the distance factor, and d k Represents the distance between particle k and the global optimal particle.

[0063] S4. Separate the dynamic equations of a single finger according to the distributed dynamic equations of the multi-finger system and the coupling relationship between fingers, decouple the nonlinear tracking error system into linear subsystems based on inverse dynamics, and design a multi-finger cooperative hierarchical tubular model predictive control strategy based on disturbance observer compensation to achieve multi-finger envelope trajectory tracking control. Figure 4 , as follows:

[0064] (1) The linearized feedforward torque is used to represent the inertial coupling and velocity coupling between fingers, and the dynamic equation of a single finger is separated according to the distributed dynamic equation of the multi-finger system and the coupling relationship between fingers:

[0065]

[0066] in, represents the inertia matrix of the finger, Representation and and The related nonlinear terms, represents external disturbance, τ (k) represents the control torque of the finger, represents the coupling term between fingers, Δw (k) represents the friction force of the joint;

[0067] (2) Linearize the coupling torque based on the symmetrical distribution of finger crossing and the coupling relationship between adjacent fingers:

[0068]

[0069] Among them, K kj The parameter matrix representing the stiffness, D kjThe parameter matrix representing the damping is used to reduce the nonlinear tracking error system with multiple inputs and multiple outputs to a linear system with single input and single output based on the inverse dynamics control to compensate for the nonlinearity and coupling effects of the system.

[0070] (3) Design a disturbance observation controller to estimate the uncertainty in the system and actively compensate for external disturbances based on the disturbance observations:

[0071]

[0072] Among them, η i represents auxiliary variables, represents the observer parameters, represents the perturbation observation of the system, z i,e (t) represents the actual state trajectory, represents the state matrix of the system, represents the input matrix of the system;

[0073] (4) The change of inertial force of a single finger is used to indirectly measure the coordination degree of the multi-finger system. The multi-finger coordination optimization cost function is established based on the energy, state tracking error and terminal state constraints of the nominal model in the prediction time domain:

[0074]

[0075] Among them, J sync (z i,e (t k )) represents an indicator for measuring multi-finger synergy. represents the energy, state tracking error and terminal state constraint cost function, u i,nmpc represents the nominal model predictive controller, Indicates time t τ Control input, represents the nominal state trajectory, Indicates time t τ In the prediction state, and represents the set of constraints corresponding to the nominal variables, A set of constraints representing terminal states;

[0076] The model predictive controller is designed to suppress residual disturbances based on the feedback of the optimal control input trajectory and its deviation from the actual state trajectory:

[0077]

[0078] Among them, u i,m (t) represents the control input of the tube model controller, represents the optimal control input trajectory, K i,a Represents feedback gain.

Claims

1. The optimal envelope trajectory tracking control method for spatial non-cooperative targets under multi-objective constraints is characterized by: The method comprises the following steps: S1. Design a fully-driven multi-finger spatial capture configuration with adaptive configuration adjustment and anti-escape capability, and establish a multi-finger envelope capture coupling dynamics model by comprehensively utilizing the Lagrange method and the assumed modal method; S2. Generate multi-finger candidate configurations based on the target's shape characteristics and joint thresholds, use bidirectional Hausdorff distance to select valid multi-finger envelope configurations similar to the target contour, and construct a multi-finger dynamic capture domain based on the target's motion characteristics and joint characteristic lengths; S3. Use time interval mapping to establish continuous high-order Bezier curves, design the optimal trajectory optimization cost function that integrates joint motion restrictions, palm posture disturbances, and link self-collision avoidance constraints, and solve the multi-finger smooth envelope trajectory that minimizes base disturbances based on the particle collaborative learning mechanism of multi-modal characteristics; S4. The dynamic equations of a single finger are separated according to the distributed dynamic equations of the multi-finger system and the coupling relationship between fingers. The nonlinear tracking error system is decoupled into linear subsystems based on inverse dynamics. A multi-finger collaborative hierarchical tubular model predictive control strategy based on disturbance observer compensation is designed to realize multi-finger envelope trajectory tracking control.

2. The optimal envelope trajectory tracking control method for spatial non-cooperative targets under multi-objective constraints according to claim 1 is characterized in that: The step S2 comprises: S201. Determine the near joint angle of the multi-finger mechanism according to the shape characteristics of the rolling target, and generate a series of multi-finger candidate configurations of finger joints using joint thresholds and sampling steps; S202. Using the position coordinates of the finger joints, fingertips and connecting rod center points as the feature point sequence, the bidirectional Hausdorff distance is calculated based on the finger feature set and the target edge feature point set: in, represents the feature set of the kth finger, t e represents the target edge feature point set, and the function max(·) represents the maximum value. and Both represent one-way Hausdorff distance; S203. According to the projection relationship between the multi-finger end effector and the bottom plane of the palm, the constraint conditions for the inter-finger escape of the rolling target are established: in, represents the finger proximal joint angle that satisfies the constraint conditions, Represents the initial multi-finger configuration projection and vertical The angle between Indicates joints and The distance between Indicates joints and The distance between the fingers is 1 / 4, and the configuration with the smallest sum of the motion ranges of the finger joints is selected as the optimal configuration of the multi-finger mechanism: in, represents the optimal registration of multiple fingers, represents a multi-finger candidate configuration, Δθ i represents the range of motion of the i-th joint, and N represents the number of finger joints.

3. The optimal envelope trajectory tracking control method for spatial non-cooperative targets under multi-objective constraints according to claim 1 is characterized in that: The step S3 comprises: S301. Mapping the trajectory planning time interval to the standard interval by normalization, and establishing a fifth-order Bezier curve of the desired trajectory in the joint space according to the starting point, end point and shape control point; S302. The connecting rod position relationship between any two fingers in the enveloping process is expressed as a spatial geometric vector, and a repulsive potential field function is constructed according to the minimum distance between connecting rods to avoid collision: Among them, f re represents the virtual repulsion, k r represents the repulsion coefficient, d safe Represents the effective range of repulsion, d min represents the minimum distance between connecting rods, κ is a constant; Considering the motion restrictions of multi-finger joints, the disturbance of palm posture and the constraints of multi-finger linkage self-collision avoidance, the multi-objective optimization cost function of the optimal joint trajectory is constructed: Where θ(t) represents the joint angle, λ t represents the weight matrix of the multi-finger terminal, λ p represents the penalty factor for palm posture disturbance, Indicates the terminal position of multiple fingers, represents the drift disturbance of the palm posture, ||·|| represents the Euclidean norm; S303. Design a dynamic collaborative learning mechanism based on fitness, search progress and particle distance, integrate multimodal characteristics to adjust the speed update strategy of different regions, and design an update mechanism to enhance the particle swarm optimization algorithm: in, represents the non-negative inertia weight, x k (s) and v k (s) represent the position and velocity of particle k at the sth iteration, p k (s) represents the individual optimal position of particle k, g k (s) represents the optimal position of particle k for collaborative learning, Δv k (s) represents the velocity adjustment of particles based on multimodal recognition, and γ(s) represents the dynamic synergy weight.

4. The optimal envelope trajectory tracking control method for spatial non-cooperative targets under multi-objective constraints according to claim 1 is characterized in that: The step S4 comprises: S401. Use linearized feedforward torque to represent the inertial coupling and velocity coupling between fingers, and separate the dynamic equation of a single finger according to the distributed dynamic equation of the multi-finger system and the coupling relationship between fingers: in, represents the inertia matrix of the finger, Representation and and The related nonlinear terms, represents external disturbance, τ (k) represents the control torque of the finger, represents the coupling term between fingers, Δw (k) represents the friction force of the joint; S402. Linearize the coupling torque according to the finger crossing symmetric distribution and the coupling relationship between adjacent fingers, and reduce the nonlinear tracking error system with multiple inputs and multiple outputs to a linear system with single input and single output based on the nonlinear and coupling effects of the inverse dynamics control compensation system; S403. Design a disturbance observation controller to estimate the uncertainty in the system and actively compensate for external disturbances based on the disturbance observations: Among them, η i represents auxiliary variables, represents the observer parameters, represents the perturbation observation of the system, z i,e (t) represents the actual state trajectory, A represents the state matrix of the system, and B represents the input matrix of the system; S404. The change of inertial force of a single finger is used to indirectly measure the coordination degree of the multi-finger system. The multi-finger coordination optimization cost function is established based on the energy, state tracking error and terminal state constraint of the nominal model in the prediction time domain. The model predictive controller is designed to suppress residual interference based on the feedback of the optimal control input trajectory and its deviation from the actual state trajectory: Among them, u i,m (t) represents the control input of the tube model controller, represents the optimal control input trajectory, K i,a is the feedback gain, represents the nominal state trajectory.

Images