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
- spacecraft
- posture
- directed toward
- 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
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 present invention relates to a kind of mechanical arm method for planning track for considering attitude motion of spacecraft, belong to multi-body system track
Planning field.
Background technique
The Space Vehicle System of mechanical arm is carried in executing in-orbit operating process, control target is usually that mechanical arm tail end is made
The position of dynamic device and posture are directed toward, and the physical quantity that can be directly controlled in Space Vehicle System model is the position of spacecraft centerbody
Set the corner with posture and Ge Jie joint of mechanical arm.Therefore, in order to make end effectors be directed toward certain in space with certain posture
Desired locations need to plan the movement of mechanical arm by trajectory planning algorithm.It is ground in the correlation of mechanical arm trajectory planning
In studying carefully, for the physical condition of engineering survey, the position of mechanical arm and posture direction are all usually in spacecraft centerbody coordinate
It is described in system.But there may be certain attitude motions for spacecraft centerbody in actual task, thus when the sky of expectation arrival
Between position and posture be directed toward it is constant in the case where, relative to spacecraft centerbody, former desired locations and posture direction can also occur
Variation.Therefore, in spacecraft centerbody, there are in the case where attitude motion, need to consider spacecraft in the trajectory planning of mechanical arm
The movement of centerbody influences and corresponding motion compensation is added.But in existing pertinent literature, most of researchs are only referred to
The real-time motion compensation of mechanical arm is carried out to the measurement of target based on spaceborne sensor, this method operand is larger, to spaceborne
Computer real-time operation ability proposes higher requirement, and rarer document compensates this manipulator motion and carries out open loop rule
It draws.
Summary of the invention
The mechanical arm method for planning track disclosed by the invention for considering spacecraft ontology attitude motion, will solve the problems, such as
It is: when the desired locations of end effectors and direction immobilize in inertial space on mechanical arm, according to spacecraft centerbody
The trajectory planning of attitude motion information compensation mechanical arm tail end actuator, enables mechanical arm tail end actuator at spacecraft center
Body reaches original desired locations and direction in inertial space in the case where having attitude motion, realize and consider spacecraft ontology posture
The mechanical arm trajectory planning of movement, has the advantages that planning efficiency is high.
Object of the present invention is to what is be achieved through the following technical solutions.
The mechanical arm method for planning track disclosed by the invention for considering spacecraft ontology attitude motion, first at the beginning of spacecraft
Under beginning state, the change that the position of end effectors and posture are directed toward on mechanical arm is determined by quintic algebra curve paths planning method
Change track.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, compensates the motion planning of mechanical arm with this.By mechanical arm tail end
Former trajectory planning it is corresponding with compensation proposition summation, be denoted as the final mechanical arm tail end locus of points, obtained by kinematics Pseudoinverse algorithm
The motion profile of each joint angle of mechanical arm is obtained, and then realizes the mechanical arm trajectory planning for considering spacecraft ontology attitude motion.It should
Method has many advantages, such as to mitigate calculates pressure in real time, improves trajectory planning efficiency.
The mechanical arm method for planning track disclosed by the invention for considering spacecraft ontology attitude motion, includes the following steps:
Step 1: under spacecraft original state, end on mechanical arm is determined by quintic algebra curve paths planning method
The variation track that the position of actuator and posture are directed toward.
Under spacecraft original state, the centerbody body coordinate system f of the Servicing spacecraft of initial time is definedb0With inertial system
feIt is overlapped, centerbody body coordinate system fbIt moves and moves with centerbody.According to the initial shape of Servicing spacecraft centerbody and mechanical arm
State obtains initial position of the mechanical arm tail end actuator under this systemIt is directed toward with initial attitudePreset service space flight
Former desired locations under the maximum value of device centerbody and manipulator motion speed, spacecraft centerbody coordinate systemIt is expected with original
Posture is directed towardSince initial attitude is directed towardVector sum original it is expected that posture is directed towardIt is respectively provided in vector not completely independent
Three components, initial attitude be directed towardVector sum original it is expected that posture is directed towardVector passes through direction cosines expression, vector
Mould be 1, thus each the first two component of choosing is as control amount, but corresponding third component there are both positive and negatives can
Energy.For avoid it is described may cause uncertain there are both positive and negative, convert sky for the posture indicated by direction cosines direction
Between in two azimuth angle alphasnAnd αe, the single order and second time derivative of end effectors attitude are sought, then converted
For the single order and second time derivative of direction cosines.Define azimuth angle alphanRepresent posture directionWith this system XbObZbPlane it
Between angle, αeRepresent posture directionIn this system XbObZbProjection and this system O in planebZbAngle between axis.
According to azimuthal αnAnd αeDefinition, have
Wherein:Indicate that posture is directed towardIn second component.
It is directed toward based on postureSolve azimuth angle alphaeDuring, it, will be square in order to avoid denominator is the unusual of zero generation
Parallactic angle αeSolution procedure is defined as:
Wherein:Indicate that posture is directed towardIn second component,Indicate that posture is directed towardMiddle third point
Amount.
It is directed toward by end effectors initial attitudeAcquire initial azimuth αn_0And αe_0, pass through end effectors original
It is expected that posture is directed towardAcquire former expected orientation angle αn_r0And αe_r0.By end effectors original desired locationsWith former expectation side
Parallactic angle αn_r0And αe_r0With initial positionWith initial azimuth αn_0And αe_0It makes the difference, obtains the original position variation of end effectors
Amount and former azimuthal variation amount.
In quintic algebra curve planing method, difference and change rate according to variable in task whole story state are limited, and are found out
The undetermined coefficient of quintic algebra curve, and then obtain the planning amount change procedure smooth about time second order.By the expectation of planning amount θ
Value is denoted as θr, the value of original state is denoted as θ0, it is specified that the maximum value of first derivative is in Parameters variationSecond dervative
Maximum value isThen according to quintic algebra curve, have:
Wherein,Time coefficient τ is current time t and task duration tfRatio.
According to the characteristic of quintic algebra curve and planning quantitative change rate be limited etc., constraint conditions, task duration have following constraint:
It selects while meeting in formula (1.4) described condition the smallest one as the required by task shortest time, thus
To the most short task duration t for meeting each manipulator motion constraintf.Obtaining task duration tfLater, based on quintic algebra curve
Method, planning amount θ and its single order and second time derivative are as follows:
During mechanical arm trajectory planning, planning amount is the position of mechanical arm tail end actuatorIt is directed toward with posture
Mechanical arm tail end actuator position is acquired by the method for quintic algebra curve described in formula (1.5)The azimuth andWithIt is smooth
Continuous single order and second time derivative.
Due to the azimuth of mechanical arm tail end actuatorWithVariation be difficult to be write as the aobvious angle of angle containing joint of mechanical arm
The form of speed, and posture is directed towardTrack can then be write as the form of the aobvious angular speed of angle containing joint of mechanical arm, be conducive to machine
Tool shoulder joint trajectory planning.It therefore, will be mechanical before solving the joint angles characteristics of motion by mechanical arm inverse kinematics relationship
The azimuth of end effectors on armWithAnd its track is converted into posture directionVariation track.According to azimuthWithIt is directed toward with postureBetween geometrical relationship, have:
Posture is directed towardSingle order and second time derivative are asked, is had
Wherein α is azimuthWithThe matrix of composition, Φ are that azimuth angle alpha matrix and posture are directed towardBetween conversion
Matrix, Writing:
So far, it plans to obtain end effectors position under cartesian space by quintic algebra curve methodRefer to posture
ToSmooth continuous ideal single order and second time derivative matrixWith
Since posture is directed towardThree components in vector are not completely independent, enableIndicate that posture is directed towardIn vector
The first two component, is denoted asWherein:
The mechanical arm tail end actuator position that formula (1.8) acquiresIt is directed toward with postureSmooth continuous single order and two
Rank time-derivative matrixWithAs transported by the mechanical arm tail end actuator that quintic algebra curve planing method acquires
Move former desired locationsIt is directed toward with original expectation postureTrack.
Step 2: carrying out attitude motion planning according to spacecraft centerbody attitude motion demand, while seeking under this system
Former desired locationsIt is directed toward with postureThe variation track generated due to centerbody attitude motion advises the track of mechanical arm
It draws and compensates.
A certain desired locations in spacecraft centerbodyWith the inertial space position vector where itIn spacecraft
Relative position between inertial space position vector R where the heart constitution heart indicates, and is transformed into spacecraft sheet from inertial system
Conversion process under system are as follows:
Wherein: AbeIndicate inertial system to spacecraft this system coordinate conversion matrix,It indicates in spacecraft centerbody
A certain desired locationsIt is indicated under inertial system.
According to coordinate system rotation relationship, coordinate conversion matrix AbeChange rate and spacecraft centerbody rotational angular velocity ωb
Between relationship are as follows:
Then the relative motion first derivative and second dervative of former desired point position indicate are as follows:
A certain posture under inertial space is directed toward efeIt is transformed into the process of spacecraft this system are as follows:
efb=Abeefe (1.11)
According to the relationship between coordinate conversion matrix change rate and coordinate system velocity of rotation, the posture under spacecraft this system
It is directed toward efbSingle order and second time derivative are as follows:
Wherein: AbeIndicate inertial system to spacecraft this system coordinate conversion matrix,Indicate the rotation of spacecraft centerbody
Angular velocity omegabMultiplication cross matrix you, Writing:
Thus spacecraft centerbody is obtained in the case where attitude motion, a certain desired locations in inertial spaceAnd certain
One posture is directed toward efbThe single order and second time derivative changed under spacecraft centerbody coordinate system, is denoted as:
Wherein:Indicate posture pointing vector efbIn the first two component.
It is acquired in formula (1.13)WithIt is directed toward for position a certain in inertial space and posture due in spacecraft
Heart body attitude motion and the variation track generated, compensate according to trajectory planning of the variation track to mechanical arm.
Step 3: by step 1 summation corresponding with distal point motion profile obtained in step 2, using manipulator motion
The motion profile that Pseudoinverse algorithm solves each joint angle is learned, realizes the mechanical arm track rule for considering spacecraft ontology attitude motion situation
It draws.
By obtained in step 1 under spacecraft original state mechanical arm tail end point trackWithWith step
Spacecraft centerbody attitude motion obtained in rapid two and the compensation rate generatedWithCorresponding summation, is denoted as distal point
TrackWithInput as joint of mechanical arm trajectory planning:
Position based on end effectors and posture are directed toward the kinematic relation between mechanical arm system, by it is refined can
It is the movement under joint configuration space by conversion of motion of the mechanical arm in cartesian space than the mode that matrix pseudoinverse solves.
Matrix is directed toward with posture in the position of end effectors in cartesian space to be denoted asIt will be each
Section joint of mechanical arm angle is denoted as η, then has following relationship between position auto―control and joint angle first derivative:
Wherein J(η)For the Jacobian matrix at joint of mechanical arm angle.
According to formula (1.15), according to the obtained end locus of pointsWithWith known Mechanical transmission test relationship,
The reversed motion profile for solving joint of mechanical arm angle.Pseudo-inverse operation, the fortune of solution are carried out to Jacobian matrix in solution procedure
It is as follows to calculate formula:
Wherein
The ideal movements track of each joint angle of mechanical arm is obtained by formula (1.16)WithMechanical arm is set to navigate
Its device centerbody reach in the case where attitude motion original desired locations and posture direction in inertial space, and then realizes
Consider the mechanical arm trajectory planning of spacecraft ontology attitude motion.
It further include step 4: by each joint angle track of the mechanical arm planned in step 3WithController is passed to, is passed through
Controller realizes the motion control to mechanical arm.
Preferably, unusual situation is easy to appear during to improve Jacobian matrix pseudo-inverse operation, in operation
Robust adjustment item is added, is evaded to a certain extent using to sacrifice precision as cost unusual.Influence in robust adjustment item adjusts weight
The factor be denoted as λ, the value of λ is bigger, bigger to the adjustment effect of Jacobian matrix, at the same also imply that bring error compared with
Greatly.In pseudo-inverse operation expression formula (1.16) adjustedAre as follows:
Wherein In×nIndicate unit matrix, the end locus of points matrix of dimension and planningWithLine number it is identical.
The utility model has the advantages that
1, the mechanical arm method for planning track disclosed by the invention for considering spacecraft ontology attitude motion, in planning spacecraft
The variation that former inertial space desired locations and posture are directed toward under this system is sought while centerbody attitude motion, to mechanical arm
Motion planning compensates, so that mechanical arm remains to arrival inertial space Central Plains when centerbody carries out attitude motion and periodically hopes position
It sets and is directed toward with posture.
2, the mechanical arm method for planning track disclosed by the invention for considering spacecraft ontology attitude motion, by spacecraft center
The function influence of body attitude motion is in view of in the open loop motion planning of mechanical arm, it is not necessary to be directed toward by desired locations and posture
Metrical information plans the movement of mechanical arm in real time, and then mechanical arm track rule are reduced in the case where there is attitude motion of spacecraft conditions of demand
The calculating demand drawn mitigates the calculating pressure of the in-orbit real-time track planning of mechanical arm, improves planning efficiency.
3, the mechanical arm method for planning track disclosed by the invention for considering spacecraft ontology attitude motion, passes at two respectively
It has done in system method for solving and has a little supplemented.First, being directed toward by postureSolve azimuth angle alphaeWhen, in order to avoid denominator is zero
What is generated is unusual, increases a judgement in the azimuthal acquiring method of tradition, when posture is directed towardIn first and third point
It measures while defining azimuth angle alpha when being 0eIt is 0.Second, unusual to reduce the solution generation of joint of mechanical arm angle Jacobian matrix pseudoinverse
A possibility that, robust adjustment item is added in pseudoinverse solution procedure, is evaded to a certain extent using to sacrifice precision as cost unusual.
Detailed description of the invention
Fig. 1 is the process signal of the mechanical arm method for planning track disclosed by the invention for considering spacecraft ontology attitude motion
Figure.
Fig. 2 is end effectors posture pointing vector and azimuth schematic diagram.
Fig. 3 is that spacecraft centerbody carries out whether designing mechanical arm compensation campaign when attitude motion, with a certain fixed expectation
The comparison chart of position.Figure (a) is x-axis location tracking as a result, figure (b) is y-axis location tracking as a result, figure (c) is z-axis location tracking
As a result.
Fig. 4 is in the case of spacecraft centerbody progress attitude motion and when designing mechanical arm compensation campaign, mechanical arm is to sky
Between in certain fix the tracking result that position and posture are directed toward.Wherein figure (a) is between mechanical arm tail end point position and desired locations
Difference, figure (b) be mechanical arm tail end point posture be directed toward and it is expected posture be directed toward between difference.
Fig. 5 is during mechanical arm trajectory planning, whether the determinant of joint of mechanical arm angle Jacobian matrix is being supplemented
The comparison of robust adjustment item.
Specific embodiment
Objects and advantages in order to better illustrate the present invention with reference to the accompanying drawing do further summary of the invention with example
Explanation.
Embodiment 1:
The present embodiment, which is disclosed, carries out trajectory planning to the mechanical arm of five joint arm bar composition, for there is spacecraft centerbody
Attitude motion task situation is directed toward mechanical arm tail end position and posture and carries out trajectory planning and emulation.As shown in Figure 1, this reality
The mechanical arm method for planning track for applying the consideration spacecraft ontology attitude motion that example discloses, can be completed by following step:
Step 1: under spacecraft original state, end on mechanical arm is determined by quintic algebra curve paths planning method
The variation track that the position of actuator and posture are directed toward.
If the initial value of spacecraft and its mechanical arm relevant parameter of carrying is as shown in table 1.
1 spacecraft centerbody of table and mechanical arm initial parameter
Define azimuth angle alphanRepresent posture directionWith this system XbObZbAngle between plane, αeRepresent posture directionIn this system XbObZbProjection and this system O in planebZbAngle between axis.According to azimuthal αnAnd αeDefinition,
Have
Wherein:Indicate that posture is directed towardIn second component.
It is directed toward based on postureSolve azimuth angle alphaeDuring, it, will be square in order to avoid denominator is the unusual of zero generation
Parallactic angle αeSolution procedure is defined as:
Wherein:Indicate that posture is directed towardIn second component,Indicate that posture is directed towardMiddle third point
Amount.
According to data shown in table 1, mechanical arm tail end is obtained under spacecraft this system by spacecraft kinematic relation
Initial positionWith azimuthWithThe matrix azimuth matrix α of composition0Are as follows:
α0=[14.94 95.14]Tdeg
The difference that centerbody posture, mechanical arm tail end position and posture are directed toward between whole story state is as shown in table 2.
2 whole story state parameter difference of table
In quintic algebra curve planing method, difference and change rate according to variable in task whole story state are limited, and are found out
The undetermined coefficient of quintic algebra curve, and then obtain the planning amount change procedure smooth about time second order.By the expectation of planning amount θ
Value is denoted as θr, the value of original state is denoted as θ0, it is specified that the maximum value of first derivative is in Parameters variationSecond dervative
Maximum value isThen according to quintic algebra curve, have:
Wherein,Time coefficient τ is current time t and task duration tfRatio.
According to the characteristic of quintic algebra curve and variable under the task whole story state constraint conditions such as change rate, to task duration
There is following constraint:
The constraint conditions such as joint of mechanical arm movement are substituted into, are met the movement of spacecraft centerbody and each mechanical arm simultaneously
The most short task duration t of kinematic constraintfFor 57.5s.
Using the method for quintic algebra curve, to mechanical arm tail end positionAnd azimuth angle alphan、αeWith spacecraft centerbody posture
Movement is planned that planning amount and its single order and second dervative indicate are as follows:
According to azimuth angle alphanAnd αeIt is directed toward with postureBetween geometrical relationship, have:
Posture is directed towardSingle order and second time derivative are asked, is had
Wherein, Φ is that azimuth angle alpha matrix and posture are directed towardBetween transition matrix, Writing:
So far, it plans to obtain end effectors position under cartesian space by quintic algebra curve methodRefer to posture
ToSmooth continuous ideal single order and second time derivative matrixWith
Since posture is directed towardThree components in vector are not completely independent, enableIndicate that posture is directed towardIn vector
The first two component, is denoted asWherein:
The mechanical arm tail end actuator position acquiredIt is directed toward with postureSmooth continuous single order and second time derivative
MatrixWithThe mechanical arm tail end actuator as acquired by quintic algebra curve planing method moves to former expectation
PositionIt is directed toward with original expectation postureTrack.Legend as shown in Figure 3 is the solid line of " p1 ", is fixed only to carry out former space
Result of the position without compensatory tracking situation.
Step 2: attitude motion planning is carried out according to spacecraft centerbody attitude motion demand, while being sought under this system
Former desired locationsIt is directed toward with postureThe variation track generated due to centerbody attitude motion advises the track of mechanical arm
It draws and compensates.
A certain desired locations in spacecraft centerbodyWith the inertial space position vector where itIn spacecraft
Relative position between inertial space position vector R where the heart constitution heart indicates, and is transformed into spacecraft sheet from inertial system
Conversion process under system are as follows:
Wherein: AbeIndicate inertial system to spacecraft this system coordinate conversion matrix,Indicate certain in spacecraft centerbody
One desired locationsIt is indicated under inertial system.
According to coordinate system rotation relationship, coordinate conversion matrix AbeChange rate and spacecraft centerbody rotational angular velocity ωb
Between relationship are as follows:
Then the relative motion first derivative and second dervative of former desired point position indicate are as follows:
A certain posture under inertial space is directed toward efeIt is transformed into the process of spacecraft this system are as follows:
efb=Abeefe
According to the relationship between coordinate conversion matrix change rate and coordinate system velocity of rotation, the posture under spacecraft this system
It is directed toward efbSingle order and second time derivative are as follows:
Wherein: AbeIndicate coordinate conversion matrix of the inertial system to spacecraft this system, ωbIndicate the rotation of spacecraft centerbody
Angular speed, above tilde be multiplication cross matrix label,Writing:
Thus spacecraft centerbody is obtained in the case where attitude motion, a certain desired locations in inertial spaceAnd certain
One posture is directed toward efbThe single order and second time derivative changed under spacecraft centerbody coordinate system, is denoted as:
Wherein:Indicate posture pointing vector efbIn the first two component.
WithFor position a certain in inertial space and posture are directed toward due to spacecraft centerbody attitude motion
The variation track of generation is compensated according to trajectory planning of the variation track to mechanical arm.
Step 3: by step 1 summation corresponding with distal point motion profile obtained in step 2, using manipulator motion
The motion profile that Pseudoinverse algorithm solves each joint angle is learned, realizes the mechanical arm track rule for considering spacecraft ontology attitude motion situation
It draws.
By obtained in step 1 under spacecraft original state mechanical arm tail end point trackWithWith step
Spacecraft centerbody attitude motion obtained in rapid two and the compensation rate generatedWithCorresponding summation, is denoted as distal point
TrackWithInput as joint of mechanical arm trajectory planning:
Position based on end effectors and posture are directed toward the kinematic relation between mechanical arm system, by it is refined can
It is the movement under joint configuration space by conversion of motion of the mechanical arm in cartesian space than the mode that matrix pseudoinverse solves.
Matrix is directed toward with posture in the position of end effectors in cartesian space to be denoted asBy each section mechanical arm
Joint angle is denoted as η=[θ1 θ2 θ3 θ4 θ5]T, then there is following relationship between position auto―control and joint angle first derivative:
Wherein J(η)For the Jacobian matrix at joint of mechanical arm angle:
A in formulabjCoordinate conversion matrix for jth section mechanical arm coordinate system to this system, ΓjFor jth section mechanical arm shaft
Direction matrix, ejtPosture pointing vector, r between jth section mechanical arm and end effectorsjtFor jth section mechanical arm mass center to end
Hold the vector of actuator mass center.
In order to make the planning demand that mechanical arm number of degrees of freedom meets position and posture is directed toward, have mechanical arm tail end joint
There are three freedom degrees.In order to determine that the posture of end effectors is directed toward, turning for the relatively upper section mechanical arm of end effectors is defined
Under " 3-1-2 " rotation mode in relationship and attitude motion of spacecraft between dynamic angular speed and its joint angles angular speed and
Relationship between attitude angle is identical.End effectors coordinate system f5With upper section mechanical arm coordinate system f4Between transition matrix be A45
=(Ay(θ(2))Ax(θ(1))Az(θ(3)))T, θ (k) expression k-th of component of attitude angle θ.
According to the obtained end locus of pointsWithIt is reversed to solve machinery with known Mechanical transmission test relationship
The motion profile at shoulder joint angle.Pseudo-inverse operation is carried out to Jacobian matrix in solution procedure, the operational formula of solution is as follows:
Wherein
The ideal movements track of each joint angle of obtained mechanical armWithIt can be realized and consider spacecraft ontology posture fortune
Dynamic mechanical arm trajectory planning enables mechanical arm to reach inertial space in the case where spacecraft centerbody carries out attitude motion
In original desired locations and posture be directed toward.Legend as shown in Figure 3 is the dotted line of " p12 ", for according to centerbody attitude motion into
The compensation of row manipulator motion as a result, difference when significantly reducing without compensation proposition between expectation target, is improved
Planning precision.It is illustrated in figure 4 the mechanical arm trajectory planning and desired locations and posture for considering spacecraft ontology attitude motion
Difference between direction finally converges to zero, and explanation can reach desired value.
It further include step 4: by each joint angle track of the mechanical arm planned in step 3WithController is passed to, is led to
Cross motion control of the controller realization to mechanical arm.
Preferably, unusual situation is easy to appear during to improve Jacobian matrix pseudo-inverse operation, in operation
Robust adjustment item is added, is evaded to a certain extent using to sacrifice precision as cost unusual.Influence in robust adjustment item adjusts weight
The factor be denoted as λ, the value of λ is bigger, bigger to the adjustment effect of Jacobian matrix, at the same also imply that bring error compared with
Greatly.In modified pseudo-inverse operation expression formulaAre as follows:
I in formula5×5Indicate the unit matrix of five dimensions.
Regulatory factor λ=10 are taken in the present embodiment-3.It is illustrated in figure 5 and whether adds robust adjustment item to Jacobian matrix
The influence of determinant is more easy to produce unusual closer to zero.
Under the method for planning track of above-mentioned mechanical arm, mechanical arm tail end can carry out certain posture in spacecraft centerbody
It reaches the desired locations of a certain fixation and posture in inertial space in the case where movement to be directed toward, it is not necessary to rely on desired locations and posture
The metrical information of direction plans the movement of mechanical arm in real time, mitigates the calculating pressure of the in-orbit real-time track planning of mechanical arm, improves
Planning efficiency.
The purpose of invention, technical scheme and beneficial effects are further elaborated in above-described specific descriptions,
It should be understood that the above is only a specific embodiment of the present invention, it is not intended to limit the scope of protection of the present invention,
All within the spirits and principles of the present invention, any modification, equivalent substitution, improvement and etc. done should be included in of the invention
Within protection scope.
Claims (6)
1. considering the mechanical arm method for planning track of spacecraft ontology attitude motion, it is characterised in that: include the following steps,
Step 1: under spacecraft original state, mechanical arm tail end actuator is determined by quintic algebra curve paths planning method
Position and posture be directed toward variation track;
Step 2: carrying out attitude motion planning according to spacecraft centerbody attitude motion demand, while seeking the former phase under this system
Hope positionIt is directed toward with postureThe variation track generated due to centerbody attitude motion, to the trajectory planning of mechanical arm into
Row compensation;
Step 3: by step 1 summation corresponding with distal point motion profile obtained in step 2, using Mechanical transmission test puppet
Algorithm for inversion solves the motion profile of each joint angle, realizes the mechanical arm trajectory planning for considering spacecraft ontology attitude motion situation.
2. considering the mechanical arm method for planning track of spacecraft ontology attitude motion as described in claim 1, it is characterised in that:
It further include step 4, by each joint angle track of the mechanical arm planned in step 3WithController is passed to, controller is passed through
Realize the motion control to mechanical arm.
3. considering the mechanical arm method for planning track of spacecraft ontology attitude motion as claimed in claim 1 or 2, feature exists
It is in: step 1 concrete methods of realizing,
Under spacecraft original state, the centerbody body coordinate system f of the Servicing spacecraft of initial time is definedb0With inertial system feWeight
It closes, centerbody body coordinate system fbIt moves and moves with centerbody;According to the original state of Servicing spacecraft centerbody and mechanical arm,
Obtain initial position of the mechanical arm tail end under this systemIt is directed toward with initial attitudePreset service spacecraft centerbody with
Former desired locations under the maximum value of manipulator motion speed, spacecraft centerbody coordinate systemIt is directed toward with original expectation posture
Since initial attitude is directed towardVector sum original it is expected that posture is directed towardIt is indicated by direction cosines in vector and the mould of vector is equal
It is 1, so that initial attitude is directed towardVector sum original it is expected that posture is directed towardThree not completely independent components are respectively provided with, because
And the first two component is respectively chosen as variable, but there are both positive and negative possibility for corresponding third component;To avoid described deposit
It may cause two uncertain, to convert the posture indicated by direction cosines direction in space azimuths in both positive and negative
αnAnd αe, the single order and second time derivative of end effectors attitude are sought, then be translated into the single order of direction cosines
And second time derivative;Define azimuth angle alphanRepresent posture directionWith this system XbObZbAngle between plane, αeRepresent appearance
State is directed towardIn this system XbObZbProjection and this system O in planebZbAngle between axis;
According to azimuthal αnAnd αeDefinition, have
Wherein:Indicate that posture is directed towardIn second component;
It is directed toward based on postureSolve azimuth angle alphaeDuring, in order to avoid denominator is the unusual of zero generation, by azimuth angle alphae
Solution procedure is defined as:
Wherein:Indicate that posture is directed towardIn second component,Indicate that posture is directed towardMiddle third component;
It is directed toward by end effectors initial attitudeAcquire initial azimuth αn_0And αe_0, appearance it is expected by end effectors original
State is directed towardAcquire former expected orientation angle αn_r0And αe_r0;By end effectors original desired locationsWith former expected orientation angle αn_r0
And αe_r0With initial positionWith initial azimuth αn_0And αe_0It makes the difference, obtains original position variable quantity and the original side of end effectors
Parallactic angle variable quantity;
In quintic algebra curve planing method, difference and change rate according to variable in task whole story state are limited, and are found out five times
Polynomial undetermined coefficient, and then obtain the planning amount change procedure smooth about time second order;The desired value of planning amount θ is remembered
For θr, the value of original state is denoted as θ0, it is specified that the maximum value of first derivative is in Parameters variationThe maximum of second dervative
Value isThen according to quintic algebra curve, have:
Wherein,Time coefficient τ is current time t and task duration tfRatio;
According to the characteristic of quintic algebra curve and planning quantitative change rate be limited etc., constraint conditions, task duration have following constraint:
It selects while meeting in formula (0.4) described condition the smallest one as the required by task shortest time, thus expired
The most short task duration t of each manipulator motion constraint of footf;Obtaining task duration tfLater, based on the side of quintic algebra curve
Method, planning amount θ and its single order and second time derivative are as follows:
During mechanical arm trajectory planning, planning amount is the position of mechanical arm tail end actuatorIt is directed toward with postureBy formula
(0.5) method of the quintic algebra curve described in acquires mechanical arm tail end actuator positionThe azimuth andWithIt is smooth continuous
Single order and second time derivative.
Due to the azimuth of mechanical arm tail end actuatorWithVariation be difficult to be write as the aobvious angular speed of angle containing joint of mechanical arm
Form, and posture is directed towardTrack can then be write as the form of the aobvious angular speed of angle containing joint of mechanical arm, be conducive to mechanical arm pass
Save trajectory planning;It therefore, will be last on mechanical arm before solving the joint angles characteristics of motion by mechanical arm inverse kinematics relationship
Hold the azimuth of actuatorWithAnd its track is converted into posture directionVariation track;According to azimuthWith
It is directed toward with postureBetween geometrical relationship, have:
Posture is directed towardSingle order and second time derivative are asked, is had
Wherein α is azimuthWithThe matrix of composition, Φ are that azimuth angle alpha matrix and posture are directed towardBetween conversion square
Battle array, Writing:
So far, it plans to obtain end effectors position under cartesian space by quintic algebra curve methodIt is directed toward with posture
Smooth continuous ideal single order and second time derivative matrixWith
Since posture is directed towardThree components in vector are not completely independent, enableIndicate that posture is directed towardPreceding two in vector
A component, is denoted asWherein:
The mechanical arm tail end actuator position that formula (0.8) acquiresIt is directed toward with postureWhen smooth continuous single order and second order
Between Jacobian matrixWithThe mechanical arm tail end actuator as acquired by quintic algebra curve planing method moves to
Former desired locationsIt is directed toward with original expectation postureTrack.
4. considering the mechanical arm method for planning track of spacecraft ontology attitude motion as claimed in claim 3, it is characterised in that:
Step 2 concrete methods of realizing is,
A certain desired locations in spacecraft centerbodyWith the inertial space position vector where itWith spacecraft centerbody
Relative position between inertial space position vector R where mass center indicates, and is transformed into spacecraft this system from inertial system
Under conversion process are as follows:
Wherein: AbeIndicate inertial system to spacecraft this system coordinate conversion matrix,Indicate a certain phase in spacecraft centerbody
Hope positionIt is indicated under inertial system;
According to coordinate system rotation relationship, coordinate conversion matrix AbeChange rate and spacecraft centerbody rotational angular velocity ωbBetween
Relationship are as follows:
Then the relative motion first derivative and second dervative of former desired point position indicate are as follows:
A certain posture under inertial space is directed toward efeIt is transformed into the process of spacecraft this system are as follows:
efb=Abeefe (0.11)
According to the relationship between coordinate conversion matrix change rate and coordinate system velocity of rotation, the posture is directed toward under spacecraft this system
efbSingle order and second time derivative are as follows:
Wherein: AbeIndicate inertial system to spacecraft this system coordinate conversion matrix,Indicate spacecraft centerbody angle of rotation speed
Spend ωbMultiplication cross matrix you, Writing:
Thus spacecraft centerbody is obtained in the case where attitude motion, a certain desired locations in inertial spaceWith a certain appearance
State is directed toward efbThe single order and second time derivative changed under spacecraft centerbody coordinate system, is denoted as:
Wherein:Indicate posture pointing vector efbIn the first two component;
It is acquired in formula (0.13)WithIt is directed toward for position a certain in inertial space and posture due to spacecraft centerbody
Attitude motion and the variation track generated, compensate according to trajectory planning of the variation track to mechanical arm.
5. considering the mechanical arm method for planning track of spacecraft ontology attitude motion as claimed in claim 4, it is characterised in that:
Step 3 concrete methods of realizing is,
By obtained in step 1 under spacecraft original state mechanical arm tail end point trackWithWith step 2
Obtained in spacecraft centerbody attitude motion and the compensation rate that generatesWithCorresponding summation, is denoted as the end locus of pointsWithInput as joint of mechanical arm trajectory planning:
Position based on end effectors and posture are directed toward the kinematic relation between mechanical arm system, by Jacobean matrix
Conversion of motion of the mechanical arm in cartesian space is the movement under joint configuration space by the mode that battle array pseudoinverse solves;
Matrix is directed toward with posture in the position of end effectors in cartesian space to be denoted asEach section is mechanical
Shoulder joint angle is denoted as η, then has following relationship between position auto―control and joint angle first derivative:
Wherein J(η)For the Jacobian matrix at joint of mechanical arm angle;
According to formula (0.15), according to the obtained end locus of pointsWithWith known Mechanical transmission test relationship, reversely
Solve the motion profile at joint of mechanical arm angle;Pseudo-inverse operation is carried out to Jacobian matrix in solution procedure, the operation of solution is public
Formula is as follows:
Wherein
The ideal movements track of each joint angle of mechanical arm is obtained by formula (0.16)WithEnable mechanical arm in spacecraft
Centerbody reach in the case where attitude motion original desired locations and posture direction in inertial space, and then realizes and consider
The mechanical arm trajectory planning of spacecraft ontology attitude motion.
6. considering the mechanical arm method for planning track of spacecraft ontology attitude motion as claimed in claim 5, it is characterised in that:
It is easy to appear unusual situation during to improve Jacobian matrix pseudo-inverse operation, robust adjustment item is added in operation, with
Sacrifice precision is evaded unusual to a certain extent for cost;The factor that influence in robust adjustment item adjusts weight is denoted as λ, the value of λ
It is bigger, it is bigger to the adjustment effect of Jacobian matrix, while also implying that bring error is larger;Pseudo-inverse operation adjusted
In expression formula (0.16)Are as follows:
Wherein In×nIndicate unit matrix, the end locus of points matrix of dimension and planningWithLine number it is identical.
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 | |
CN108241339B (en) | Motion solving and configuration control method of humanoid mechanical arm | |
CN109623812A (en) | Consider the mechanical arm method for planning track of spacecraft ontology attitude motion | |
CN106945020B (en) | A kind of space double mechanical arms system motion control method for coordinating | |
CN108381553B (en) | Relative navigation close-range tracking method and system for space non-cooperative target capture | |
CN109782601B (en) | Design method of self-adaptive neural network synchronous robust controller of coordinated mechanical arm | |
CN109048890A (en) | Coordination method for controlling trajectory, system, equipment and storage medium based on robot | |
CN110405762B (en) | Biped robot attitude control method based on spatial second-order inverted pendulum model | |
CN111538949A (en) | Redundant robot inverse kinematics solving method and device and redundant robot | |
CN108326852A (en) | A kind of space manipulator method for planning track of multiple-objection optimization | |
CN108068113B (en) | 7-DOF humanoid arm flying object operation minimum acceleration trajectory optimization | |
Nagarajan et al. | Planning in high-dimensional shape space for a single-wheeled balancing mobile robot with arms | |
CN110053044B (en) | Model-free self-adaptive smooth sliding mode impedance control method for clamping serial fruits by parallel robot | |
CN106708078B (en) | A kind of rapid posture antihunt means under actuator failures suitable for robot for space | |
CN110154024B (en) | Assembly control method based on long-term and short-term memory neural network incremental model | |
CN112809666B (en) | 5-DOF mechanical arm strength position tracking algorithm based on neural network | |
CN111506095A (en) | Method for tracking and controlling relative pose of saturation fixed time between double rigid body feature points | |
CN114055467B (en) | Space pose online simulation system based on five-degree-of-freedom robot | |
Zhao et al. | Minimum base disturbance control of free-floating space robot during visual servoing pre-capturing process | |
CN108664040A (en) | The attitude angle control method of 3-freedom parallel mechanism | |
CN112847373B (en) | Robot track synchronous control method and computer readable storage medium | |
CN109015657A (en) | A kind of final state network optimized approach towards redundant mechanical arm repeating motion planning | |
JP4133381B2 (en) | Space robot attitude control method and apparatus | |
CN116540721A (en) | Space robot optimal track planning method based on improved genetic particle swarm algorithm | |
CN115122327A (en) | Method for accurately positioning tail end of dangerous chemical transport mechanical arm based on dual neural network |
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 |