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 PDF

Info

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
Application number
CN201811470615.5A
Other languages
Chinese (zh)
Other versions
CN109623812B (en
Inventor
藏悦
张尧
张景瑞
王典军
经姚翔
陈士明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN201811470615.5A priority Critical patent/CN109623812B/en
Publication of CN109623812A publication Critical patent/CN109623812A/en
Application granted granted Critical
Publication of CN109623812B publication Critical patent/CN109623812B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1656Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664Programme 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

Consider the mechanical arm method for planning track of spacecraft ontology attitude motion
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.
CN201811470615.5A 2018-12-04 2018-12-04 Mechanical arm trajectory planning method considering spacecraft body attitude motion Active CN109623812B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (6)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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