CN103009389A - Track planning method of redundant space mechanical arm for on-track catching - Google Patents

Abstract

The invention relates to a track planning method of a redundant space mechanical arm for on-track catching. The catching operation of the space mechanical arm is divided into a rough catching stage and a fine catching stage, so that problems are simplified. By aiming at outstanding problems of different stages, different planning algorithms are adopted. In the rough catching stage, a track searching algorithm based on configuration space is adopted, and the algorithm is based on the test in the configuration space; and in the fine catching stage, a pseudo-inverse algorithm based on kinematics is adopted, and the final precise catching is guaranteed by compensating the relative movement of a target.

Description

The method for planning track that a kind of redundant space mechanical arm is arrested at rail

Technical field

The present invention relates to the method for planning track that a kind of redundant space mechanical arm is arrested at rail, belong to the Spacecraft Attitude Control field.

Background technology

Space manipulator is arrested in the operation at rail, for fuel saving, the position of space report control platform is not controlled, only the attitude of space report control platform is stablized control, belong to the controlled free flying robot of attitude, each joint angle that only has mechanical arm that therefore in the variable of system model, can be controlled arbitrarily.The basic demand of trajectory planning namely is that position and the attitude of the end effector expected is oriented to target, come to determine by certain algorithm position, angle, angular speed and the angular acceleration track of each joint angle, thereby realized the operation of arresting of mechanical arm by controller.Here redundancy refers to, mechanical arm has 7 frees degree, except can so that end effector arrive the position and attitude of appointment, also have the unnecessary free degree and be used for evading collision with barrier, can adopt simultaneously that a plurality of mechanical arms are collaborative to be controlled, so that change flexibly end effector, firmly arrest safely and reliably noncooperative target.

If mechanical arm bumps with barrier in arresting operation, then the space report control platform is subject to the effect of unknown reaction force, whole system is brought unknown disturbances power and disturbance torque, make platform unforeseen motion occur, destroy the attitude stabilization of platform and the motion stabilization of mechanical arm, not only can cause arresting the failure of operation, also may cause uncertain infringement to the space report control platform.Therefore, evading collision is that space manipulator is arrested the factor that must consider in the operation trace planning.In addition, the compensation problem of the singular problem of mechanical arm and target relative motion also is the problem that must consider in the trajectory planning.

The trajectory planning algorithm of mechanical arm mainly contains Pseudoinverse algorithm, Artificial Potential Field Method and based on the free-space Method of configuration space.Pseudoinverse algorithm is prone to unusual in finding the solution.There is the local minimum problem in Artificial Potential Field Method, and for challenge, amount of calculation is excessive, is applicable to mobile robot's trajectory planning.In the configuration space, the coordinate of configuration point is each joint angle of mechanical arm.The device object pose be will act on and barrier is mapped to configuration space by working space by kinematic relation, target configuration point and configuration space obstacle obtained.Collision-free Trajectory Planning namely will search out in configuration space and connect initial and target configuration point and without the path of obstacle.The method that exists at present is demand solution configuration space all, and very large for multi freedom degree mechanical arm amount of calculation, the configuration space obstacle can only obtain by discretization, need take a large amount of internal memories for improving precision.This causes existing algorithm can't be applied to the space manipulator of computational resource anxiety, is difficult to be applied to space mission.

Summary of the invention

Technical problem to be solved by this invention is: overcome the deficiencies in the prior art, and the method for planning track that provides a kind of collisionless to arrest at rail without unusual redundant space mechanical arm, the method engineering construction is simple, can save computational resource.

Technical scheme of the present invention is:

The method for planning track that a kind of redundant space mechanical arm is arrested at rail; Described redundant space mechanical arm is installed on the report control platform of space, and mechanical arm tail end is installed end effector; The space report control platform is equipped with vision measurement sensor and spaceborne computer; The redundant space mechanical arm rail arrest to as if the space non-cooperative target; The operation of arresting of space manipulator is divided into the roughly section of arresting and the meticulous section of arresting,

The method for planning track of the section of arresting comprises the steps: roughly

(1.1) by roughly desired locations and the attitude of the section of arresting end effector, utilize collisionless target configuration derivation algorithm to try to achieve collisionless target configuration;

(1.2) according to the current configuration of mechanical arm and target configuration, utilize the collisionless path search algorithm, try to achieve the path in the collisionless configuration space;

(1.3) utilize path optimization's algorithm, the path in the collisionless configuration space is optimized the motion sequence of each joint angle of the mechanical arm that acquisition do not collide with barrier;

The method for planning track of the meticulous section of arresting comprises the steps:

(2.1) designed in advance the movement locus of end effector position/attitude by the position/attitude of the end effector position/attitude of capture target of expectation and current end effector, speed track and acceleration trajectory;

(2.2) the target relative motion that measures according to the vision measurement sensor is to the movement locus of described end effector position/attitude, speed track and acceleration trajectory compensate the movement locus of the end effector position/attitude after obtaining to compensate, speed track and acceleration trajectory;

(2.3) according to the movement locus of the end effector position/attitude after the compensation, the Pseudoinverse algorithm that speed track and acceleration trajectory adopt based on motion to learn obtains angular speed track and the angular acceleration track of each joint angle of mechanical arm.

In the described step (1.1), collisionless target configuration derivation algorithm is as follows:

At first calculate the target configuration according to target configuration derivation algorithm;

