CN111098299B - Method for compensating disturbance of space robot - Google Patents
Method for compensating disturbance of space robot Download PDFInfo
- Publication number
- CN111098299B CN111098299B CN201911241734.8A CN201911241734A CN111098299B CN 111098299 B CN111098299 B CN 111098299B CN 201911241734 A CN201911241734 A CN 201911241734A CN 111098299 B CN111098299 B CN 111098299B
- Authority
- CN
- China
- Prior art keywords
- platform
- moment
- formula
- rigid body
- disturbance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a method for compensating disturbance of a space robot, which comprises the following steps: s1, establishing an overall dynamics model of the space robot based on Kennel dynamics; s2, establishing a rigid body dynamics model of the platform according to the space robot overall dynamics model, and calculating the disturbance moment of the mechanical arm to the platform posture; and S3, calculating the feedforward compensation amount of the platform attitude control from the angular momentum layer, and realizing the compensation of the disturbance torque of the platform attitude. The method is based on the Keynen dynamics principle to respectively establish the space robot overall dynamics model and the dynamics model of the space robot platform, so that the disturbance moment of the mechanical arm to the platform attitude is quickly and accurately calculated, the platform attitude disturbance moment is effectively compensated, the method has the characteristics of less calculation amount and accurate calculation result, and meanwhile, a new thought is provided for on-satellite control.
Description
Technical Field
The invention relates to the field of space robots, in particular to a disturbance compensation method in a capture process of a space floating robot based on a Kennel dynamics principle.
Background
The spatial floating robot comprises a platform and mechanical arms, a large amount of research is conducted on mechanical arm modeling at home and abroad, various methods such as Lagrange, Newton-Euler, Kenn and the like are adopted, various factors are considered, however, for practical application, the mechanical arms are mostly used as disturbance to conduct anti-disturbance control on the platform, and integrated modeling is not conducted on the spatial floating robot to conduct disturbance compensation. And the international method for compensating the interference torque of the space floating robot is to establish a dynamic model of the space floating robot based on the Newton Euler principle, calculate the interference force/torque, and perform the compensation control of the interference torque by adopting a feedback mode, so that the calculation times are more, the simulation efficiency is very low, and the resource consumption of the controller is larger.
Disclosure of Invention
The invention provides a method for compensating disturbance of a space robot, which is characterized in that the space robot is integrally modeled based on the Kenn dynamics principle, the disturbance moment of a mechanical arm of the space robot on the platform attitude is calculated and obtained, and the disturbance moment of the platform attitude is subjected to feedforward compensation by utilizing an integrated dynamics model to calculate angular momentum.
In order to achieve the above object, the present invention provides a method for compensating disturbance of a space robot, comprising the following steps:
s1, establishing an overall dynamics model of the space robot based on Kennel dynamics;
s2, establishing a rigid body dynamics model of the platform according to the space robot overall dynamics model, and calculating the disturbance moment of the mechanical arm to the platform posture;
and S3, calculating the feedforward compensation amount of the platform attitude control from the angular momentum layer, and realizing the compensation of the disturbance torque of the platform attitude.
Preferably, the overall dynamic model of the space robot is as follows:
in the formula (I), the compound is shown in the specification,
in the formula, mjIs a single rigid body BjI is a mass unit matrix, Sj、JjIs a single rigid body BjOrigin O of coordinate systemjThe static moment and the inertia moment of the rotor,andrespectively represent a single rigid body BjAcceleration and angular acceleration of, omegajRepresents a rigid body BjThe angular velocity of (a) of (b),representing the first derivative of the control input quantity,pVjrepresenting a single rigid body BjThe speed of the deviation is controlled by the speed controller,pωjrepresenting a single rigid body BjThe speed of the deflection angle is set to be,representing a single rigid body BjOrigin O of coordinate systemjIn the form of a cross-product of the static moments,represents a rigid body BjForm of angular velocity cross product of fbAnd τbRespectively the main power and the main moment of the platform;
the driving moment of the platform is taken as the front 6 on the right side of the equal sign of the formula (11), and the disturbance moment of the robot arm to the platform attitude is taken as the right side of the equal sign of the formula (11) from the 7 th row.
Preferably, the rigid body dynamics model of the platform is:
in the formula (I), the compound is shown in the specification,respectively representing the projection change rate of the platform speed and the angular speed on the platform rigid body coordinate system, mbRepresenting platform quality, SbRepresenting the origin O of the platform coordinate systembThe static moment of (a) of (b),representing the origin O of the platform coordinate systembCross multiplication form of the static moment of (J)bRepresenting the origin O of the platform coordinate systembThe inertia of the rotor is reduced to zero,form of cross multiplication, V, representing the projection of a rigid body on a platform rigid body coordinate systembRepresenting the projection of the platform velocity on the platform rigid body coordinate system, frfAnd τrfThe difference between the main power and the disturbance power of the space robot platform and the difference between the main moment and the disturbance moment of the space robot platform are respectively; the right side of the equal sign of the formula (12) comprises the main moment of the platform and the disturbance moment of the mechanical arm to the attitude of the platform;
take "first 6 rows" of equation (11):
in the formula, MbAnd MbmRespectively an inertia matrix of the platform and an inertia matrix of the mechanical arm coupled with the platform,is the angular acceleration of the joints of the mechanical arm,is the generalized inertial force of the system;
subtracting the formula (13) from the formula (12) to obtain the disturbance torque of the mechanical arm to the platform attitude:
preferably, the angular momentum feedforward compensation quantity for attitude control of the space robot platform is as follows:
in the formula IkIs the moment of inertia of the kth arm, phikIs an array formed by joint angles of a kth mechanical arm, KffIs a proportionality coefficient, and the value range is more than 0 and less than Kff<1;
Increase arm angular momentum compensation circuit in platform attitude control system's control circuit, realize the compensation of arm to platform gesture disturbance moment, arm angular momentum compensation circuit is:
in the formula, hdTo command angular momentum, KmIs a symmetric positive definite matrix.
The invention has the following advantages:
the invention discloses a method for compensating the disturbance of a space robot, which is characterized in that an overall dynamics model of the space robot and a dynamics model of a space robot platform are respectively established based on the Kene dynamics principle, so that the disturbance torque of a mechanical arm to the platform attitude is rapidly and accurately calculated, feedforward supplementary control is established from the angular momentum level, and the feedforward compensation of the disturbance torque of the platform attitude is effectively realized. The invention has the characteristics of less calculation amount and accurate calculation result, and simultaneously provides a new idea for on-satellite control.
Drawings
FIG. 1 is a structural diagram of a space robot provided by the present invention;
fig. 2 is a flowchart of a method for compensating a disturbance torque of a space robot according to an embodiment of the present invention.
Detailed Description
The method for compensating the disturbance of the space robot provided by the invention is further described in detail by combining the figures and the specific embodiment. Advantages and features of the present invention will become apparent from the following description and from the claims. It is to be noted that the drawings are in a very simplified form and are all used in a non-precise ratio for the purpose of facilitating and distinctly aiding in the description of the embodiments of the invention.
As shown in fig. 1, the space robot includes a platform and a plurality of robot arms, and the robot arms may disturb the posture of the platform during movement to generate a disturbance torque.
As shown in fig. 2, the method for compensating for disturbance of a space robot disclosed by the present invention comprises the following steps:
s1, establishing an overall dynamics model of the space robot based on Kennel dynamics;
each rod piece of the space robot is equivalent to each single rigid body, and based on the Keen power principle, a single rigid body dynamic model is as follows:
in the formula, mjIs a single rigid body BjI is a mass unit matrix, Sj、JjIs a single rigid body BjOrigin O of coordinate systemjThe static moment and inertia of the physical quantity, the sign of the above physical quantity to indicate that the physical quantity and the following physical quantity carry out cross multiplication operation,andrespectively represent a single rigid body BjAcceleration and angular acceleration of, omegajRepresents a rigid body BjAngular velocity of fjTo act on BjPrincipal vector of the force system of (1), τjTo act on BjForce system ofjPrincipal moment of (f)j,τjIncluding all actions on BjThe effect of the force of (c).
Formula (1) left-hand multiplying velocity and angular velocity partial derivativesAnd single rigid body BjAcceleration and angular acceleration ofSubstituting the compound into the formula (1),
wherein v isjRepresents a rigid body BjThe speed of (2).
Summing the dynamic models of all the single rigid bodies of the space robot, the following can be obtained:
order to
Then, formula (3) is:
order to
Then
due to the fact thatFor active and/or constraining forces in the joints of the space robot and externally applied forces, namely:
when formula (6) is substituted into formula (4), the right side of the equal sign of formula (4) is:
the second term of the formula (7) is the contact force between the platform and the tail end of the mechanical arm (external force applied to the platform);
the first term of the formula (7) is active and/or constraint force in a space robot joint, the space robot joint refers to a part which can generate relative motion between two rod pieces of the space robot, and the first term of the formula (7) is expanded according to the joint:
due to the fact that
Then, the first term of equation (7) is simplified to:
The generalized main moment acting on the mechanical arm joint is TjthjWherein, TjIs the yaw rate, thjIs the joint active moment, then:
thjreduced to tjJ is 1,.. N, the main power of the robot arm is
Defining a virtual joint, wherein the virtual joint represents that six degrees of freedom exist between a platform and an inertia system of the space robot, namely displacement in three directions and rotation around the three directions, and the virtual joint has no force action, namely the space robot is not stressed except the platform, then:
pvb=[I 0…0]
pωb=[0 I 0…0]
Therefore, the main system power for introducing the virtual joint is
By combining the joint (equation (8)) and virtual joint (equation (9)) functions of the space robot, it is possible to obtain:
combining the formula (5) and the formula (10), the space robot overall dynamics model is established based on kahn dynamics:
because the disturbance force and the disturbance torque of the space robot are 6 dimensions in total, the main moment of the front 6 behavior platform on the right side of the equation (11) with the same sign is the disturbance force torque of the robot arm to the platform attitude on the right side of the equation (11) with the same sign from the 7 th row.
S2, establishing a rigid body dynamics model of the platform according to the space robot overall dynamics model, and calculating the disturbance moment of the mechanical arm to the platform posture;
acceleration of the platformSubstituting into formula (1), a rigid body dynamics model of the platform can be obtained:
in the formula (I), the compound is shown in the specification,respectively representing the change rate of the platform speed and the angular speed projected in the platform rigid coordinate system,and the acceleration of the platform is different,as with the angular acceleration of the space robot,namely:
the device comprises an active moment of the platform and a disturbance moment of the mechanical arm to the attitude of the platform.
The "first 6 rows" of equation (11) may be written as:
subtracting the formula (13) from the formula (12) to obtain the disturbance torque of the mechanical arm to the platform attitude:
and S3, calculating the feedforward compensation amount of the platform attitude control from the angular momentum layer, and realizing the compensation of the disturbance torque of the platform attitude.
In order to compensate the disturbance of the motion of the mechanical arm to the platform attitude, feedforward compensation control is established through the coordination relation of the angular momentum layer.
When the initial angular momentum of the space robot is zero and no external moment acts on the space robot, the angular momentum of the space robot is always zero:
in the formula, L0Angular momentum of the space robot to the platform centroid, I0Is the moment of inertia of the platform, omega0As angular velocity of the platform, IkIs the moment of inertia of the kth arm, phikFormed by joint angles of kth armAnd (3) an array.
Variation of angular momentum of space robot and platform control torque tau0And disturbance torque taudThe relationship of (1) is:
in order to derive the angular momentum of the platform disturbance torque to be compensated, if the platform keeps the attitude unchanged in the motion process of the mechanical arm and the initial angular momentum is zero, the following steps are provided:
equation (17) gives the compensation amount for the platform attitude control angular momentum feedforward compensation. In practice, the attitude of the platform cannot be completely kept still, and the attitude cannot be completely performed according to equation (17) during compensation, but should be properly reduced, so that the final feedforward compensation amount is:
in the formula, KffIs a proportionality coefficient, and the value range is more than 0 and less than Kff<1。
In order to enable a platform attitude control system in a space robot to receive an angular momentum instruction, a mechanical arm angular momentum compensation loop is added in a control loop of the platform attitude control system, so that compensation of a mechanical arm on a platform attitude disturbance torque is realized, and the mechanical arm angular momentum compensation loop is as follows:
in the formula, hdTo command angular momentum, KmIs a symmetric positive definite matrix.
The conventional method for calculating the disturbance torque of the space robot platform comprises the following steps: a dynamic model of the space robot is established based on a Newton Euler method, and the interference torque is calculated according to the Newton Euler dynamic model. Compared with the conventional method, the calculation method of the attitude disturbance torque of the space robot platform provided by the invention has the advantages that the calculation result is the same, the calculation amount is different, the method provided by the invention adopts 6 times of addition and 8 times of multiplication, and the conventional calculation method adopts 27 times of addition and 27 times of multiplication, so that the calculation method of the disturbance torque of the attitude of the robot platform provided by the invention has the characteristic of less calculation amount.
The invention discloses a method for disturbance compensation of a space robot, which is characterized in that an overall dynamics model of the space robot is established based on the Kenn dynamics principle, and a mechanical arm and a platform are described by adopting the same dynamics model, so that the disturbance torque of the mechanical arm on the attitude of the platform is quickly and accurately calculated, feedforward supplementary control is established from the aspect of angular momentum, and the feedforward compensation of the disturbance torque of the attitude of the platform is effectively realized. The invention has the characteristics of less calculation amount and accurate calculation result, and simultaneously provides a new idea for on-satellite control.
While the present invention has been described in detail with reference to the preferred embodiments, it should be understood that the above description should not be taken as limiting the invention. Various modifications and alterations to this invention will become apparent to those skilled in the art upon reading the foregoing description. Accordingly, the scope of the invention should be determined from the following claims.
Claims (3)
1. A method for compensating disturbance of a space robot is characterized by comprising the following steps:
s1, establishing an overall dynamics model of the space robot based on Kennel dynamics;
s2, establishing a rigid body dynamics model of the platform according to the space robot overall dynamics model, and calculating the disturbance moment of the mechanical arm to the platform posture;
s3, calculating a feedforward compensation quantity of the platform attitude control from the angular momentum layer to realize the compensation of the disturbance moment of the platform attitude;
the calculation formula of the feedforward compensation quantity of the platform attitude control is as follows:
in the formula IkIs the moment of inertia of the kth arm, phikIs an array formed by joint angles of a kth mechanical arm, KffIs proportional coefficient, t is time, and the value range is more than 0 and less than Kff<1;
Increase arm angular momentum compensation circuit in platform attitude control system's control circuit, realize the compensation of arm to platform gesture disturbance moment, arm angular momentum compensation circuit is:
in the formula, hdTo command angular momentum, KmIs a symmetric positive definite matrix.
2. The method of claim 1, wherein the global dynamics model of the space robot is:
in the formula (I), the compound is shown in the specification,
in the formula, mjIs a single rigid body BjI is a mass unit matrix, Sj、JjIs a single rigid body BjOrigin O of coordinate systemjThe static moment and the inertia moment of the rotor,andrespectively represent a single rigid body BjAcceleration and angular acceleration of, omegajRepresents a rigid body BjThe angular velocity of (a) of (b),representing the first derivative of the control input quantity,pVjrepresenting a single rigid body BjThe speed of the deviation is controlled by the speed controller,pωjrepresenting a single rigid body BjThe speed of the deflection angle is set to be,representing a single rigid body BjOrigin O of coordinate systemjIn the form of a cross-product of the static moments,represents a rigid body BjForm of angular velocity cross product of fbAnd τbRespectively the main power and the main moment of the platform;
the driving moment of the platform is taken as the front 6 on the right side of the equal sign of the formula (11), and the disturbance moment of the robot arm to the platform attitude is taken as the right side of the equal sign of the formula (11) from the 7 th row.
3. The method of claim 2, wherein the rigid body dynamics model of the platform is:
in the formula (I), the compound is shown in the specification,respectively representing platform speedsRate of change of degree and angular velocity projected in platform rigid coordinate system, mbRepresenting platform quality, SbRepresenting the origin O of the platform coordinate systembThe static moment of (a) of (b),representing the origin O of the platform coordinate systembCross multiplication form of the static moment of (J)bRepresenting the origin O of the platform coordinate systembThe inertia of the rotor is reduced to zero,form of cross multiplication, V, representing the projection of a rigid body on a platform rigid body coordinate systembRepresenting the projection of the platform velocity on the platform rigid body coordinate system, frfAnd τrfThe difference between the main power and the disturbance power of the space robot platform and the difference between the main moment and the disturbance moment of the space robot platform are respectively;
the right side of the equal sign of the formula (12) comprises the main moment of the space robot platform and the disturbance moment of the mechanical arm to the platform attitude;
take "first 6 rows" of equation (11):
in the formula, MbAnd MbmRespectively an inertia matrix of the platform and an inertia matrix of the mechanical arm coupled with the platform,is the angular acceleration of the joints of the mechanical arm,is the generalized inertial force of the system;
subtracting the formula (13) from the formula (12) to obtain the disturbance torque of the mechanical arm to the platform attitude:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911241734.8A CN111098299B (en) | 2019-12-06 | 2019-12-06 | Method for compensating disturbance of space robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911241734.8A CN111098299B (en) | 2019-12-06 | 2019-12-06 | Method for compensating disturbance of space robot |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111098299A CN111098299A (en) | 2020-05-05 |
CN111098299B true CN111098299B (en) | 2021-09-07 |
Family
ID=70422477
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911241734.8A Active CN111098299B (en) | 2019-12-06 | 2019-12-06 | Method for compensating disturbance of space robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111098299B (en) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114800496B (en) * | 2022-04-18 | 2023-11-03 | 中国科学院空间应用工程与技术中心 | Risk judgment method, system, medium and equipment for space robot contact operation |
CN115128960B (en) * | 2022-08-30 | 2022-12-16 | 齐鲁工业大学 | Method and system for controlling motion of biped robot based on deep reinforcement learning |
CN115556111B (en) * | 2022-10-26 | 2023-08-18 | 哈尔滨工业大学 | Flight mechanical arm coupling disturbance control method based on variable inertia parameter modeling |
CN117001675B (en) * | 2023-09-28 | 2024-05-31 | 江苏云幕智造科技有限公司 | Double-arm cooperative control non-cooperative target obstacle avoidance trajectory planning method |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05178295A (en) * | 1991-12-11 | 1993-07-20 | Ishikawajima Harima Heavy Ind Co Ltd | Space robot and cooperative control method thereof |
JPH06198585A (en) * | 1993-01-05 | 1994-07-19 | Ishikawajima Harima Heavy Ind Co Ltd | Attitude control method and apparatus for space robot |
CN101726296A (en) * | 2009-12-22 | 2010-06-09 | 哈尔滨工业大学 | Vision measurement, path planning and GNC integrated simulation system for space robot |
CN107703753A (en) * | 2017-10-25 | 2018-02-16 | 南京邮电大学 | A kind of fault tolerant control method of space manipulator |
CN107967241A (en) * | 2017-11-21 | 2018-04-27 | 中国运载火箭技术研究院 | A kind of space freely floats the pedestal disturbance computational methods of robot |
CN110532581A (en) * | 2019-05-14 | 2019-12-03 | 武汉弗雷德斯科技发展有限公司 | A kind of dynamic modeling method of four shaft mechanicals arm |
-
2019
- 2019-12-06 CN CN201911241734.8A patent/CN111098299B/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05178295A (en) * | 1991-12-11 | 1993-07-20 | Ishikawajima Harima Heavy Ind Co Ltd | Space robot and cooperative control method thereof |
JPH06198585A (en) * | 1993-01-05 | 1994-07-19 | Ishikawajima Harima Heavy Ind Co Ltd | Attitude control method and apparatus for space robot |
CN101726296A (en) * | 2009-12-22 | 2010-06-09 | 哈尔滨工业大学 | Vision measurement, path planning and GNC integrated simulation system for space robot |
CN107703753A (en) * | 2017-10-25 | 2018-02-16 | 南京邮电大学 | A kind of fault tolerant control method of space manipulator |
CN107967241A (en) * | 2017-11-21 | 2018-04-27 | 中国运载火箭技术研究院 | A kind of space freely floats the pedestal disturbance computational methods of robot |
CN110532581A (en) * | 2019-05-14 | 2019-12-03 | 武汉弗雷德斯科技发展有限公司 | A kind of dynamic modeling method of four shaft mechanicals arm |
Also Published As
Publication number | Publication date |
---|---|
CN111098299A (en) | 2020-05-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111098299B (en) | Method for compensating disturbance of space robot | |
WO2022252863A1 (en) | Control method and apparatus for wheel-legged robot, and wheel-legged robot and device | |
CN111558941B (en) | Floating base dynamics feedforward control method and device and multi-legged robot | |
Huang et al. | Adaptive control for space debris removal with uncertain kinematics, dynamics and states | |
CN106985139A (en) | Robot for space active disturbance rejection control method for coordinating with compensating is observed based on extended mode | |
Metni et al. | Visual tracking control of aerial robotic systems with adaptive depth estimation | |
CN113119100B (en) | Redundant mechanical arm control method and device, readable storage medium and equipment | |
Meng et al. | Vibration suppression control of free-floating space robots with flexible appendages for autonomous target capturing | |
CN108181914A (en) | A kind of neutral buoyancy robot pose and track Auto-disturbance-rejection Control | |
CN112558621A (en) | Decoupling control-based flying mechanical arm system | |
Park et al. | Pose and posture estimation of aerial skeleton systems for outdoor flying | |
CN111026160A (en) | Trajectory tracking control method for quad-rotor unmanned aerial vehicle | |
Asl | Robust vision-based tracking control of VTOL unmanned aerial vehicles | |
CN115556111B (en) | Flight mechanical arm coupling disturbance control method based on variable inertia parameter modeling | |
Hu et al. | Recursive decentralized control for robotic manipulators | |
Hu et al. | Singularity and steering logic for control moment gyros on flexible space structures | |
CN107894775B (en) | Under-actuated unmanned underwater vehicle track generation and control method | |
CN115122325A (en) | Robust visual servo control method for anthropomorphic manipulator with view field constraint | |
CN115480583A (en) | Visual servo tracking and impedance control method of flying operation robot | |
Salamat et al. | A swash mass pendulum with passivity-based control | |
Shi et al. | Modeling and simulation of space robot visual servoing for autonomous target capturing | |
De Magistris et al. | Optimized humanoid walking with soft soles | |
He et al. | Cdm-mpc: An integrated dynamic planning and control framework for bipedal robots jumping | |
Chen et al. | Integrable whole-body orientation coordinates for legged robots | |
Kim et al. | Dynamic behaviors on the NAO robot with closed-loop whole body operational space control |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |