A variable-parameter cooperative control method for a spaceborne manipulator to capture a target

Abstract

A kind of variable parameter collaborative control method of satellite borne mechanical arm capture task, camera fixed on satellite continuously outputs the relative pose of target, the joint control instruction of mechanical arm controller on satellite is calculated according to the current angle of each joint of mechanical arm installed on satellite, to realize the control of mechanical arm;At the same time, the attitude controller and position controller of six degree of freedom controller on satellite are responsible for the attitude control and position control of satellite respectively, the total torque instruction is output by attitude controller when attitude control;When position control, attitude controller outputs reaction torque to offset the disturbance of satellite attitude caused by mechanical arm movement, and the expected total thrust of each thruster on satellite is calculated by position controller according to the real-time updated center of mass position of satellite and mechanical arm combination of variable parameter dynamics identification module, and the control instruction of each thruster is decomposed from expected total thrust.This application realizes high-precision, high-continuity capture control.

Description

Technical Field

[0001] This invention relates to a control method for a spaceborne robotic arm, specifically a variable parameter cooperative control method for a spaceborne robotic arm grasping task, belonging to the field of space robot control technology. Background Technology

[0002] In space-based servicing missions, using satellites equipped with robotic arms to capture targets is a key technology for achieving tasks such as debris removal and on-orbit maintenance. Existing technologies typically employ cameras fixed to the satellite body to measure the target's attitude and guide the robotic arm's end effector to approach the target. Some existing technologies use fuzzy neural network models to input interactive torques and the desired minimum torque, outputting impedance control parameters and obtaining position corrections based on these parameters and interactive torques, achieving compliant control when the robotic arm's end effector contacts the target and avoiding excessive collisions. Other existing technologies construct recursive expressions for inertial forces and inertial torques based on outward and inward recursion, using optimization algorithms to solve for the angular acceleration of each link and calculate the joint driving torque, enabling control of the satellite base's attitude through robotic arm motion and reducing fuel consumption. Still other existing technologies utilize angular velocities measured by satellite base gyroscopes to compensate for the desired motion velocity of the robotic arm's end effector in tool coordinates, obtaining the angular velocities of each joint of the robotic arm through inverse kinematics calculations, achieving motion planning for the space robotic arm under a floating base. These existing technologies have the following main shortcomings: they do not consider the problem of visual measurement loss caused by camera occlusion during the movement of the robotic arm; they do not establish a differentiated motion compensation strategy based on visual state; they do not simultaneously handle the impact of satellite motion on the positioning of the robotic arm end effector and the reaction effect of the robotic arm motion on the satellite attitude; they do not consider the time-varying characteristics of the centroid and inertial tensor of the combined body caused by the movement of the robotic arm; and they do not fuse multi-source sensor data to solve the problem of insufficient accuracy of virtual pose estimation during occlusion. Summary of the Invention

[0003] To address the aforementioned problems, this invention proposes a variable-parameter collaborative control method for spaceborne robotic arm capture missions. This method solves the problems of visual guidance interruption, dynamic interference from the base, strong dynamic coupling, and large pose estimation errors during occlusion, thereby achieving high-precision and high-continuity capture control.

[0004] The technical means adopted by this invention to solve the above problems is: a variable parameter cooperative control method for spaceborne robotic arm capture missions, wherein a camera fixed on the satellite continuously outputs the relative pose of the target. The robotic arm controller on the satellite calculates the joint control command Q(t) based on the current angle Q(t-1) of each joint of the robotic arm installed on the satellite, thereby controlling the robotic arm. Where Q = (q1, q2, q3, ..., q m), where m is the number of joints on the robotic arm; Meanwhile, the attitude controller and position controller of the six-degree-of-freedom controller on the satellite are responsible for the satellite's attitude control and position control, respectively. During attitude control, the attitude controller outputs the total torque. instruction: In the formula: e q For the satellite attitude error, ω e For angular velocity error, K p and K d All are preset gain matrices, I total (t) represents the real-time inertia. It is the reaction torque; During position control, the position controller uses the variable parameter dynamic identification module to update the center of mass position r of the satellite and robotic arm combination in real time. cm (t), calculate the expected total thrust Fdes of each thruster on the satellite, and decompose the expected total thrust into the control commands f of each thruster. thruster .

[0005] Furthermore, In the formula: Suppose that the satellite is equipped with N thrusters, and the installation position of the j-th thruster (relative to the origin of the satellite's coordinate system) is P. j The unit vector of the thrust direction is u. j .

[0006] Furthermore, when the camera is not obstructed by the robotic arm, Visual measurements from the camera are used; when the camera is obstructed by the robotic arm, Using virtual relative pose recursive values When the camera recovers from being obstructed by the robotic arm, Using time-varying weighted fusion value T fusion (t) to avoid switching shocks.

[0007] Furthermore, In the formula: To determine the optimal estimated absolute pose of the satellite at the start of the occlusion, The last valid visual measurement value before occlusion. The satellite motion state estimator outputs the optimal estimated absolute pose of the satellite, and the satellite motion state estimator integrates multi-source sensor data from IMU, star sensor, and GPS.

[0008] Furthermore, In the formula: ω imu (t) and a imu (t) represents the angular velocity and acceleration measured by the IMU, respectively. star (t) represents the attitude measured by the star sensor, pgps (t) represents the location measured by GPS.

[0009] Furthermore, In the formula: λ(t) represents the interpolation operation on SE(3), and λ(t) is the fusion coefficient, which monotonically increases from 0 to 1 within a set smooth transition time.

[0010] Furthermore, In the formula: τ arm Let the joint torque vector of the robotic arm be... Q, , Let q1, q2, q3, ..., q2 be the joint angles, angular velocities, and angular accelerations of the robotic arm, respectively, and let Q = (q1, q2, q3, ..., q3). m ), , , where m is the number of joints in the robotic arm.

[0011] Furthermore, f thruster Through the following calculations The method yielded the following results: Solve , making , In the formula: assuming the satellite is equipped with N thrusters, p j Let u be the installation position of the j-th thruster. j f is the unit vector in the direction of thrust. j Let F be the output force of the j-th thruster. des This represents the expected total thrust.

[0012] Furthermore, In the formula: M total The total mass of the satellite; a des To employ the PD control law, based on the desired satellite position P... des With current position P sat The expected acceleration of error generation.

[0013] Furthermore, let the satellite position error be e. p e p =P des -P sat Speed ​​error e v for: The expected acceleration is: In the formula: and This is the preset position control gain matrix.

[0014] The beneficial effects of this invention are: This invention achieves automatic switching between visual guidance and occlusion estimation by establishing a visual state machine. It employs a differentiated motion compensation strategy (directly using visual measurements without additional compensation when vision is unobstructed, and recursively calculating the virtual relative pose using the fused absolute satellite pose when occlusion occurs). A variable-parameter dynamics identification module is constructed to update the combined body's center of mass and inertial tensor in real time and feed them back to the satellite's six-degree-of-freedom controller. Multi-sensor fusion technology is introduced to combine the high-frequency dynamic characteristics of IMU integration with the drift-free characteristics of absolute measurements to improve the accuracy of virtual relative pose estimation during occlusion. A reaction torque feedforward module is established to map the joint torques of the robotic arm into reaction torques on the satellite body and combine them with real-time inertia feedforward compensation to the satellite attitude controller. This solves the problems of visual guidance interruption, base dynamic interference, strong dynamic coupling, and large pose estimation errors during occlusion, achieving high-precision and high-continuity capture control. Attached Figure Description

[0015] Figure 1 This is a block diagram of the hardware and software structure of the satellite body in Example 1. Detailed Implementation

[0016] The present invention will be further described below with reference to the accompanying drawings. The drawings are for illustrative purposes only, representing schematic diagrams rather than actual physical objects, and should not be construed as limiting the scope of this patent. To better illustrate the embodiments of the present invention, some components in the drawings may be omitted, enlarged, or reduced, and do not represent the actual dimensions of the product. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings. Example 1