Target configuration derivation algorithm be input as each current joint angle of mechanical arm, and position and the attitude of the required end effector of capture target, target configuration derivation algorithm utilizes numerical integration method to find the solution mechanical arm inverse kinematics equation and obtains each required joint angle of capture target, i.e. target configuration;

Then utilize collision detection algorithm to judge whether the target configuration that calculates collides with barrier, if collide with barrier, then the current joint angle of mechanical arm that inputs to target configuration derivation algorithm is adjusted, re-start calculating according to target configuration derivation algorithm, until obtain collisionless target configuration.

The collisionless path search algorithm is divided into N part with the total angle of rotation of each joint angle from current configuration to the target configuration in the described step (1.2), each joint angle is turned over every part of corner be defined as a son motion, thereby formed pN son motion, wherein p is the joint number;

The son motion of remembering i joint angle is son motion i, i=1, and 2 ..., P, the collisionless path search algorithm with suitable arranged sequentially, forms the motion sequence that a pN goes on foot with pN son motion;

Set serial number gencration function F (i), this function generates the sub-motion sequence number that next step will be soundd out by current sub-motion sequence number i; F (i) is for increasing progressively, successively decrease or generating function at random;

With m represent the step number, T represents to sound out total degree, T _MaxExpression can be accepted the maximum number of times of souning out;

Collisionless path search algorithm step is as follows:

1) initialize, make m=1, T=1 chooses i, and T is set _Max

2) judge whether m equals pN+1;

If m=pN+1 then exports collisionless joint motions sequence, and finish searching algorithm; Otherwise utilize collision detection algorithm, judge whether mechanical arm m step execution motion i collides with barrier; If do not collide, mechanical arm is carried out and is given motion i, and keeps a record execution in step 3); If collision, execution in step 4);

3) m=m+1, i=F (i), T=T+1, execution in step 6);

4) judge whether m step soundd out all possible son motion i, if, execution in step 5); Otherwise i=F (i), T=T+I, execution in step 6);

5) in the rebound m-1 step, make m=m-1; Judge whether m step soundd out all possible son motion, if re-execute step 5); Otherwise make i=F (i (m)), i (m) refers to the original sub-motion sequence number that adopts of m step, T=T+1, execution in step 6);

6) judge whether T equals T _Max,

If T=T _Max, then output algorithm is searched for failure and is finished searching algorithm, otherwise execution in step 2);

If the algorithm search failure is then adopted different exploration rule F (i) or increase path to cut apart constant N and is re-executed the collisionless path search algorithm.

Whether path optimization's algorithmic method is as follows in the described step (1.3): two adjacent son motions are merged into a son motion in the motion sequence of each joint angle of the collisionless mechanical arm that the collisionless path search algorithm is searched, then adopt the son motion behind the collision detection algorithm combining data detection to collide with barrier; If do not collide, then these two motions are merged; If colliding then, cancellation merges; Adjacent son motion repeats said process, until can't further merge in the motion sequence that obtains.

Described collision detection algorithm method is as follows: the line segment that mechanical arm is reduced to some connections, then carry out the discretization on the space, the discrete a series of point that turns on the mechanical arm, barrier is merotomized, every part finds the envelope of a convex polyhedron, then with the approximate barrier that replaces of these convex polyhedrons itself; The central point of remembering i face of certain convex polyhedron is P _i, the exterior normal vector of i face is

Remember that point to be detected is P _T, then put P _TThe sufficient and necessary condition that is positioned at this convex polyhedron is: all faces for convex polyhedron have If exist measuring point to be checked inner at barrier, think that then mechanical arm and barrier collide, otherwise think that mechanical arm and barrier can not bump.

The angular speed track of each joint angle of the mechanical arm in the described step (2.3)

Formula is:

