CN109623812A - Consider the mechanical arm method for planning track of spacecraft ontology attitude motion - Google Patents
Consider the mechanical arm method for planning track of spacecraft ontology attitude motion Download PDFInfo
- Publication number
- CN109623812A CN109623812A CN201811470615.5A CN201811470615A CN109623812A CN 109623812 A CN109623812 A CN 109623812A CN 201811470615 A CN201811470615 A CN 201811470615A CN 109623812 A CN109623812 A CN 109623812A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- attitude
- spacecraft
- motion
- planning
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 71
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 57
- 230000008859 change Effects 0.000 claims description 27
- 239000013598 vector Substances 0.000 claims description 25
- 239000012636 effector Substances 0.000 claims description 19
- 230000008569 process Effects 0.000 claims description 19
- 230000009466 transformation Effects 0.000 claims description 10
- 238000009499 grossing Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 230000000694 effects Effects 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 6
- 238000005259 measurement Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 2
- 101100460704 Aspergillus sp. (strain MF297-2) notI gene Proteins 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
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/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The mechanical arm method for planning track disclosed by the invention for considering spacecraft ontology attitude motion, belongs to multi-body system trajectory planning field.Concrete methods of realizing of the present invention is as follows: first under spacecraft original state, determining the variation track that the position of mechanical arm tail end and posture are directed toward by quintic algebra curve paths planning method；According to the planning of spacecraft centerbody attitude motion, seeks former desired locations under spacecraft body coordinate system and be directed toward the variable quantity generated due to centerbody attitude motion, motion planning of mechanical arm is compensated with this；By the summation corresponding with compensation proposition of the former trajectory planning of mechanical arm tail end, it is denoted as the final mechanical arm tail end locus of points, it is planned by motion profile of the kinematics Pseudoinverse algorithm to each joint angle of mechanical arm, and then realizes the mechanical arm trajectory planning for considering spacecraft ontology attitude motion；The present invention has many advantages, such as that mitigate spaceborne computer calculates pressure in real time, improves trajectory planning efficiency.
Description
Technical Field
The invention relates to a mechanical arm trajectory planning method considering spacecraft attitude motion, and belongs to the field of multi-body system trajectory planning.
Background
In the process of executing on-orbit operation by a spacecraft system carrying the mechanical arm, a control target is usually the position and attitude pointing direction of an actuator at the tail end of the mechanical arm, and physical quantities which can be directly controlled in a model of the spacecraft system are the position and attitude of a central body of the spacecraft and the rotation angle of each section of mechanical arm joint. Therefore, in order to point the end effector at a desired position in space in a certain attitude, the motion of the robotic arm needs to be planned by a trajectory planning algorithm. In the related research of the trajectory planning of the mechanical arm, the position and the attitude orientation of the mechanical arm are usually described in a spacecraft central body coordinate system under the actual conditions of engineering measurement. However, in an actual task, a spacecraft central body may have a certain attitude motion, so that under the condition that the spatial position and the attitude orientation which are expected to be reached are not changed, the original expected position and the attitude orientation are also changed relative to the spacecraft central body. Therefore, under the condition that the spacecraft central body has attitude motion, the motion influence of the spacecraft central body needs to be considered and corresponding motion compensation needs to be added in the trajectory planning of the mechanical arm. However, in the existing related documents, most of researches only mention real-time motion compensation of the mechanical arm based on measurement of a target by a satellite-borne sensor, the method has a large calculation amount, and puts high requirements on the real-time calculation capability of a satellite-borne computer, and few documents perform open-loop planning on the motion compensation of the mechanical arm.
Disclosure of Invention
The invention discloses a mechanical arm trajectory planning method considering spacecraft body attitude motion, which aims to solve the problems that: when the expected position and the direction of the end actuator on the mechanical arm are fixed and unchanged in the inertial space, the trajectory planning of the end actuator of the mechanical arm is compensated according to the attitude motion information of the central body of the spacecraft, so that the end actuator of the mechanical arm can reach the original expected position and the direction in the inertial space under the condition that the central body of the spacecraft has attitude motion, the trajectory planning of the mechanical arm considering the attitude motion of the spacecraft body is realized, and the planning efficiency is high.
The purpose of the invention is realized by the following technical scheme.
The invention discloses a mechanical arm track planning method considering spacecraft body attitude motion. According to the planning of the attitude motion of the central body of the spacecraft, the original expected position and the variation of the pointing direction of the central body in the coordinate system of the spacecraft body, which are generated due to the attitude motion of the central body, are obtained, so that the motion planning of the mechanical arm is compensated. And correspondingly summing the original trajectory planning and the compensation planning at the tail end of the mechanical arm, recording the sum as a final mechanical arm tail end point trajectory, and obtaining the motion trajectory of each joint angle of the mechanical arm through a kinematic pseudo-inverse algorithm so as to realize the mechanical arm trajectory planning considering the spacecraft body attitude motion. The method has the advantages of reducing the real-time calculation pressure, improving the track planning efficiency and the like.
The invention discloses a mechanical arm trajectory planning method considering spacecraft body attitude motion, which comprises the following steps:
the method comprises the following steps: and determining the position and attitude pointing change track of the tail end actuator on the mechanical arm by a quintic polynomial path planning method in the initial state of the spacecraft.
Defining a central body coordinate system f of a service spacecraft at an initial moment in an initial state of the spacecraftb0And the system of inertia feCoincident, central body coordinate system fbMoves with the movement of the central body. Obtaining the initial position of the mechanical arm end actuator under the system according to the initial states of the service spacecraft central body and the mechanical armAnd initial attitude headingPresetting the maximum value of the motion speed of the service spacecraft central body and the mechanical arm and the original expected position of the service spacecraft central body under a coordinate systemPointing to the original expected attitudeDue to initial attitude pointingVector and original expected attitude pointingThree incompletely independent components are respectively arranged in the vector, and the initial attitude points toVector and original expected attitude pointingIn order to avoid the uncertainty caused by the existence of the positive and negative components, the attitude direction represented by the direction cosine is converted into two azimuth angles α in spacenAnd αeThe first and second time derivatives of the attitude of the end effector are determined and converted to first and second time derivatives of the direction cosine, angle α is definednRepresenting attitude headingAnd the main system XbObZbAngle between planes, αeRepresenting attitude headingIn the main system XbObZbPlane surfaceUpper projection and body system ObZbThe angle between the axes.
According to the azimuth angle αnAnd αeIs defined as
Wherein:representing gestural pointingThe second component of (a).
In gesture-based pointingSolving azimuth αeIn order to avoid singularity due to zero denominator, azimuth α is seteThe solution process of (2) is defined as:
wherein:representing gestural pointingThe second component of (a) is,representing gestural pointingThe third component.
Initial attitude pointing by end actuatorsObtaining an initial azimuth αn_0And αe_0Pointing by the end-effector to the desired attitudeObtaining an original desired azimuth αn_r0And αe_r0. Bringing the end effector to the desired positionAnd original desired azimuth αn_r0And αe_r0And initial positionAnd initial azimuth αn_0And αe_0And performing difference to obtain the original position variation and the original azimuth angle variation of the end actuator.
In the method for planning the fifth-order polynomial, the coefficient to be determined of the fifth-order polynomial is solved according to the difference value of variables in the states of the start and the end of a task and the limitation of the change rate, and then the second-order smooth change process of the planning quantity with respect to time is obtained. The expected value of the planned quantity theta is recorded as thetarThe value of the initial state is recorded as theta0Defining the maximum value of the first derivative in the parameter variation asMaximum of the second derivative isThen according to the fifth order polynomial, there are:
wherein,time coefficientTau is the current time t and the task duration tfThe ratio of (a) to (b).
According to the constraint conditions such as the limitation of the characteristic of the fifth-order polynomial and the change rate of the planning quantity, the task duration has the following constraint:
selecting the minimum one of the conditions simultaneously meeting the formula (1.4) as the shortest time required by the task, thereby obtaining the shortest task time t meeting the motion constraint of each mechanical armf. On finding the task duration tfThen, based on the method of the fifth order polynomial, the planning quantity θ and its first and second time derivatives are:
in the process of planning the track of the mechanical arm, the planned quantity is the position of an actuator at the tail end of the mechanical armAnd attitude pointingObtaining the position of the mechanical arm end actuator by the method of the quintic polynomial in the formula (1.5)And azimuth angleAndsmooth continuous first and second time derivatives.
Due to the azimuth angle of the mechanical arm end actuatorAndis difficult to write into a form of showing the angular velocity of the mechanical arm joint, and the posture points toThe track can be written into a form of showing the angular velocity of the mechanical arm joint, and the planning of the mechanical arm joint track is facilitated. Therefore, before solving the joint angle motion law through the inverse kinematics relation of the mechanical arm, the azimuth angle of the end actuator on the mechanical arm is usedAndand its trajectory is converted into attitude pointingThe change trajectory of (2). According to the azimuthAndpointing to the postureThe geometrical relationship between the two is as follows:
to the gesture directionCalculating first and second time derivatives of
Wherein α is the azimuth angleAndformed matrix of phi azimuth α matrix and attitude orientationThe conversion matrix between is specifically written as:
at this point, the lower position of the end effector in the Cartesian space is obtained through the programming of a quintic polynomial methodPointing to the postureSmoothing successive ideal first and second time derivative matricesAnd
due to attitude pointingThree components in the vector are notIs totally independent ofRepresenting gestural pointingThe first two components of the vector, denotedWherein:
position of mechanical arm end actuator obtained by formula (1.8)Pointing to the postureSmoothing successive first and second time derivative matricesAndnamely, the mechanical arm end actuator obtained by the quintic polynomial programming method moves to the original expected positionPointing to the original expected attitudeThe trajectory of (2).
Secondly, performing attitude motion planning according to the attitude motion requirement of the central body of the spacecraft, and simultaneously obtaining the original expected position of the systemPointing to the postureThe changing trajectory due to the central body attitude motion compensates for the trajectory planning of the robotic arm.
At a desired position in the central body of the spacecraftBy the inertial space position vector in which it is locatedThe relative position with respect to an inertial space position vector R of the center of mass of the spacecraft centerbody is represented, and the transition from the inertial frame to the spacecraft frame is:
wherein: a. thebeA coordinate transformation matrix representing the inertial system to the spacecraft body system,indicating a desired position in the central body of a spacecraftExpressed under the inertial system.
Converting the coordinate into matrix A according to the rotation relationship of the coordinate systembeRate of change and angular velocity ω of rotation of the central body of the spacecraftbThe relationship between them is:
the first and second derivatives of the relative motion of the original desired point position are expressed as:
a certain attitude orientation e in inertial spacefeThe process of switching to the spacecraft system is as follows:
efb＝Abeefe(1.11)
according to the relation between the change rate of the coordinate transformation matrix and the rotation speed of the coordinate system, the attitude of the spacecraft system points to efbThe first and second time derivatives of (d) are:
wherein: a. thebeA coordinate transformation matrix representing the inertial system to the spacecraft body system,representing angular velocity omega of rotation of the central body of a spacecraftbThe cross-multiplication matrix of (1) is specifically written as:
thereby obtaining a certain expected position of the central body of the spacecraft in the inertial space under the condition of attitude motionAnd a certain attitude heading efbThe first and second time derivatives of the change in the spacecraft centerbody coordinate system are noted as:
wherein:representing an attitude pointing vector efbThe first two components.
Obtained in formula (1.13)Andand compensating the trajectory planning of the mechanical arm according to the change trajectory generated by the attitude motion of the spacecraft central body in the position and the attitude direction in the inertial space.
Step three: and correspondingly summing the motion tracks of the tail end points obtained in the first step and the second step, and solving the motion track of each joint angle by adopting a mechanical arm kinematics pseudo-inverse algorithm to realize mechanical arm track planning considering the attitude motion condition of the spacecraft body.
The track of the tail end point of the mechanical arm in the initial state of the spacecraft, which is obtained in the step oneAndthe compensation quantity generated by the attitude motion of the spacecraft central body obtained in the step twoAndcorresponding sums, recorded as end point trajectoriesAndas input for the planning of the joint trajectory of the mechanical arm:
based on the kinematic relationship between the position and attitude orientation of the end actuator and the mechanical arm system, the motion of the mechanical arm in a Cartesian space is converted into the motion in a joint configuration space in a manner of pseudo-inverse solution of the Jacobian matrix.
The position and attitude orientation matrix of the end-effector in Cartesian space is recorded asRecording the joint angle of each mechanical arm as η, the position and orientation matrix has the following relationship with the first derivative of the joint angle:
wherein J(η)Is the jacobian matrix of the joint angle of the mechanical arm.
According to the formula (1.15), the terminal point track is obtainedAndand solving the motion trail of the joint angle of the mechanical arm in a reverse way according to the known kinematic relationship of the mechanical arm. And performing pseudo-inverse operation on the Jacobian matrix in the solving process, wherein the solved operation formula is as follows:
wherein
Obtaining the ideal motion trail of each joint angle of the mechanical arm through a formula (1.16)Andthe mechanical arm can reach the preset expected position and the posture pointing direction in the inertial space under the condition that the central body of the spacecraft carries out the posture motion, and then the mechanical arm trajectory planning considering the posture motion of the spacecraft body is realized.
The method also comprises the following four steps: the angular trajectory of each joint of the mechanical arm planned in the third stepAndthe motion control to the mechanical arm is realized through the controller.
Preferably, in order to improve the condition that singularity is easy to occur in the pseudo-inverse operation process of the Jacobian matrix, a robust adjusting item is added in the operation, and singularity is avoided to a certain extent at the cost of sacrificing precision. The factor influencing the adjusting weight in the robust adjusting term is recorded as lambda, the larger the value of the lambda is, the larger the adjusting effect on the Jacobian matrix is, and the larger the error is brought. Adjusted pseudo-inverse operation expression (1.16)Comprises the following steps:
wherein In×nRepresenting an identity matrix, its dimensions and a planned end point trajectory matrixAndthe number of rows of (c) is the same.
Has the advantages that:
1. the invention discloses a mechanical arm trajectory planning method considering spacecraft body attitude motion, which is used for solving the change of an expected position and attitude direction of an original inertia space of a system while planning spacecraft central body attitude motion, and compensating motion planning of a mechanical arm, so that the mechanical arm can still reach the original expected position and attitude direction in the inertia space when the central body performs attitude motion.
2. According to the method for planning the trajectory of the mechanical arm in consideration of the attitude motion of the spacecraft body, disclosed by the invention, the influence of the attitude motion of the central body of the spacecraft is taken into consideration in the open-loop motion planning of the mechanical arm, the motion of the mechanical arm is planned in real time without depending on the measurement information of the expected position and the attitude direction, so that the calculation requirement of the trajectory planning of the mechanical arm is reduced under the condition of the requirement of the attitude motion of the spacecraft, the calculation pressure of the on-orbit real-time trajectory planning of the mechanical arm is reduced, and the.
3. The mechanical arm trajectory planning method considering the spacecraft body attitude motion disclosed by the invention is supplemented in two traditional solving methods. First, pointing at the passing attitudeSolving azimuth αeIn order to avoid singularity caused by denominator being zero, a judgment is added in the traditional azimuth angle solving method, and when the attitude points toWhere the first and third components are both 0 define azimuth αeIs 0. Secondly, in order to reduce the possibility of singularity occurrence in pseudo-inverse solution of the Jacobian matrix of the joint angle of the mechanical arm, Luru is added in the pseudo-inverse solution processAnd the stick adjustment item avoids singularity to a certain extent at the cost of sacrificing precision.
Drawings
Fig. 1 is a schematic flow chart of a robot arm trajectory planning method considering spacecraft body attitude motion disclosed by the invention.
FIG. 2 is a schematic view of an end effector attitude heading vector and azimuth.
Fig. 3 is a comparison diagram of whether to design the mechanical arm compensation motion when the spacecraft central body performs the attitude motion and a certain fixed expected position. The results of the x-axis position tracking are shown in fig. (a), the results of the y-axis position tracking are shown in fig. (b), and the results of the z-axis position tracking are shown in fig. (c).
Fig. 4 shows the result of tracking a fixed position and attitude orientation in space by a robot arm when the spacecraft central body performs attitude motion and the robot arm is designed to perform compensation motion. Where graph (a) is the difference between the robot arm end point position and the desired position and graph (b) is the difference between the robot arm end point attitude heading and the desired attitude heading.
Fig. 5 is a comparison of whether determinant values of a jacobian matrix of joint angles of the mechanical arm supplement robust adjustment items in the mechanical arm trajectory planning process.
Detailed Description
For a better understanding of the objects and advantages of the present invention, reference should be made to the following detailed description taken in conjunction with the accompanying drawings and examples.
Example 1:
the embodiment discloses a track planning method for a mechanical arm composed of five sections of arm rods, and the track planning and simulation are carried out on the tail end position and the attitude direction of the mechanical arm aiming at the attitude motion task condition of a central body of a spacecraft. As shown in fig. 1, the method for planning the trajectory of the robot arm considering the attitude motion of the spacecraft body disclosed in this embodiment may be completed through the following steps:
the method comprises the following steps: and determining the position and attitude pointing change track of the tail end actuator on the mechanical arm by a quintic polynomial path planning method in the initial state of the spacecraft.
Initial values of relevant parameters of the spacecraft and the mechanical arm carried by the spacecraft are shown in table 1.
TABLE 1 spacecraft Central body and robot arm initial parameters
Defining azimuth αnRepresenting attitude headingAnd the main system XbObZbAngle between planes, αeRepresenting attitude headingIn the main system XbObZbProjection on plane and body system ObZbAngle between axes according to azimuth αnAnd αeIs defined as
Wherein:representing gestural pointingThe second component of (a).
In gesture-based pointingSolving azimuth αeIn order to avoid singularity due to zero denominator, azimuth α is seteThe solution process of (2) is defined as:
wherein:representing gestural pointingThe second component of (a) is,representing gestural pointingThe third component.
According to the data shown in the table 1, the initial position of the tail end of the mechanical arm under the spacecraft system is obtained through the spacecraft kinematics relationAnd azimuth angleAndformed matrix azimuth matrix α0Comprises the following steps:
α0＝[14.94 95.14]Tdeg
the differences between the attitude of the central body, the end position of the robot arm and the attitude heading at the beginning and end states are shown in table 2.
TABLE 2 Start and end State parameter Differences
In the method for planning the fifth-order polynomial, the coefficient to be determined of the fifth-order polynomial is solved according to the difference value of variables in the states of the start and the end of a task and the limitation of the change rate, and then the second-order smooth change process of the planning quantity with respect to time is obtained. The expected value of the planned quantity theta is recorded as thetarThe value of the initial state is recorded as theta0Defining the maximum value of the first derivative in the parameter variation asMaximum of the second derivative isThen according to the fifth order polynomial, there are:
wherein,the time coefficient tau is the current time t and the task duration tfThe ratio of (a) to (b).
According to the constraint conditions such as the change rate of the characteristics of the fifth-order polynomial and the variables in the initial and final states of the task, the task duration is constrained as follows:
substituting constraint conditions such as mechanical arm joint motion and the like to obtain the shortest task time t meeting the constraint of the spacecraft central body motion and each mechanical arm motion simultaneouslyfIt was 57.5 s.
Adopting a method of a fifth-order polynomial to adjust the tail end position of the mechanical armAnd azimuth angle αn、αeAnd spacecraft central body attitude motion, the planning quantity and the first and second derivatives thereof are expressed as:
according to azimuth αnAnd αePointing to the postureThe geometrical relationship between the two is as follows:
to the gesture directionCalculating first and second time derivatives of
Wherein phi is an azimuth α matrix and an attitude headingThe conversion matrix between is specifically written as:
at this point, the lower position of the end effector in the Cartesian space is obtained through the programming of a quintic polynomial methodPointing to the postureSmoothing successive ideal first and second time derivative matricesAnd
due to attitude pointingThe three components in the vector are not completely independent, so thatRepresenting gestural pointingThe first two components of the vector, denotedWherein:
derived mechanical arm end actuator positionPointing to the postureSmoothing successive first and second time derivative matricesAndnamely, the mechanical arm end actuator obtained by the quintic polynomial programming method moves to the original expected positionPointing to the original expected attitudeThe trajectory of (2). The solid line illustrated as "p 1" in fig. 3 is the result of performing only the original spatial fixation position without compensating for the tracking situation.
Step two: carrying out attitude motion planning according to the attitude motion requirement of the central body of the spacecraft, and simultaneously obtaining the original expected position of the systemPointing to the postureThe changing trajectory due to the central body attitude motion compensates for the trajectory planning of the robotic arm.
At a desired position in the central body of the spacecraftBy the inertial space position vector in which it is locatedThe relative position with respect to an inertial space position vector R of the center of mass of the spacecraft centerbody is represented, and the transition from the inertial frame to the spacecraft frame is:
wherein: a. thebeA coordinate transformation matrix representing the inertial system to the spacecraft body system,indicating a desired position in the central body of a spacecraftExpressed under the inertial system.
Converting the coordinate into matrix A according to the rotation relationship of the coordinate systembeRate of change and angular velocity ω of rotation of the central body of the spacecraftbThe relationship between them is:
the first and second derivatives of the relative motion of the original desired point position are expressed as:
a certain attitude orientation e in inertial spacefeThe process of switching to the spacecraft system is as follows:
efb＝Abeefe
according to the relation between the change rate of the coordinate transformation matrix and the rotation speed of the coordinate system, the attitude of the spacecraft system points to efbThe first and second time derivatives of (d) are:
wherein: a. thebeCoordinate transformation matrix, omega, representing the inertial system to the spacecraft body systembRepresenting the rotating angular speed of the central body of the spacecraft, the wave number above the central body is marked by a cross-multiplication matrix,the specific writing is as follows:
thereby obtaining a certain expected position of the central body of the spacecraft in the inertial space under the condition of attitude motionAnd a certain attitude heading efbThe first and second time derivatives of the change in the spacecraft centerbody coordinate system are noted as:
wherein:representing an attitude pointing vector efbThe first two components.
Andand compensating the trajectory planning of the mechanical arm according to the change trajectory generated by the attitude motion of the spacecraft central body in the position and the attitude direction in the inertial space.
Step three: and correspondingly summing the motion tracks of the tail end points obtained in the first step and the second step, and solving the motion track of each joint angle by adopting a mechanical arm kinematics pseudo-inverse algorithm to realize mechanical arm track planning considering the attitude motion condition of the spacecraft body.
The track of the tail end point of the mechanical arm in the initial state of the spacecraft, which is obtained in the step oneAndthe compensation quantity generated by the attitude motion of the spacecraft central body obtained in the step twoAndcorresponding sums, recorded as end point trajectoriesAndas input for the planning of the joint trajectory of the mechanical arm:
based on the kinematic relationship between the position and attitude orientation of the end actuator and the mechanical arm system, the motion of the mechanical arm in a Cartesian space is converted into the motion in a joint configuration space in a manner of pseudo-inverse solution of the Jacobian matrix. The position and attitude orientation matrix of the end-effector in Cartesian space is recorded asThe joint angle of each mechanical arm is recorded as η ═ theta1θ2θ3θ4θ5]TThen the pose matrix and the first derivative of the joint angle have the following relationship:
wherein J(η)Jacobian matrix for the arm joint angle:
in the formula AbjIs a coordinate transformation matrix from the j section mechanical arm coordinate system to the body system, gammajIs a j section mechanical arm rotating shaft direction matrix, ejtIs the attitude pointing vector r between the jth section of mechanical arm and the tail end actuatorjtIs the vector from the center of mass of the jth arm to the center of mass of the end effector.
In order to enable the number of the degrees of freedom of the mechanical arm to meet the planning requirements of position and posture pointing, the tail end joint of the mechanical arm has three degrees of freedom. To determine the attitude heading of the end-effector, the angular rotational velocity and joint of the end-effector relative to the previous segment of the arm is definedThe relation between the angles is the same as the relation between angular speed and attitude angle in the 3-1-2 rotation mode in spacecraft attitude kinematics. End effector coordinate system f5Coordinate system f of the previous section of mechanical arm4A conversion matrix of45＝(Ay(θ(2))Ax(θ(1))Az(θ(3)))TAnd θ (k) represents the kth component of the attitude angle θ.
According to the obtained tail end point trackAndand solving the motion trail of the joint angle of the mechanical arm in a reverse way according to the known kinematic relationship of the mechanical arm. And performing pseudo-inverse operation on the Jacobian matrix in the solving process, wherein the solved operation formula is as follows:
wherein
The obtained ideal motion track of each joint angle of the mechanical armAndthe planning method can realize the planning of the mechanical arm track considering the attitude motion of the spacecraft body, so that the mechanical arm can reach the preset expected position and the attitude direction in the inertial space under the condition that the spacecraft central body carries out the attitude motion. As shown in the legend of FIG. 3The dotted line of "p 12" is the result of the mechanical arm motion compensation based on the central body attitude motion, which greatly reduces the difference between the target and the expected target when no compensation planning is performed, and improves the planning precision. Fig. 4 shows that the difference between the planned trajectory of the robot arm considering the attitude motion of the spacecraft body and the expected position and attitude direction is finally converged to zero, which indicates that the expected value can be reached.
The method also comprises the following four steps: the angular trajectory of each joint of the mechanical arm planned in the third stepAndthe motion control to the mechanical arm is realized through the controller.
Preferably, in order to improve the condition that singularity is easy to occur in the pseudo-inverse operation process of the Jacobian matrix, a robust adjusting item is added in the operation, and singularity is avoided to a certain extent at the cost of sacrificing precision. The factor influencing the adjusting weight in the robust adjusting term is recorded as lambda, the larger the value of the lambda is, the larger the adjusting effect on the Jacobian matrix is, and the larger the error is brought. In the modified pseudo-inverse operation expressionComprises the following steps:
in the formula I5×5Representing a five-dimensional identity matrix.
In this embodiment, the adjustment factor λ is 10-3. As shown in fig. 5, whether to add a robust adjustment term has an effect on the jacobian matrix determinant values, the closer to zero the more likely it is to generate singularities.
Under the trajectory planning method of the mechanical arm, the tail end of the mechanical arm can reach a certain fixed expected position and attitude direction in an inertial space under the condition that the central body of the spacecraft carries out certain attitude motion, the motion of the mechanical arm is planned in real time without depending on measurement information of the expected position and the attitude direction, the calculation pressure of the on-orbit real-time trajectory planning of the mechanical arm is reduced, and the planning efficiency is improved.
The above detailed description is intended to illustrate the objects, aspects and advantages of the present invention, and it should be understood that the above detailed description is only exemplary of the present invention and is not intended to limit the scope of the present invention, and any modifications, equivalents, improvements and the like within the spirit and principle of the present invention should be included in the scope of the present invention.
Claims (6)
1. The mechanical arm trajectory planning method considering spacecraft body attitude motion is characterized by comprising the following steps of: comprises the following steps of (a) carrying out,
the method comprises the following steps: determining the position of a mechanical arm end actuator and the change track of the attitude pointing direction by a quintic polynomial path planning method in the initial state of the spacecraft;
secondly, performing attitude motion planning according to the attitude motion requirement of the central body of the spacecraft, and simultaneously obtaining the original expected position of the systemPointing to the postureCompensating the trajectory planning of the mechanical arm due to the changing trajectory generated by the attitude motion of the central body;
step three: and correspondingly summing the motion tracks of the tail end points obtained in the first step and the second step, and solving the motion track of each joint angle by adopting a mechanical arm kinematics pseudo-inverse algorithm to realize mechanical arm track planning considering the attitude motion condition of the spacecraft body.
2. The method for planning a trajectory of a robotic arm taking into account attitude motion of a spacecraft of claim 1, wherein: step four, the angular trajectory of each joint of the mechanical arm planned in the step three is further includedAndthe motion control to the mechanical arm is realized through the controller.
3. The method for planning a trajectory of a robotic arm taking into account the attitude motion of a spacecraft body of claim 1 or 2, wherein: the specific implementation method of the step one is that,
defining a central body coordinate system f of a service spacecraft at an initial moment in an initial state of the spacecraftb0And the system of inertia feCoincident, central body coordinate system fbMove with the movement of the central body; obtaining the initial position of the tail end of the mechanical arm under the system according to the initial states of the central body and the mechanical arm of the service spacecraftAnd initial attitude headingPresetting the maximum value of the motion speed of the service spacecraft central body and the mechanical arm and the original expected position of the service spacecraft central body under a coordinate systemPointing to the original expected attitudeDue to initial attitude pointingVector and original expected attitude pointingAll vectors are expressed by direction cosine and the moduli of the vectors are all 1, so that the initial attitude points toVector and original expected attitude pointingRespectively having three components which are not completely independent, so that the first two components are respectively selected as variables, but the corresponding third component has positive and negative possibilities, and in order to avoid uncertainty caused by the positive and negative possibilities, the attitude direction represented by direction cosine is converted into two azimuth angles α in spacenAnd αeThe first and second time derivatives of the attitude of the end effector are obtained and converted into the first and second time derivatives of the direction cosine, the azimuth is defined αnRepresenting attitude headingAnd the main system XbObZbAngle between planes, αeRepresenting attitude headingIn the main system XbObZbProjection on plane and body system ObZbThe included angle between the axes;
according to the azimuth angle αnAnd αeIs defined as
Wherein:representing gestural pointingThe second component of (a);
in gesture-based pointingSolving azimuth αeIn order to avoid singularity due to zero denominator, azimuth α is seteThe solution process of (2) is defined as:
wherein:representing gestural pointingThe second component of (a) is,representing gestural pointingTo middleThree components;
initial attitude pointing by end actuatorsObtaining an initial azimuth αn_0And αe_0Pointing by the end-effector to the desired attitudeObtaining an original desired azimuth αn_r0And αe_r0(ii) a Bringing the end effector to the desired positionAnd original desired azimuth αn_r0And αe_r0And initial positionAnd initial azimuth αn_0And αe_0Making a difference to obtain the original position variation and the original azimuth variation of the tail end actuator;
in the quintic polynomial planning method, a coefficient to be determined of a quintic polynomial is solved according to a difference value of a variable in a task starting state and a task ending state and a change rate limit, and a second-order smooth change process of a planning quantity with respect to time is further obtained; the expected value of the planned quantity theta is recorded as thetarThe value of the initial state is recorded as theta0Defining the maximum value of the first derivative in the parameter variation asMaximum of the second derivative isThen according to the fifth order polynomial, there are:
wherein,the time coefficient tau is the current time t and the task duration tfThe ratio of (A) to (B);
according to the constraint conditions such as the limitation of the characteristic of the fifth-order polynomial and the change rate of the planning quantity, the task duration has the following constraint:
selecting the minimum one of the conditions simultaneously meeting the formula (0.4) as the shortest time required by the task, thereby obtaining the shortest task time t meeting the motion constraint of each mechanical armf(ii) a On finding the task duration tfThen, based on the method of the fifth order polynomial, the planning quantity θ and its first and second time derivatives are:
in the process of planning the track of the mechanical arm, the planned quantity is the position of an actuator at the tail end of the mechanical armAnd attitude pointingObtaining the position of the mechanical arm end actuator by the method of the fifth-order polynomial according to the formula (0.5)And azimuth angleAndsmooth continuous first and second time derivatives.
Due to the orientation of the end-effector of the robot armCornerAndis difficult to write into a form of showing the angular velocity of the mechanical arm joint, and the posture points toThe track can be written into a form of showing the angular velocity of the mechanical arm joint, which is beneficial to planning the mechanical arm joint track; therefore, before solving the joint angle motion law through the inverse kinematics relation of the mechanical arm, the azimuth angle of the end actuator on the mechanical arm is usedAndand its trajectory is converted into attitude pointingThe change trajectory of (2); according to the azimuthAndpointing to the postureThe geometrical relationship between the two is as follows:
to the gesture directionCalculating first and second time derivatives of
Wherein α is the azimuth angleAndformed matrix of phi azimuth α matrix and attitude orientationThe conversion matrix between is specifically written as:
at this point, the lower position of the end effector in the Cartesian space is obtained through the programming of a quintic polynomial methodPointing to the postureSmoothing successive ideal first and second time derivative matricesAnd
due to attitude pointingThe three components in the vector are not completely independent, so thatRepresenting gestural pointingThe first two components of the vector, denotedWherein:
position of the end actuator of the robot arm calculated by the formula (0.8)Pointing to the postureSmoothing successive first and second time derivative matricesAndnamely, the mechanical arm end actuator obtained by the quintic polynomial programming method moves to the original expected positionPointing to the original expected attitudeThe trajectory of (2).
4. The method for planning a trajectory of a robotic arm taking into account the attitude motion of a spacecraft of claim 3, wherein: the concrete implementation method of the second step is that,
at a desired position in the central body of the spacecraftBy the inertial space position vector in which it is locatedThe relative position with respect to an inertial space position vector R of the center of mass of the spacecraft centerbody is represented, and the transition from the inertial frame to the spacecraft frame is:
wherein: a. thebeA coordinate transformation matrix representing the inertial system to the spacecraft body system,indicating a desired position in the central body of a spacecraftExpressed in the inertial system;
converting the coordinate into matrix A according to the rotation relationship of the coordinate systembeRate of change and angular velocity ω of rotation of the central body of the spacecraftbThe relationship between them is:
the first and second derivatives of the relative motion of the original desired point position are expressed as:
a certain attitude orientation e in inertial spacefeThe process of switching to the spacecraft system is as follows:
efb＝Abeefe(0.11)
according to the relation between the change rate of the coordinate transformation matrix and the rotation speed of the coordinate system, the attitude of the spacecraft system points to efbThe first and second time derivatives of (d) are:
wherein: a. thebeA coordinate transformation matrix representing the inertial system to the spacecraft body system,representing angular velocity omega of rotation of the central body of a spacecraftbThe cross-multiplication matrix of (1) is specifically written as:
thereby obtaining a certain expected position of the central body of the spacecraft in the inertial space under the condition of attitude motionAnd a certain attitude heading efbThe first and second time derivatives of the change in the spacecraft centerbody coordinate system are noted as:
wherein:representing an attitude pointing vector efbThe first two components;
obtained by the formula (0.13)Andand compensating the trajectory planning of the mechanical arm according to the change trajectory generated by the attitude motion of the spacecraft central body in the position and the attitude direction in the inertial space.
5. The method for planning a trajectory of a robotic arm taking into account the attitude motion of a spacecraft of claim 4, wherein: the third step is realized by the concrete method that,
the track of the tail end point of the mechanical arm in the initial state of the spacecraft, which is obtained in the step oneAndthe compensation quantity generated by the attitude motion of the spacecraft central body obtained in the step twoAndcorresponding sums, recorded as end point trajectoriesAndas input for the planning of the joint trajectory of the mechanical arm:
based on the kinematic relationship between the position and attitude orientation of the tail end actuator and the mechanical arm system, converting the motion of the mechanical arm in a Cartesian space into the motion in a joint configuration space by a pseudo-inverse solution mode of a Jacobian matrix;
the position and attitude orientation matrix of the end-effector in Cartesian space is recorded asRecording the joint angle of each mechanical arm as η, the position and orientation matrix has the following relationship with the first derivative of the joint angle:
wherein J(η)A jacobian matrix of the joint angle of the mechanical arm;
according to the formula (0.15), the terminal point trace is obtainedAndsolving the motion trail of the joint angle of the mechanical arm in a reverse way according to the known kinematic relationship of the mechanical arm; and performing pseudo-inverse operation on the Jacobian matrix in the solving process, wherein the solved operation formula is as follows:
wherein
Obtaining the ideal motion trail of each joint angle of the mechanical arm through a formula (0.16)Andthe mechanical arm can reach the preset expected position and the posture pointing direction in the inertial space under the condition that the central body of the spacecraft carries out the posture motion, and then the mechanical arm trajectory planning considering the posture motion of the spacecraft body is realized.
6. The method for planning a trajectory of a robotic arm taking into account the attitude motion of a spacecraft of claim 5, wherein: in order to improve the condition that singularity is easy to occur in the pseudo-inverse operation process of the Jacobian matrix, a robust adjusting item is added in the operation, and singularity is avoided to a certain extent at the cost of sacrificing precision; the factor influencing the adjusting weight in the robust adjusting item is recorded as lambda, the larger the value of the lambda is, the larger the adjusting effect on the Jacobian matrix is, and meanwhile, the larger the error is brought; adjusted pseudo-inverse operation expression (0.16)Comprises the following steps:
wherein In×nRepresenting an identity matrix, its dimensions and a planned end point trajectory matrixAndthe number of rows of (c) is the same.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811470615.5A CN109623812B (en) | 2018-12-04 | 2018-12-04 | Mechanical arm trajectory planning method considering spacecraft body attitude motion |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811470615.5A CN109623812B (en) | 2018-12-04 | 2018-12-04 | Mechanical arm trajectory planning method considering spacecraft body attitude motion |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109623812A true CN109623812A (en) | 2019-04-16 |
| CN109623812B CN109623812B (en) | 2020-09-15 |
Family
ID=66070836
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811470615.5A Active CN109623812B (en) | 2018-12-04 | 2018-12-04 | Mechanical arm trajectory planning method considering spacecraft body attitude motion |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109623812B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108068108A (en) * | 2016-11-16 | 2018-05-25 | 沈阳高精数控智能技术股份有限公司 | Concertina type motion planning and robot control method is swung in plane |
| CN112318512A (en) * | 2020-11-03 | 2021-02-05 | 北京理工大学 | Method and system for determining degree of freedom of spinal vertebra of robot mouse |
| CN114505865A (en) * | 2022-03-15 | 2022-05-17 | 上海大学 | Pose tracking-based mechanical arm path generation method and system |
| CN116494250A (en) * | 2023-06-26 | 2023-07-28 | 极限人工智能(北京)有限公司 | Mechanical arm control method, controller, medium and system based on speed compensation |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103235597A (en) * | 2013-04-09 | 2013-08-07 | 北京理工大学 | Rapid stable joint control method for attitude maneuver of spacecraft |
| CN106891335A (en) * | 2017-03-23 | 2017-06-27 | 北京空间飞行器总体设计部 | A kind of submissive and control method for coordinating of the in-orbit capture process of robot for space |
| CN108132601A (en) * | 2017-12-06 | 2018-06-08 | 西北工业大学 | A kind of method for inhibiting spacecraft pedestal attitude disturbance using mechanical arm |
| CN108214519A (en) * | 2017-12-18 | 2018-06-29 | 北京航空航天大学 | A kind of aerial any attitude extremely lands the self-adjusting quadruped robot of posture |
| EP3351355A1 (en) * | 2015-09-18 | 2018-07-25 | Kawasaki Jukogyo Kabushiki Kaisha | Device and method for positioning processing tool |
| CN108621162A (en) * | 2018-05-09 | 2018-10-09 | 广西科技大学 | A kind of manipulator motion planning method |
-
2018
- 2018-12-04 CN CN201811470615.5A patent/CN109623812B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103235597A (en) * | 2013-04-09 | 2013-08-07 | 北京理工大学 | Rapid stable joint control method for attitude maneuver of spacecraft |
| EP3351355A1 (en) * | 2015-09-18 | 2018-07-25 | Kawasaki Jukogyo Kabushiki Kaisha | Device and method for positioning processing tool |
| CN106891335A (en) * | 2017-03-23 | 2017-06-27 | 北京空间飞行器总体设计部 | A kind of submissive and control method for coordinating of the in-orbit capture process of robot for space |
| CN108132601A (en) * | 2017-12-06 | 2018-06-08 | 西北工业大学 | A kind of method for inhibiting spacecraft pedestal attitude disturbance using mechanical arm |
| CN108214519A (en) * | 2017-12-18 | 2018-06-29 | 北京航空航天大学 | A kind of aerial any attitude extremely lands the self-adjusting quadruped robot of posture |
| CN108621162A (en) * | 2018-05-09 | 2018-10-09 | 广西科技大学 | A kind of manipulator motion planning method |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108068108A (en) * | 2016-11-16 | 2018-05-25 | 沈阳高精数控智能技术股份有限公司 | Concertina type motion planning and robot control method is swung in plane |
| CN108068108B (en) * | 2016-11-16 | 2021-02-02 | 沈阳高精数控智能技术股份有限公司 | Method for controlling motion of in-plane swinging telescopic robot |
| CN112318512A (en) * | 2020-11-03 | 2021-02-05 | 北京理工大学 | Method and system for determining degree of freedom of spinal vertebra of robot mouse |
| CN114505865A (en) * | 2022-03-15 | 2022-05-17 | 上海大学 | Pose tracking-based mechanical arm path generation method and system |
| CN116494250A (en) * | 2023-06-26 | 2023-07-28 | 极限人工智能(北京)有限公司 | Mechanical arm control method, controller, medium and system based on speed compensation |
| CN116494250B (en) * | 2023-06-26 | 2023-11-03 | 极限人工智能(北京)有限公司 | Mechanical arm control method, controller, medium and system based on speed compensation |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109623812B (en) | 2020-09-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107490965B (en) | Multi-constraint trajectory planning method for space free floating mechanical arm | |
| CN109623812B (en) | Mechanical arm trajectory planning method considering spacecraft body attitude motion | |
| CN112757306B (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
| CN109159151B (en) | Mechanical arm space trajectory tracking dynamic compensation method and system | |
| CN101402199B (en) | Hand-eye type robot movable target extracting method with low servo accuracy based on visual sensation | |
| CN107505947B (en) | Space robot captured target despinning and coordination control method | |
| CN110561440B (en) | Multi-objective planning method for acceleration layer of redundant manipulator | |
| CN108508749A (en) | A kind of anti-interference iterative learning control method of Space Manipulator System for arresting noncooperative target | |
| CN107263466B (en) | Base undisturbed control method of space robot based on quadratic programming problem | |
| CN107450317A (en) | A kind of space manipulator self-adapting power control method for coordinating | |
| Li et al. | A hybrid visual servo control method for simultaneously controlling a nonholonomic mobile and a manipulator | |
| CN112666947B (en) | Design method of snake-shaped robot self-adaptive trajectory tracking controller based on improved LOS method | |
| CN114942593B (en) | Mechanical arm self-adaptive sliding mode control method based on disturbance observer compensation | |
| CN111890349A (en) | Four-degree-of-freedom mechanical arm motion planning method | |
| CN115302502A (en) | Mechanical arm trajectory planning method combining grabbing network and grabbing pose selection and adjustment | |
| CN114670191A (en) | Seven-degree-of-freedom elbow offset mechanical arm track planning method | |
| Zhao et al. | Minimum base disturbance control of free-floating space robot during visual servoing pre-capturing process | |
| CN110695994B (en) | Finite time planning method for cooperative repetitive motion of double-arm manipulator | |
| CN117301064B (en) | Double-arm robot safety cooperative control method based on fixed time convergence | |
| Ren et al. | Trajectory tracking control for a 3-DOF planar parallel manipulator using the convex synchronized control method | |
| CN114939869A (en) | Mechanical arm trajectory tracking method based on nonsingular rapid terminal sliding mode | |
| CN114055467A (en) | Space pose online simulation system based on five-degree-of-freedom robot | |
| Tangpattanakul et al. | Optimal trajectory of robot manipulator using harmony search algorithms | |
| CN115256371B (en) | Neural network-based method for presetting movement time of road rescue mobile mechanical arm | |
| CN116540721A (en) | Space robot optimal track planning method based on improved genetic particle swarm algorithm |
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 |