[0017] A variable parameter cooperative control method for spaceborne robotic arm grasping missions, such as Figure 1 As shown, the satellite body is equipped with the following hardware and software structures: Camera: Used to acquire the real-time pose of the target relative to the satellite's coordinate system. ; Robotic arm: It consists of n links, with adjacent links connected by joints, and a total of m joints. The end effector is used to grasp the target, and the base coordinate system of the robotic arm coincides with the coordinate system of the satellite body. Robotic arm controller: Used to generate joint control commands based on the target's pose relative to the satellite and the current joint angle of the robotic arm, drive the robotic arm to move, and together with the robotic arm, form the robotic arm subsystem; Six-degree-of-freedom controller: Equipped with a high-precision IMU, star sensor, GPS receiver, and other measurement sensors; Central Coordination Controller: Includes a variable parameter dynamics identification module, a vision state machine and switching module, a satellite motion state estimator, a motion compensation module, and a reaction feedforward module.

[0018] During the satellite's motion, the variable parameter dynamics identification module identifies in real time the changes in the dynamic parameters of the satellite and robotic arm assembly caused by the robotic arm's motion. Let the mass of the satellite body be m. s The position of the center of mass r s Inertial tensor I s The mass m of each link of the robotic arm i The position of the center of mass r j Inertial tensor I ci These are the basic parameters, which can be obtained from the satellite and robotic arm model files, or through dynamic identification. The robotic arm joint angle is Q, and the angular velocity is... Angular acceleration is Then the position of the centroid r of the composite is cm (t) and inertial tensor I total (t) can be represented by the following mapping relationship: ,in This module is an operator based on the kinematics and mass characteristics of a robotic arm. It takes the system's fundamental physical parameters and joint motion states as input and outputs the current position of the center of mass and the inertia tensor of the assembled body. The module will update the center of mass position r in real time. cm (t) and inertial tensor I total (t) is fed to the satellite's six-degree-of-freedom controller for dynamic adjustment of control parameters to compensate for changes in the combined dynamic characteristics caused by the robotic arm's movement. In the above equation: Q = (q1, q2, q3, ..., q...) m ), , .

[0019] The satellite motion state estimator integrates data from multiple sensors, including IMU, star sensor, and GPS, to estimate the satellite's absolute attitude, velocity, and angular velocity in real time. Let the angular velocity and acceleration measured by the IMU be ω0 and ω1, respectively. imu (t) and a imu (t), the attitude measured by the star sensor is q star (t), the GPS-measured position is p gps If (t), then the optimal estimate of the satellite's absolute pose can be expressed as: ,in This is a multi-sensor data fusion operator that takes historical data and current measurements from each sensor as input and outputs the optimal estimated absolute pose after fusion. This estimation combines the high-frequency dynamic characteristics of IMU integration with the advantages of absolute measurements (star sensor, GPS) without long-term drift, achieving higher accuracy than single-sensor measurements.

[0020] The capture process involves two parts: the movement of the robotic arm and the movement of the satellite itself, which will be explained separately below.

[0021] I. Robotic Arm Movement The vision state machine and switching module comprehensively determine the camera's occlusion status based on the spatial relationship between the robotic arm configuration and the camera's field of view, as well as the camera image analysis results, and output a status flag. (0 = no occlusion, 1 = occlusion), and manage the validity of visual data and historical caching. When the flag changes from 0 to 1, record the occlusion start time t0 and the last valid visual measurement value before occlusion. .

[0022] The motion compensation module outputs the target's pose relative to the satellite based on the camera's occlusion status. ; (a) When the camera is not obstructed, Visual measurements from the camera were used. (ii) When the camera is obstructed by the robotic arm, the visual data becomes invalid. The compensation mode is activated, and the optimal estimated absolute pose is used from the satellite motion state estimator. And the data stored at the start of the occlusion. and The virtual relative pose is recursively derived using the following formula: This virtual pose serves as a guidance command for the robotic arm's end effector and also as a relative navigation reference for the satellite's six-degree-of-freedom controller, maintaining the continuity of the capture process.

[0023] at this time, Using virtual relative pose recursive values .

[0024] (iii) When the camera recovers from being obstructed by the robotic arm, time-varying weighted fusion is used to avoid switching shock: ,in, The interpolation operation on SE(3) is represented by the quaternion spherical interpolation method for the attitude part and the linear interpolation method for the position part; λ(t) is the fusion coefficient, which monotonically increases from 0 to 1 within the set smooth transition time.