${\stackrel{\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

Wherein, X _Er(t) and

Be movement locus and the speed track of the end effector position/attitude after the compensation, X _E(t) be the position/attitude of end effector in the mechanical arm actual motion process; δ (X _Er(t)-X _E(t)) regulate item, wherein δ＞0 for the ratio that adds; J ⁺=J ^T(JJ ^T) ^-1Pseudoinverse for Jacobian matrix J;

The joint angle acceleration trajectory of each joint angle of described mechanical arm

For:

${\stackrel{\·\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·({\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)-{\stackrel{\·}{X}}_E\left(t\right))]+{\stackrel{\·}{J}}^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

Wherein,

Acceleration trajectory for the end effector position/attitude after the compensation;

Time-derivative for the pseudoinverse of Jacobian matrix J.

The method for planning track of the described meticulous section of arresting also comprises the steps: to utilize the redundant degree of freedom of mechanical arm, adopts idle running to make mechanical arm away from singular configuration.

The angular speed track of each joint angle of the mechanical arm of band idle running is

${{\stackrel{\·}{q}}_r}^{\′}={\stackrel{\·}{q}}_r\left(t\right)+P_{J}y,$

P _J=(I-J ^T(JJ ^T) ^-1J) be the orthogonal intersection cast shadow matrix of Jacobian matrix J; Column vector y=α v;

ξ (q) is unusual tolerance: ξ (q)=det (JJ ^T); Scalar parameter α is chosen for:

Subscript ". " represents derivative operation; The transposition computing is asked in subscript " T " representative; Subscript " 1 " represents inversion operation;

Represent scalar ξ vector q is asked gradient.

The present invention's beneficial effect compared with prior art is:

(1) the present invention a kind ofly brand-new arrests the collisionless of operation without unusual trajectory planning algorithm for the spatial redundancy mechanical arm at rail.Be divided into the roughly section of arresting and the meticulous section of arresting by the operation of arresting with space manipulator, problem is simplified.For the outstanding problem of different phase, adopted different planning algorithms.Adopt track search algorithm based on configuration space in the rough section of arresting, this algorithm is based on the exploration in the configuration space; Pseudoinverse algorithm in that the meticulous section of arresting adopts based on motion to learn guarantees final accurately arresting by the compensation to the target relative motion.

(2) the method by the track search algorithm based on configuration space, has realized evading barrier in the rough section of arresting.Configuration space is the abstract space of a higher-dimension take each joint angle of mechanical arm as coordinate.This track search algorithm based in configuration space, souning out, do not need to find the solution configuration space, just can in configuration space, find out the collisionless path by the collision detection exploration, compared with prior art, computational resource and internal memory have been saved, be applicable to the quick planning of track of the space manipulator of computational resource anxiety, using for aerospace engineering provides a kind of simple effective method.By the track optimizing algorithm, the final track that obtains is comparatively level and smooth, is easy to Project Realization.

(3) the method is at the Pseudoinverse algorithm of the meticulous section of arresting by based on motion of adding target relative motion compensation, can be by the metrical information of the vision sensor on the report control platform of space, obtain the relative motion of target, the relative motion of target is incorporated in the Pseudoinverse algorithm, realizes the compensation to the target relative motion.In addition, in Pseudoinverse algorithm, additional proportion is regulated, and the tracking error of track is compensated, and increases antijamming capability and the robustness of system, has guaranteed that in the meticulous section of arresting, mechanical arm is accurately arrested target.

(4) related methods engineering is implemented simply among the present invention, and popularizing application prospect is good.The method has not only satisfied the basic demand of space manipulator collisionless without unusual trajectory planning, and algorithm is simply easy to implement, save computing time, save computational resource and internal memory, be specially adapted to the quick trajectory planning of the space manipulator of computational resource anxiety.The method is particularly useful for space manipulator to the arresting fast of noncooperative target, also is applicable to the collision-free Trajectory Planning of mechanical arm on the lunar rover.

Description of drawings

Fig. 1 is that space of the present invention report control platform is arrested the extraterrestrial target schematic diagram;

Fig. 2 is that collisionless of the present invention is without the schematic flow sheet of unusual mechanical arm trajectory planning algorithm;

Fig. 3 is the schematic flow sheet of collisionless path search algorithm among the present invention;

Fig. 4 is initial configuration and the rotating shaft positive direction of 7 degree-of-freedom manipulators among the present invention;

Fig. 5 is the level and smooth front collisionless path of algorithm among the present invention;

Fig. 6 is the level and smooth front joint angle speed of algorithm among the present invention; Wherein curve 11,12,13 is respectively the angular velocity varies curve in the 1st, 2,3 joints among Fig. 6 (a); Curve 14,15,16 is respectively the angular velocity varies curve in the 4th, 5,6 joints among Fig. 6 (b);

Fig. 7 is the collisionless path of algorithm after level and smooth among the present invention;

Fig. 8 is the joint angle speed of algorithm after level and smooth among the present invention; Wherein curve 21,22,23 is respectively the angular velocity varies curve in the 1st, 2,3 joints among Fig. 8 (a); Curve 24,25,26 is respectively the angular velocity varies curve in the 4th, 5,6 joints among Fig. 8 (b).

The specific embodiment

The invention provides collisionless that a kind of redundant space mechanical arm arrests at rail without unusual method for planning track, as shown in Figure 1, the equipment that method is relied on comprises space report control platform 1 and is installed in mechanical arm system 3 on the report control platform of space; The space report control platform is equipped with vision measurement sensor and spaceborne computer; Mechanical arm tail end is equipped with end effector; Mechanical arm arrest to as if space non-cooperative target 2.For the spatial operation platform generally can refer to mechanical arm at the rail controllable satellite, the space non-cooperative target refers generally to the uncontrollable satellite in space etc.Mechanical arm system 3 comprises n section mechanical arm, and every mechanical arm comprises p joint 4, begins to be designated as successively the 1st joint from the space report control platform, and end effector 5 is installed in the 2nd joint ... p joint on the p joint.The collisionless of mechanical arm system 3 capture targets that is provided by the invention is without unusual method for planning track.

For example mechanical arm system 3 can comprise 3 mechanical arms, and each mechanical arm has 7 frees degree.

Below in conjunction with accompanying drawing 2 and example the present invention is elaborated.As shown in Figure 2 space manipulator whole arrested operation and be divided into the roughly section of arresting and the meticulous section of arresting.The trajectory planning problem of space manipulator is reduced to the trajectory planning problem of fixed pedestal mechanical arm in the rough section of arresting, employing is based on the track search algorithm of configuration space, end effector is moved near the position of capture target and the state of attitude, with the difference of the position of final capture target and attitude and roughly the section of arresting revised by the meticulous section of arresting in the lump by the error that the target relative motion causes.And the meticulous section of arresting operational ton is very little, and the target relative motion is not obvious, and the Pseudoinverse algorithm that adopts simultaneously based on motion to learn carries out trajectory planning, and real-Time Compensation is carried out in the relative motion to target in algorithm, and final realization is accurately arrested.

One, the rough trajectory planning of the section of arresting

Specifically comprise the steps:

(1.1) by roughly desired locations and the attitude of the section of arresting end effector, utilize collisionless target configuration derivation algorithm to try to achieve collisionless target configuration;

The desired locations of the section of arresting end effector and attitude are hoped position and the attitude that reaches for the rough section of arresting tail end roughly, namely position and the attitude of the required end effector of capture target.

Described collisionless target configuration derivation algorithm at first calculates the target configuration according to target configuration derivation algorithm;

Target configuration derivation algorithm be input as each current joint angle of mechanical arm, i.e. position and the attitude of the end effector that current mechanical arm configuration, and capture target is required are output as each required joint angle of capture target, i.e. the target configuration.The method that adopts is for utilizing numerical integration method (such as 4 rank Runge-Kutta methods) to find the solution mechanical arm inverse kinematics equation

$\stackrel{\·}{q}\left(t\right)=J^{+}\left(q\right){\stackrel{\·}{X}}_E\left(t\right).$

The initial joint angle of inverse kinematics equation integration is each current joint angle of mechanical arm, and the end value of integration is the output of algorithm: each joint angle that capture target is required, i.e. target configuration.In equation, X _EBe mechanical arm tail end effect device position/orientation vector;

For by position and the attitude of the required end effector of the position of current end effector and attitude, capture target and arrest the mechanical arm tail end effect device position of time design/orientation vector X _EThe characteristics of motion, can be polynomial function or the SIN function about the time; Q is joint of mechanical arm angle column vector;

Be joint of mechanical arm angular speed column vector; J ⁺(q)=J ^T(JJ ^T) ^-1Generalized inverse for Jacobian matrix J; Subscript ". " represents derivative operation; Subscript " T " represents the transposition computing; Subscript " 1 " represents inversion operation.

Then utilize collision detection algorithm to judge whether the target configuration that calculates collides with barrier, if target configuration and barrier that integration obtains are collided, then can be to the input of target configuration derivation algorithm: the current joint angle of mechanical arm be adjusted (as changing the angle of first joint angle) arbitrarily, re-start calculating according to target configuration derivation algorithm, the target configuration of the required end effector position of capture target and attitude is satisfied in acquisition, until obtain collisionless target configuration.Said process is just adjusted the current joint angle of mechanical arm that inputs to target configuration derivation algorithm, and the current joint angle of mechanical arm does not change.

(1.2) according to the current configuration of mechanical arm and target configuration, utilize the collisionless path search algorithm, try to achieve the path in the collisionless configuration space;

As shown in Figure 3, collisionless path search algorithm thinking is as follows: algorithm is input as the configuration (initial configuration) of current mechanical arm, the target configuration that mechanical arm will arrive and the parameter of barrier are output as not the motion sequence of each joint angle of the mechanical arm of colliding with barrier.Make that positive integer N is that constant is cut apart in the path, the total angle of rotation of each joint angle from current configuration to the target configuration is divided into N part, each joint angle is turned over every part of corner be defined as a son motion, just formed pN son and moved, wherein p is the joint number.As long as N is enough large, cut apart the path enough thin so, just can be spliced into a collisionless dog leg path by suitable order with this pN son motion.

The son motion of remembering i joint angle is son motion i, i=1, and 2 ..., p, the task of searching algorithm is to determine the order of this pN son motion by continuous exploration.Be about to total path and be divided into the pN step, pN son motion with suitable arranged sequentially, formed the motion sequence that a pN goes on foot.

Set serial number gencration function F (i), this function generates the sub-motion sequence number that next step will be soundd out by current sub-motion sequence number i, and F (i) can choose as required, as increases progressively, successively decreases or generate at random.With m represent the step number, T represents to sound out total degree, T _MaxExpression can be accepted the maximum number of times of souning out, and algorithm is as follows:

1) initialize, m=1, T=1 chooses i, and T is set _Max

2) if m=pN+1, algorithm successfully finishes, Output rusults, otherwise utilize collision detection algorithm judges that the mechanical arm m step carries out son motion i and whether collide with barrier.If do not collide, mechanical arm is carried out son motion i (this is the simulation in computer), and keeps a record execution in step 3); If collision, execution in step 4);

3) m=m+1, i=F (i), T=T+1, execution in step 6);

4) judge whether m step soundd out all possible son motion, if, execution in step 5); Otherwise i=F (), T=T+1, execution in step 6);

5) in the rebound m-1 step, make m=m-1.Judge whether m step soundd out all possible son motion, if re-execute step 5); Otherwise make i=F (i (m)), i (m) refers to the original sub-motion sequence number that adopts of m step, T=T+1, execution in step 6);

6) if T=T _Max, algorithm unsuccessfully finishes, otherwise carries out 2.

If this algorithm search failure then can be adopted different exploration rule F (i) or increase path to cut apart constant N and again search for.Exploration rule F (i) can have multiple, sounds out if adopt at random, and each result who sounds out can difference.Sound out the diversity of rule, guaranteed the validity of searching algorithm.Increase the path and cut apart the success rate that constant N then can increase search, but this is to increase amount of calculation as cost.

(1.3) utilize path optimization's algorithm, the path in the collisionless configuration space is optimized the motion sequence of each joint angle of the mechanical arm that acquisition do not collide with barrier.

The motion sequence of each joint angle of the mechanical arm of not colliding with barrier that algorithm is input as that the collisionless path search algorithm obtains is output as the motion sequence of each joint angle of the mechanical arm of not colliding with barrier after the optimization.Two adjacent in the motion sequence of each joint angle of the collisionless mechanical arm that trial searches collisionless path search algorithm motions are merged into a son motion, two son motions in chronological sequence sequentially carrying out are in turn before carried out simultaneously, and then whether the motion of the son behind the combining data detection collides with barrier.If do not collide, then these two son motions are merged; If colliding then, cancellation merges.Constantly attempt, until adjacent son motion can't further merge in the motion sequence that obtains.

Whether described collision detection algorithm can detect a motion process or some configurations of mechanical arm and collide with barrier.

The collision detection algorithm method is as follows: the some motion processes that are input as mechanical arm to be detected of algorithm, i.e. each joint angle characteristics of motion in time.At first the motion process with mechanical arm carries out temporal discretization, and is discrete for being in the mechanical arm of a series of configurations; Under each configuration, mechanical arm is reduced to the line segment of some connections, then carry out the discretization on the space, the discrete a series of point that turns on the mechanical arm is whether test point is inner at barrier with problem reduction; Barrier is merotomized, and every part finds the envelope of a convex polyhedron, then with the approximate barrier that replaces of these convex polyhedrons itself.The central point of remembering i face of certain convex polyhedron is P _i, the exterior normal vector of i face is Remember that point to be detected is P _T, then put P _TThe sufficient and necessary condition that is positioned at this convex polyhedron is: all faces for convex polyhedron have

If exist measuring point to be checked inner at barrier, think that then mechanical arm and barrier collide, otherwise think that mechanical arm and barrier can not bump.

Two, the trajectory planning of the meticulous section of arresting

The method for planning track of the meticulous section of arresting comprises the steps:

(2.1) designed in advance the movement locus of end effector position/attitude by the position/attitude of the end effector position/attitude of capture target of expectation and current end effector, speed track and acceleration trajectory;

After the section of arresting finishes roughly, utilize vision sensor on the spatial operation platform to measure the position/attitude of the end effector of the capture target that obtains expectation to arresting a little position/attitude on the target, and with the position/attitude of current end effector do poor, then design the end effector position that connects a desired value and currency/attitude motion track, speed track and acceleration trajectory.

The speed track of end effector position/attitude of designing in advance and acceleration trajectory are 0 at starting point and the destination county of operation.

(2.2) the target relative motion that measures according to the vision measurement sensor is to the movement locus of described end effector position/attitude, speed track and acceleration trajectory compensate the movement locus of the end effector position/attitude after obtaining to compensate, speed track and acceleration trajectory; Thereby realize the real-Time Compensation to the relative motion of target;

(2.3) according to the movement locus of the end effector position/attitude after the compensation, the Pseudoinverse algorithm that speed track and acceleration trajectory adopt based on motion to learn obtains angular speed track and the angular acceleration track of each joint angle of mechanical arm; Thereby can make the position of end effector and attitude follow the in advance movement locus of design, the mechanical arm of speed track and acceleration trajectory.

The angular speed track of each joint angle of mechanical arm

Formula is:

${\stackrel{\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

Wherein, X _Er(t) and

Be movement locus and the speed track of the end effector position/attitude after the compensation, X _E(t) be the position/attitude of end effector in the mechanical arm actual motion process; δ (X _Er(t)-X _E(t)) be that the ratio that adds is regulated item, can compensate the tracking error of actual motion with the track that designs in advance, increase antijamming capability and the robustness of system, wherein δ＞0; J ⁺=J ^T(JJ ^T) ^-1Pseudoinverse for Jacobian matrix J;

Subscript " " represents derivative operation; Subscript " T " represents the transposition computing; Subscript " 1 " represents inversion operation.

The joint angle acceleration trajectory is:

${\stackrel{\·\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·({\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)-{\stackrel{\·}{X}}_E\left(t\right))]+{\stackrel{\·}{J}}^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

Wherein, Acceleration trajectory for the end effector position/attitude after the compensation;

Time-derivative for the pseudoinverse of Jacobian matrix J.

(2.4) utilize the redundant degree of freedom of mechanical arm, adopt idle running to make mechanical arm away from singular configuration.

Utilize the redundant degree of freedom of mechanical arm, adopt idle running to make mechanical arm away from singular configuration, final joint angle speed is

${\stackrel{\·}{q}}_r={\stackrel{\·}{q}}_{\mathrm{rmp}}+P_{J}y,$

Wherein Basic Solutions for the inverse kinematics that obtains in the step (203); P _J=(I-J ^T(JJ ^T) ^-1J) be the orthogonal intersection cast shadow matrix of Jacobian matrix J; Column vector y=α v;

ξ (q) is unusual tolerance: ξ (q)=det (JJ ^T); Scalar parameter α is chosen for:

Subscript " " represents derivative operation; The transposition computing is asked in subscript " T " representative; Subscript " 1 " represents inversion operation;

Represent scalar ξ vector q is asked gradient.

Embodiment 1

Embodiment 1 is mainly used in verifying the roughly algorithm complexity of the section of arresting.

(1) 7 degree-of-freedom manipulators have been designed.The geometric parameter of mechanical arm is as shown in table 1, and wherein the 1st joint arm mounting points represents in base coordinate system.Each joint angle is that 0 o'clock initial configuration and the positive direction of each corner are seen Fig. 4, and the black line segment has represented mechanical arm among the figure, and the black circles representative connects the joint of respectively saving mechanical arm, and juxta-articular arrow represents the positive direction of each joint rotation angle.This moment, the body coordinate system of base coordinate system and Ge Jie arm overlapped, and saw the coordinate system oxyz among Fig. 4.Among Fig. 4,6 is the pedestal mounting points of mechanical arm, and 7 is the end effector of mechanical arm, i.e. the 7th joint arm.The below verifies the roughly algorithm complexity of the section of arresting as an example of this mechanical arm example.

Table 1 respectively saves the mounting points location parameter of arm

The initial configuration of (2) establishing mechanical arm is q _S, each joint angle is (1.5707,3.1414,1,0,0,0,0) rad.Following parameter is described in base coordinate system: the end effector endpoint location was (2.0,0.3,0.5) m when the roughly section of arresting of expectation finished; End effector axis direction cosine was (0.4699 ,-0.1987,0.8601) when the roughly section of arresting of expectation finished; Barrier is a cuboid that seamed edge is parallel with reference axis, and coordinate range is χ: 1.2～1.7m, y:0～0.5m, z:0～0.5m.Note, object pose is to the rotation not constraint of end effector along axis, and the 7th joint arm axle line and the 7th joint angle shaft parallel so the 7th joint angle can not move, is in fact the end effector pose requirement that a 6DOF mechanical arm satisfies 5DOF.

(3) at first by collisionless target configuration derivation algorithm, solve a target configuration qE who satisfies the requirement of end effector pose and do not collide with barrier, each joint angle is (0.0567,4.6127,1.4569 ,-2.1691,0.9561 ,-0.3024,0) and rad.

(4) then search out a path of not colliding with barrier by the collisionless path search algorithm, constant N=10 is separated in the path, and the 7th joint angle corner does not change, and is in fact the search problem of 6 dimension spaces, be divided into into 6N=60 son motion, function F (i) adopts and increases progressively exploration.Through 64 explorations, find a collisionless motion sequence: 1,2,3,4,5,6,1,2,3,4,5,6,1,2,3,4,5,6,1,2,3,4,5,6,1,2,3,4,5,6,1,2,3,4,5,6,1,2,3,4,5,6,2,3,4,5,6,2,3,4,5,6,2,3,4,5,6,1,1,1.The previous section that can see sequence is identical with the exploration order, does not adjust, and algorithm sequentially adjusts to avoid obstacle to exploration in the back of sequence.Here the collisionless path that obtains not is final result, but for the necessity of path optimization's algorithm is described, we here provide mechanical arm on the collision-free motion sequence of not optimizing motion process and the curve movement of joint angle.Obtain after the joint angle motion sequence of mechanical arm, also need to provide the characteristics of motion that mechanical arm is finished required joint angle of each step in the sequence.Setting each cross-talk time of doing exercises is 4s, and the joint angle characteristics of motion adopts the multinomial about the time, and when guaranteeing every height motion beginning and end, joint angle speed and angular acceleration are 0.The motion process of mechanical arm is seen Fig. 5, records at set intervals the position of one-time mechanical arm among the figure, and 8 is the mechanical arm initial configuration among the figure, and 9 is the target configuration when slightly the section of arresting finishes, and 6 is the pedestal mounting points of mechanical arm, and 10 is barrier.As shown in Figure 5, mechanical arm has arrived the target configuration from bottom cut-through thing.Fig. 6 has provided the angular speed curve of each joint angle.As can be seen from Figure 6, joint angle speed is level and smooth not as a whole.Therefore it is necessary this path being optimized.

(5) adopt the path smooth algorithm that dog leg path obtained above is carried out smoothly below.Found that sequences that original 60 son motions form only need be divided into for 2 steps and get final product avoiding obstacles, configuration in the middle of the 1st step was moved to by initial configuration, the 2nd step, to the target configuration, each configuration was shown in Table 2 by middle configuration.Obtain also needing to provide the characteristics of motion that mechanical arm is finished required joint angle of each step after the initial configuration and end configuration in each step.Setting each step run duration is 20s, and the joint angle characteristics of motion adopts the multinomial about the time, and when guaranteeing each step beginning and end, joint angle speed and angular acceleration are 0.The motion process of mechanical arm records the position of one-time mechanical arm as shown in Figure 7 at set intervals among the figure, 8 is the mechanical arm initial configuration among the figure, 9 is the target configuration when slightly the section of arresting finishes, 6 is the pedestal mounting points of mechanical arm, and 10 is barrier, and 11 is the middle configuration of mechanical arm.Configuration 11 in the middle of the mechanical arm first step is moved to by initial configuration 8, second step moves to target configuration 9 by middle configuration 11.As seen from Figure 7, mechanical arm has arrived the target configuration from bottom cut-through thing, and mechanical arm does not all have to collide with barrier in this two steps motion process.Fig. 8 has provided the angular speed curve of each joint angle.As can be seen from Figure 8, compare with Fig. 6, the characteristics of motion of each joint angle of mechanical arm is very level and smooth, be easy to realize, and the operating time greatly reduce, be convenient to arresting fast noncooperative target.

Three crucial configurations (rad) on the level and smooth rear path of table 2

Embodiment 2

Embodiment 2 has provided the control method of the whole capture process of redundant space mechanical arm.

(1) mechanical arm among the embodiment 1 is installed on the spatial operation platform, a noncooperative target is arrested.At first measure by arresting mission requirements and sensor, determine arresting a little on the noncooperative target, the position of needed end effector and attitude when calculating capture target.

(2) whole capture process is divided into the roughly section of arresting and the meticulous section of arresting.The initial state of the section of arresting is the current configuration of mechanical arm roughly, and the end effector that the rough done state of the section of arresting is set as mechanical arm is positioned at arrests a distance (such as 20cm), top.

Position and the attitude of end effector by collisionless target configuration derivation algorithm, were obtained collisionless target configuration when the roughly section of arresting that (3) is set by previous step finished.By current configuration and the target configuration of mechanical arm, try to achieve level and smooth collisionless joint angle motion sequence by collisionless path search algorithm and path optimization's algorithm.According to the time requirement of the task of arresting, set the run duration (going on foot 20s consuming time such as each) in each step on the collision-free motion sequence.Then according to the run duration of setting, the design joint angle characteristics of motion is the polynomial function of time, guarantees that joint angle speed and angular acceleration are 0 when each step beginning and end.Subsequently, the control system of spatial operation platform, adopt certain control method (such as the PID controller), the motor at driving device shoulder joint place, follow the given joint angle characteristics of motion, thereby realize the roughly target of the section of arresting, with the end effector without collision of mechanical arm move to arrest above a distance.

(4) after the section of arresting finishes roughly, by the sensor on the spatial operation platform, measure a little the position of arresting of this moment, design the movement locus of end effector position and attitude, speed track and acceleration trajectory, the Pseudoinverse algorithm that adopts based on motion to learn, real-time resolving goes out the position at joint of mechanical arm angle, speed and acceleration information, control system by the spatial operation platform, adopt certain control method (such as the PID controller), the motor at driving device shoulder joint place is followed the joint angle characteristics of motion that real-time resolving goes out.Simultaneously, by the sensor on the spatial operation platform, arrest a little motion with respect to the spatial operation platform on the measurement target, relative motion to target in Pseudoinverse algorithm is compensated, thereby realize final accurately arresting, namely mechanical arm accurately moves to the place that arrests on the target with end effector.

This specific embodiment part is arrested for the redundant mechanical arm target of 7 frees degree and has been realized collisionless without unusual trajectory planning, and engineering demands is easy to engineering and uses, and has good promotion prospect.

The content that is not described in detail in the specification of the present invention belongs to those skilled in the art's known technology.

Claims (8)

1. method for planning track that the redundant space mechanical arm is arrested at rail; Described redundant space mechanical arm is installed on the report control platform of space, and mechanical arm tail end is installed end effector; The space report control platform is equipped with vision measurement sensor and spaceborne computer; The redundant space mechanical arm rail arrest to as if the space non-cooperative target; It is characterized in that, the operation of arresting of space manipulator is divided into the roughly section of arresting and the meticulous section of arresting,

The method for planning track of the section of arresting comprises the steps: roughly

(1.1) by roughly desired locations and the attitude of the section of arresting end effector, utilize collisionless target configuration derivation algorithm to try to achieve collisionless target configuration;

(1.2) according to the current configuration of mechanical arm and target configuration, utilize the collisionless path search algorithm, try to achieve the path in the collisionless configuration space;

(1.3) utilize path optimization's algorithm, the path in the collisionless configuration space is optimized the motion sequence of each joint angle of the mechanical arm that acquisition do not collide with barrier;

The method for planning track of the meticulous section of arresting comprises the steps:

(2.1) designed in advance the movement locus of end effector position/attitude by the position/attitude of the end effector position/attitude of capture target of expectation and current end effector, speed track and acceleration trajectory;

(2.2) the target relative motion that measures according to the vision measurement sensor is to the movement locus of described end effector position/attitude, speed track and acceleration trajectory compensate the movement locus of the end effector position/attitude after obtaining to compensate, speed track and acceleration trajectory;

(2.3) according to the movement locus of the end effector position/attitude after the compensation, the Pseudoinverse algorithm that speed track and acceleration trajectory adopt based on motion to learn obtains angular speed track and the angular acceleration track of each joint angle of mechanical arm.

2. a kind of redundant space mechanical arm according to claim 1 method for planning track of arresting at rail, it is characterized in that: in the described step (1.1), collisionless target configuration derivation algorithm is as follows:

At first calculate the target configuration according to target configuration derivation algorithm;

Target configuration derivation algorithm be input as each current joint angle of mechanical arm, and position and the attitude of the required end effector of capture target, target configuration derivation algorithm utilizes numerical integration method to find the solution mechanical arm inverse kinematics equation and obtains each required joint angle of capture target, i.e. target configuration;

Then utilize collision detection algorithm to judge whether the target configuration that calculates collides with barrier, if collide with barrier, then the current joint angle of mechanical arm that inputs to target configuration derivation algorithm is adjusted, re-start calculating according to target configuration derivation algorithm, until obtain collisionless target configuration.

3. a kind of redundant space mechanical arm according to claim 1 method for planning track of arresting at rail, it is characterized in that: the collisionless path search algorithm is divided into N part with the total angle of rotation of each joint angle from current configuration to the target configuration in the described step (1.2), each joint angle is turned over every part of corner be defined as a son motion, thereby formed pN son motion, wherein p is the joint number;

The son motion of remembering i joint angle is son motion i, i=1, and 2 ..., p, the collisionless path search algorithm with suitable arranged sequentially, forms the motion sequence that a pN goes on foot with pN son motion;

Set serial number gencration function F (i), this function generates the sub-motion sequence number that next step will be soundd out by current sub-motion sequence number i; F (i) is for increasing progressively, successively decrease or generating function at random;

With m represent the step number, T represents to sound out total degree, T _MaxExpression can be accepted the maximum number of times of souning out;

Collisionless path search algorithm step is as follows:

1) initialize, make m=1, T=1 chooses i, and T is set _Max

2) judge whether m equals pN+1;

If m=pN+1 then exports collisionless joint motions sequence, and finish searching algorithm; Otherwise utilize collision detection algorithm, judge whether mechanical arm m step execution motion i collides with barrier; If do not collide, mechanical arm is carried out son motion i, and keeps a record execution in step 3); If collision, execution in step 4);

3) m=m+1, i=F (i), T=T+1, execution in step 6);

4) judge whether m step soundd out all possible son motion i, if, execution in step 5); Otherwise i=F (i), T=T+1, execution in step 6);

5) in the rebound m-1 step, make m=m-1; Judge whether m step soundd out all possible son motion, if re-execute step 5); Otherwise make i=F (i (m)), i (m) refers to the original sub-motion sequence number that adopts of m step, T=T+1, execution in step 6);

6) judge whether T equals T _Max,

If T=T _Max, then output algorithm is searched for failure and is finished searching algorithm, otherwise execution in step 2);

If the algorithm search failure is then adopted different exploration rule F (i) or increase path to cut apart constant N and is re-executed the collisionless path search algorithm.

4. a kind of redundant space mechanical arm according to claim 1 method for planning track of arresting at rail, whether it is characterized in that: path optimization's algorithmic method is as follows in the described step (1.3): two adjacent son motions are merged into a son motion in the motion sequence of each joint angle of the collisionless mechanical arm that the collisionless path search algorithm is searched, then adopt the son motion behind the collision detection algorithm combining data detection to collide with barrier; If do not collide, then these two motions are merged; If colliding then, cancellation merges; Adjacent son motion repeats said process, until can't further merge in the motion sequence that obtains.