[0025] at this time, Using time-varying weighted fusion value T fusion (t). When λ(t) increases to 1, it becomes a visual measurement value.

[0026] II. Satellite Motion The motion of a satellite includes both attitude changes and position changes. Therefore, the six-degree-of-freedom satellite controller consists of two parts: an attitude controller and a position controller, which are responsible for the attitude stabilization and position control of the satellite, respectively.

[0027] (a) During attitude control, the satellite attitude controller outputs the following total torque command: , where e q For the satellite attitude error, ω e For angular velocity error, K p and K d All are preset gain matrices, which can be obtained through actual debugging. total (t) represents the real-time inertia. This is the reaction torque output by the attitude controller when the robotic arm moves.

[0028] (ii) During position control, the combined inertia tensor I is updated in real time. total (t) and the position of the centroid r cm (t) Real-time decoupling avoids additional perturbations to attitude caused by position control.

[0029] Let the desired position of the satellite be P. des The current position is P sat Position error e p e p =P des -P sat Speed ​​error e v for: The expected acceleration is: In the formula: and This is the preset position control gain matrix.

[0030] According to satellite mass Then the expected total thrust F des for: .

[0031] To eliminate the displacement of the centroid of the composite body by r cm The position control coupled with attitude disturbance caused by (t) necessitates that thruster allocation must consider the real-time center of mass position. Assume the satellite has N thrusters, and the installation position of the j-th thruster (relative to the origin of the satellite's coordinate system) is p. j The unit vector of the thrust direction is u. j The output force is f j The total thrust F and total torque τ generated by the thruster are respectively: , .

[0032] To avoid position control interfering with attitude, the total thrust F should be achieved. des Simultaneously, the resultant torque τ = 0. This thrust distribution problem can be described as: Solve , making , This optimization allocation problem can be solved in real time using methods such as pseudo-inverse and quadratic programming. The existence of a solution depends on the thruster configuration. The control commands f for each thruster... thruste for: ,in The thrust allocation operator takes the real-time centroid position, desired total thrust, and thruster geometry parameters as input and outputs the thrust output commands for each thruster. This allocation strategy ensures that position control does not generate additional attitude torque, achieving dynamic decoupling between position and attitude.

[0033] Furthermore, during satellite position control, the movement of the robotic arm generates a reaction torque on the satellite, causing disturbance. The reaction feedforward module calculates the reaction torque of the robotic arm's movement on the satellite in real time. Let the joint torque vector of the robotic arm be... The reaction torque acting on the satellite body can be expressed as the following mapping relationship: ,in This is an operator based on a robotic arm dynamics model. It takes joint torques and motion states as input and outputs the base reaction torque. This mapping comprehensively considers the coupling effects of inertial forces, Coriolis forces, centrifugal forces, and gravity on the base from each link of the robotic arm. The module outputs... As a feedforward quantity, it controls the satellite's attitude and is used to counteract the disturbances to the satellite's attitude caused by the movement of the robotic arm.

[0034] The above embodiments involve multiple operators, which are explained below. However, since the calculation methods of each operator are quite complex and not the focus of this invention, they are not specifically described in this application. This application can adopt existing methods. The explanation is as follows: Operator: Based on the known mass, center of mass, and inertia tensor parameters of each link of the satellite body and the robotic arm, and combined with the joint angles, the current center of mass position and inertia tensor of the assembly are calculated through forward kinematics and parallel axis theorem analysis under the satellite body system; Operators: Multi-sensor data fusion can be achieved through weighted averaging, Kalman filtering, Bayesian estimation, DS evidence theory, and neural networks, among other methods. Operators: The dynamics modeling of robotic arms can be achieved through the Lagrange method, the Newton-Euler method, the Kane equation method, the virtual work principle method, and the recursive dynamics modeling method; Operators: The algorithm for thrust allocation can be implemented through methods such as pseudo-inverse allocation, weighted pseudo-inverse allocation, constrained least squares, quadratic programming optimization, and geometric analytical allocation. Operator: First, establish the forward kinematics model of the robotic arm using forward kinematics algorithms, such as the DH parameter method, the improved DH method, and the exponential product POE method; then, solve the inverse kinematics relationship of the robotic arm using inverse kinematics algorithms, such as analytical solution method, Jacobi pseudo-inverse method, damped least squares method, and numerical iteration method.

[0035] The above embodiments are for illustrative purposes only and are not intended to limit the invention. Those skilled in the art can make various changes or modifications without departing from the spirit and scope of the invention. Therefore, all equivalent technical solutions should also fall within the protection scope of the invention, which should be defined by the claims.

Claims

1. A variable parameter cooperative control method for a spaceborne robotic arm grasping mission, characterized in that: The camera fixed to the satellite continuously outputs the relative pose of the target. The robotic arm controller on the satellite calculates the joint control command Q(t) based on the current angle Q(t-1) of each joint of the robotic arm installed on the satellite, thereby controlling the robotic arm. Where Q = (q1, q2, q3, ..., q m ), where m is the number of joints on the robotic arm; Meanwhile, the attitude controller and position controller of the six-degree-of-freedom controller on the satellite are responsible for the satellite's attitude control and position control, respectively. During attitude control, the attitude controller outputs the total torque. instruction: In the formula: e q For the satellite attitude error, ω e For angular velocity error, K p and K d All are preset gain matrices, I total (t) represents the real-time inertia. For reaction torque, It is the reaction torque; During position control, the position controller uses the variable parameter dynamic identification module to update the center of mass position r of the satellite and robotic arm combination in real time. cm (t), calculate the expected total thrust Fdes of each thruster on the satellite, and decompose the expected total thrust into the control commands f of each thruster. thruster .

2. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 1, characterized in that: In the formula: Suppose that the satellite is equipped with N thrusters, and the installation position of the j-th thruster (relative to the origin of the satellite's coordinate system) is P. j The unit vector of the thrust direction is u. j .

3. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 1, characterized in that: When the camera is not obstructed by the robotic arm Visual measurements from the camera are used; when the camera is obstructed by the robotic arm, Using virtual relative pose recursive values When the camera recovers from being obstructed by the robotic arm, Using time-varying weighted fusion value T fusion (t) to avoid switching shocks.

4. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 3, characterized in that: In the formula: To determine the optimal estimated absolute pose of the satellite at the start of the occlusion, The last valid visual measurement value before occlusion. The satellite motion state estimator outputs the optimal estimated absolute pose of the satellite, and the satellite motion state estimator integrates multi-source sensor data from IMU, star sensor, and GPS.

5. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 4, characterized in that: In the formula: ω imu (t) and a imu (t) represents the angular velocity and acceleration measured by the IMU, respectively. star (t) represents the attitude measured by the star sensor, p gps (t) represents the location measured by GPS.

6. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 3, characterized in that: In the formula: λ(t) represents the interpolation operation on SE(3), and λ(t) is the fusion coefficient, which monotonically increases from 0 to 1 within a set smooth transition time.

7. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 1, characterized in that: In the formula: τ arm Let the joint torque vector of the robotic arm be... Q, , Let q1, q2, q3, ..., q2 be the joint angles, angular velocities, and angular accelerations of the robotic arm, respectively, and let Q = (q1, q2, q3, ..., q3). m ), , where m is the number of joints in the robotic arm.

8. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 2, characterized in that: f thruster Through the following calculations The method yielded the following results: Solve , making , In the formula: assuming the satellite is equipped with N thrusters, p j Let u be the installation position of the j-th thruster. j f is the unit vector in the direction of thrust. j Let F be the output force of the j-th thruster. des This represents the expected total thrust.

9. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 8, characterized in that: In the formula: M total The total mass of the satellite; a des To employ the PD control law, based on the desired satellite position P... des With current position P sat The expected acceleration of error generation.

10. The variable parameter cooperative control method for spaceborne robotic arm capture missions as described in claim 9, characterized in that: Let the satellite position error be e p e p =P des -P sat Speed ​​error e v for: The expected acceleration is: In the formula: and This is the preset position control gain matrix.

Images