5. the method for planning track of arresting at rail to the described a kind of redundant space mechanical arm of 4 arbitrary claims according to claim 2, it is characterized in that: described collision detection algorithm method is as follows: the line segment that mechanical arm is reduced to some connections, then carry out the discretization on the space, the discrete a series of point that turns on the mechanical arm, barrier is merotomized, every part finds the envelope of a convex polyhedron, then with the approximate barrier that replaces of these convex polyhedrons itself; The central point of remembering i face of certain convex polyhedron is P _i, the exterior normal vector of i face is

Remember that point to be detected is P _T, then put P _TThe sufficient and necessary condition that is positioned at this convex polyhedron is: all faces for convex polyhedron have

If exist measuring point to be checked inner at barrier, think that then mechanical arm and barrier collide, otherwise think that mechanical arm and barrier can not bump.

6. a kind of redundant space mechanical arm according to claim 1 method for planning track of arresting at rail is characterized in that: the angular speed track of each joint angle of the mechanical arm in the described step (2.3)

Formula is:

${\stackrel{\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

Wherein, X _Er(t) and

Be movement locus and the speed track of the end effector position/attitude after the compensation, X _E(t) be the position/attitude of end effector in the mechanical arm actual motion process; δ (X _Er(t)-X _E(t)) regulate item, wherein δ 〉=0 for the ratio that adds; J ⁺=J ^T(JJ ^T) ^-1Pseudoinverse for Jacobian matrix J;

The joint angle acceleration trajectory of each joint angle of described mechanical arm

For:

${\stackrel{\·\·}{q}}_r\left(t\right)=J^{+}[{\stackrel{\·\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·({\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)-{\stackrel{\·}{X}}_E\left(t\right))]+{\stackrel{\·}{J}}^{+}[{\stackrel{\·}{X}}_{\mathrm{Er}}\left(t\right)+\mathrm{\δ}\·(X_{\mathrm{Er}}\left(t\right)-X_E\left(t\right))]$

Wherein,

Acceleration trajectory for the end effector position/attitude after the compensation;

Time-derivative for the pseudoinverse of Jacobian matrix J.

7. a kind of redundant space mechanical arm according to claim 1 method for planning track of arresting at rail, it is characterized in that: the method for planning track of the described meticulous section of arresting also comprises the steps:

Utilize the redundant degree of freedom of mechanical arm, adopt idle running to make mechanical arm away from singular configuration.

8. a kind of redundant space mechanical arm according to claim 7 method for planning track of arresting at rail is characterized in that:

The angular speed track of each joint angle of the mechanical arm of band idle running is

${{\stackrel{\·}{q}}_r}^{\′}={\stackrel{\·}{q}}_r\left(t\right)+P_{J}y,$

P _J=(I-J ^T(JJ ^T) ^-1J) be the orthogonal intersection cast shadow matrix of Jacobian matrix J; Column vector y=α v;

ξ (q) is unusual tolerance: ξ (q)=det (JJ ^T); Scalar parameter α is chosen for:

Subscript ". " represents derivative operation; The transposition computing is asked in subscript " T " representative; Subscript " 1 " represents inversion operation;

Represent scalar ξ vector q is asked gradient.

Images