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

Abstract

A trajectory planning method for on-orbit capture of a redundant space manipulator simplifies the problem by dividing the capture operation of the space manipulator into a rough capture segment and a fine capture segment. Aiming at the outstanding problems in different stages, different planning algorithms are adopted. The trajectory search algorithm based on the configuration space is used in the rough capture section, which is based on the trial in the configuration space; the pseudo-inverse algorithm based on kinematics is used in the fine capture section, and the final accuracy is guaranteed by compensating the relative motion of the target. arrest.

Description

A trajectory planning method for on-orbit capture of redundant space manipulators

technical field

The invention relates to a trajectory planning method for on-orbit capture of a redundant space manipulator, belonging to the field of spacecraft attitude control.

Background technique

In the on-orbit capture operation of the space manipulator, in order to save fuel, the position of the space control platform is not controlled, but only the attitude of the space control platform is stabilized, which belongs to the attitude-controlled free-flying robot. Only the individual joint angles of the manipulator can be controlled arbitrarily. The basic requirement of trajectory planning is to target the desired position and orientation of the end effector, and determine the angular position, angular velocity and angular acceleration trajectory of each joint angle through a certain algorithm, so that the controller can realize the grasping of the mechanical arm. capture operation. Redundancy here means that the robotic arm has 7 degrees of freedom. In addition to enabling the end effector to reach the specified position and posture, it also has redundant degrees of freedom for avoiding collisions with obstacles. Multiple robotic arms can be used at the same time. Coordinated control enables flexible replacement of end effectors, stable, safe and reliable capture of non-cooperative targets.

If the robotic arm collides with an obstacle during the capture operation, the space control platform will be affected by an unknown reaction force, which will bring an unknown disturbance force and disturbance torque to the entire system, causing the platform to move unpredictably and destroy the attitude of the platform Stability and stability of the movement of the robotic arm will not only lead to the failure of the capture operation, but also may cause unpredictable damage to the space manipulation platform. Therefore, collision avoidance is a factor that must be considered in the trajectory planning of the space manipulator capture operation. In addition, the singularity of the manipulator and the compensation of the relative motion of the target are also issues that must be considered in trajectory planning.

The trajectory planning algorithms of the manipulator mainly include pseudo-inverse algorithm, artificial potential field method and free space method based on configuration space. Pseudo-inverse algorithm is prone to singularities in the solution. The artificial potential field method has a local minimum problem, and for complex problems, the amount of calculation is too large, so it is more suitable for trajectory planning of mobile robots. In the configuration space, the coordinates of configuration points are the joint angles of the manipulator. The target pose and obstacles of the actor are mapped from the workspace to the configuration space by the kinematic relationship, and the target configuration points and obstacles in the configuration space are obtained. Collision-free trajectory planning is to search for a path connecting the initial and target configuration points without passing through obstacles in the configuration space. The existing methods all require the deconstruction of the space. For the multi-degree-of-freedom manipulator, the calculation is very large, and the configuration space obstacle can only be obtained through discretization, which requires a large amount of memory to improve the accuracy. As a result, existing algorithms cannot be applied to space manipulators with limited computing resources, and are difficult to apply to space missions.

Contents of the invention

The technical problem to be solved by the present invention is to overcome the deficiencies of the prior art and provide a trajectory planning method for on-orbit capture of a redundant space manipulator without collision and singularity. The method is simple in engineering implementation and can save computing resources.

Technical scheme of the present invention is:

A trajectory planning method for on-orbit capture of a redundant space manipulator; the redundant space manipulator is installed on a space control platform, and an end effector is installed at the end of the manipulator; the space control platform is equipped with a visual measurement sensor and a space-borne computer; the object captured by the redundant space manipulator in orbit is a space non-cooperative target; the capture operation of the space manipulator is divided into a rough capture segment and a fine capture segment,

The trajectory planning method of the rough capture segment includes the following steps:

(1.1) From the expected position and attitude of the end effector in the rough capture section, the collision-free target configuration is obtained by using the non-collision target configuration algorithm;

(1.2) According to the current configuration and the target configuration of the manipulator, use the collision-free path search algorithm to obtain the path in the configuration space without collision;

(1.3) Utilize the path optimization algorithm to optimize the path in the configuration space without collision to obtain the motion sequence of each joint angle of the mechanical arm that does not collide with the obstacle;

The trajectory planning method of the fine capture segment includes the following steps:

(2.1) Pre-design the motion track, velocity track and acceleration track of the end effector position/posture from the position/posture of the end effector of the desired capture target and the position/posture of the current end effector;

(2.2) According to the relative motion of the target measured by the visual measurement sensor, the motion track of the position/posture of the end effector, the velocity track and the acceleration track are compensated to obtain the motion track of the position/posture of the end effector after compensation, and the speed track and acceleration trajectory;

(2.3) According to the motion trajectory, velocity trajectory and acceleration trajectory of the compensated end effector position/attitude, the angular velocity trajectory and angular acceleration trajectory of each joint angle of the manipulator are obtained by using a pseudo-inverse algorithm based on kinematics.

In the step (1.1), the collision-free target configuration solution algorithm is as follows:

Firstly, the target configuration is calculated according to the target configuration solving algorithm;

The input of the target configuration solving algorithm is the current joint angle of the manipulator, and the position and attitude of the end effector required to capture the target. The target configuration solving algorithm uses the numerical integration method to solve the inverse kinematics equation of the manipulator to obtain the captured Each joint angle required by the target, that is, the target configuration;

Then use the collision detection algorithm to judge whether the calculated target configuration collides with an obstacle. If it collides with an obstacle, adjust the current joint angle of the manipulator input to the target configuration solving algorithm to solve the problem according to the target configuration The algorithm recalculates until a collision-free target configuration is obtained.

In the step (1.2), the collision-free path search algorithm divides the total rotation angle of each joint angle from the current configuration to the target configuration into N parts on average, and each joint angle is defined as a sub-motion through each rotation angle, Thus, pN sub-movements are formed, where p is the number of joints;

Denote the sub-motion of the i-th joint angle as sub-motion i, i=1, 2, ..., P, and the collision-free path search algorithm arranges the pN sub-motions in an appropriate order to form a motion sequence of pN steps;

Set serial number generation function F (i), this function generates the sub-movement serial number that next step will be tried by current sub-motion serial number i; F (i) is increment, decrement or random generation function;

Use m to represent the step number, T to represent the total number of trials, and T _max to represent the maximum acceptable number of trials;

The steps of the collision-free path search algorithm are as follows:

1) Initialize, make m=1, T=1, choose i, set _Tmax ;

2) Determine whether m is equal to pN+1;

If m=pN+1, output a joint motion sequence without collision, and end the search algorithm; otherwise, use the collision detection algorithm to judge whether the sub-movement i of the m-th step of the manipulator collides with an obstacle; if there is no collision, the manipulator executes Give movement i, and make a record, execute step 3); if there is a collision, execute step 4);

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

4) judge whether all possible sub-movements i have been tested in step m, if so, perform step 5); otherwise i=F(i), T=T+I, perform step 6);

5) Jump back to the m-1th step, let m=m-1; judge whether all possible sub-movements have been tested in the m-th step, if re-execute step 5); otherwise let i=F(i(m)), i(m) refers to the sub-motion serial number that the m step originally adopted, T=T+1, and executes step 6);

6) Judging whether T is equal to T _max ,

If T=T _max , then the output algorithm search fails and the search algorithm ends, otherwise, perform step 2);

If the algorithm search fails, use a different trial rule F(i) or increase the path segmentation constant N to re-execute the collision-free path search algorithm.

The path optimization algorithm method in the step (1.3) is as follows: the two adjacent sub-motions in the motion sequence of each joint angle of the non-collision mechanical arm searched by the collision-free path search algorithm are merged into one sub-motion, and then the collision is adopted. The detection algorithm detects whether the merged sub-motion collides with an obstacle; if not, the two motions are merged; if they collide, the merge is canceled; the above process is repeated until the adjacent sub-motion in the obtained motion sequence Cannot merge further.

记待检测的点为P_T，则点P_T位于这个凸多面体的充分必要条件是：对于凸多面体的所有的面都有若存在待检测点在障碍物内部，则认为机械臂与障碍物相碰，否则认为机械臂与障碍物不会发生碰撞。The collision detection algorithm method is as follows: Simplify the manipulator into some connected line segments, then perform spatial discretization, discretize into a series of points on the manipulator, divide obstacles into several parts, each part finds a convex The envelope of the polyhedron, and then use these convex polyhedrons to approximate the obstacle itself; record the center point of the i-th face of a certain convex polyhedron as P _i , and the external normal vector of the i-th face is

Note that the point to be detected is P _T , then the necessary and sufficient condition for the point P _T to be located in this convex polyhedron is: for all faces of the convex polyhedron, there are If there is a point to be detected inside the obstacle, it is considered that the robot arm collides with the obstacle, otherwise it is considered that the robot arm will not collide with the obstacle.

公式为：The angular velocity trajectory of each joint angle of the mechanical arm in the step (2.3)

The formula is:

${\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; [</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span>$

为补偿后的末端作用器位置/姿态的运动轨迹和速度轨迹，X_E(t)为机械臂实际运动过程中末端作用器的位置/姿态；δ·(X_Er(t)-X_E(t))为加入的比例调节项，其中δ＞0；J⁺＝J^T(JJ^T)^-1为雅克比矩阵J的伪逆；Among them, X _Er (t), and

is the motion trajectory and velocity trajectory of the compensated end effector position/attitude, X _E (t) is the position/attitude of the end effector during the actual movement of the manipulator; δ·(X _Er (t)-X _E (t )) is the added proportional adjustment item, where δ>0; J ⁺ =J ^T (JJ ^T ) ^-1 is the pseudo-inverse of the Jacobian matrix J;

为：The joint angular acceleration trajectory of each joint angle of the mechanical arm

for:

${\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; [</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span><span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; J</span>}}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; [</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span>$

为补偿后的末端作用器位置/姿态的加速度轨迹；

为雅克比矩阵J的伪逆的时间导数。in,

is the acceleration trajectory of the compensated end effector position/attitude;

is the time derivative of the pseudoinverse of the Jacobian matrix J.

The trajectory planning method for the fine capture section further includes the following steps: utilizing the redundant degrees of freedom of the manipulator, and using idling to keep the manipulator away from the singular configuration.

The angular velocity trajectory of each joint angle of the manipulator with idling is

${{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; r</span>}}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; =</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; J</span>}}\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>$

ξ(q)为奇异度量：ξ(q)＝det(JJ^T)；标量参数α选取为：

上标“.”代表求导运算；上标“T”代表求转置运算；上标“-1”代表求逆运算；

代表标量ξ对矢量q求梯度。P _J = (IJ ^T (JJ ^T ) ^-1 J) is the orthogonal projection matrix of Jacobian matrix J; Column vector y=αv;

ξ(q) is a singular metric: ξ(q)=det(JJ ^T ); the scalar parameter α is selected as:

The superscript "." stands for derivative operation; the superscript "T" stands for transpose operation; the superscript "-1" stands for inverse operation;

Represents the scalar ξ to find the gradient of the vector q.

The beneficial effects of the present invention compared with prior art are:

(1) The present invention is a brand-new non-collision and non-singularity trajectory planning algorithm for on-orbit capture operations of spatially redundant manipulators. The problem is simplified by dividing the capture operation of the space manipulator into a rough capture segment and a fine capture segment. Aiming at the outstanding problems in different stages, different planning algorithms are adopted. The trajectory search algorithm based on the configuration space is used in the rough capture section, which is based on the trial in the configuration space; the pseudo-inverse algorithm based on kinematics is used in the fine capture section, and the final accuracy is guaranteed by compensating the relative motion of the target. arrest.

(2) In the rough capture stage, the method realizes the avoidance of obstacles through the trajectory search algorithm based on configuration space. The configuration space is a high-dimensional abstract space with the coordinates of each joint angle of the manipulator. This trajectory search algorithm based on trial in the configuration space does not need to solve the configuration space, and can find the collision-free path in the configuration space through the collision detection trial. Compared with the existing technology, it saves computing resources and In-memory, suitable for fast planning of trajectories of space manipulators with limited computing resources, provides a simple and effective method for aerospace engineering applications. Through the trajectory optimization algorithm, the final trajectory obtained is relatively smooth, which is easy to realize in engineering.

(3) In the fine capture stage, by adding the kinematics-based pseudo-inverse algorithm of target relative motion compensation, the relative motion of the target can be obtained through the measurement information of the visual sensor on the space manipulation platform, and the relative motion of the target can be obtained. Introduced into the pseudo-inverse algorithm to realize the compensation of the relative motion of the target. In addition, in the pseudo-inverse algorithm, a proportional adjustment item is added to compensate the tracking error of the trajectory, increase the anti-interference ability and robustness of the system, and ensure the accurate capture of the target by the robotic arm in the fine capture section.

(4) The engineering implementation of the method involved in the present invention is simple, and has good prospects for popularization and application. This method not only meets the basic requirements of collision-free and non-singular trajectory planning for space manipulators, but also has a simple algorithm that is easy to implement, saves computing time, computing resources and memory, and is especially suitable for fast trajectory planning of space manipulators with tight computing resources. This method is especially suitable for the rapid capture of non-cooperative targets by the space manipulator, and also for the collision-free trajectory planning of the manipulator on the lunar rover.

Description of drawings

Fig. 1 is a schematic diagram of capturing a space target by a space manipulation platform of the present invention;

Fig. 2 is the schematic flow chart of the non-collision and non-singular manipulator trajectory planning algorithm of the present invention;

Fig. 3 is a schematic flow chart of a collision-free path search algorithm in the present invention;

Fig. 4 is the initial configuration and the positive direction of the rotating shaft of the 7-DOF mechanical arm in the present invention;

Fig. 5 is the non-collision path before algorithm smoothing among the present invention;

Fig. 6 is the joint angular velocity before algorithm smoothing among the present invention; Wherein Fig. 6 (a) curve 11,12,13 are the angular velocity variation curves of the 1st, 2,3 joints respectively; Curve 14,15 among Fig. 6 (b) , 16 are the angular velocity curves of joints 4, 5, and 6 respectively;

Fig. 7 is the non-collision path after algorithm smoothing among the present invention;

Fig. 8 is the joint angular velocity after algorithm smoothing among the present invention; Wherein Fig. 8 (a) curve 21,22,23 are respectively the angular velocity variation curve of the 1st, 2,3 joint; Fig. 8 (b) curve 24,25 , 26 are the angular velocity curves of joints 4, 5, and 6, respectively.

Detailed ways

The present invention provides a collision-free and non-singular trajectory planning method for on-orbit capture by a redundant space manipulator. As shown in Figure 1, the equipment on which the method relies includes a space control platform 1 and a manipulator installed on the space control platform. System 3; the space control platform is equipped with a visual measurement sensor and an on-board computer; the end of the robotic arm is equipped with an end effector; the object captured by the robotic arm is the space non-cooperative target 2. Space operation platforms generally refer to on-orbit controllable satellites with robotic arms, and space non-cooperative targets generally refer to space uncontrollable satellites. The robotic arm system 3 includes n mechanical arms, and each robotic arm contains p joints 4, which are recorded as the first joint, the second joint, ... the p joint, and are installed on the p joint starting from the space manipulation platform. End effector 5. The present invention provides a collision-free and non-singular trajectory planning method for the manipulator system 3 to capture the target.

For example, the robotic arm system 3 may include 3 robotic arms, each of which has 7 degrees of freedom.

The present invention will be described in detail below in conjunction with accompanying drawing 2 and examples. As shown in Figure 2, the entire capture operation of the space manipulator is divided into a rough capture segment and a fine capture segment. In the rough capture stage, the trajectory planning problem of the space manipulator is simplified to the trajectory planning problem of the fixed base manipulator, and the trajectory search algorithm based on the configuration space is used to move the end effector to the position and attitude close to the capture target. State, the difference between the position and attitude of the final capture target and the error caused by the relative motion of the target in the rough capture segment are corrected by the fine capture segment. In the fine capture segment, the operation volume is very small, and the relative motion of the target is not obvious. At the same time, the pseudo-inverse algorithm based on kinematics is used for trajectory planning, and the relative motion of the target is compensated in real time in the algorithm, so as to finally achieve accurate capture.

1. Trajectory planning of the rough capture segment

Specifically include the following steps:

(1.1) From the expected position and attitude of the end effector in the rough capture section, the collision-free target configuration is obtained by using the non-collision target configuration algorithm;

The expected position and attitude of the end effector in the rough capture segment is the expected position and orientation at the end of the rough capture segment, that is, the position and orientation of the end effector required to capture the target.

The non-collision target configuration solving algorithm first calculates the target configuration according to the target configuration solving algorithm;

The input of the target configuration solving algorithm is the current joint angles of the manipulator, that is, the current manipulator configuration, and the position and attitude of the end effector required to capture the target, and the output is the joint angles required to capture the target. the target configuration. The method used is to solve the inverse kinematics equation of the manipulator by using the numerical integration method (such as the 4th order Runge-Kutta method)

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; )</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; .</span>$

为由当前末端作用器的位置和姿态、抓捕目标所需的末端作用器的位置和姿态以及抓捕时间设计的机械臂末端作用器位置/姿态矢量X_E的运动规律，可以是关于时间的多项式函数或正弦函数；q为机械臂关节角列向量；

为机械臂关节角速度列向量；J⁺(q)＝J^T(JJ^T)^-1为雅克比矩阵J的广义逆；上标“.”代表求导运算；上标“T”代表转置运算；上标“-1”代表求逆运算。The initial joint angle of the inverse kinematics equation integration is the current joint angle of the manipulator, and the end value of the integral is the output of the algorithm: the joint angles required to capture the target, that is, the target configuration. In the equation, X _E is the position/attitude vector of the end effector of the manipulator;

The motion law of the position/attitude vector X _E of the end effector of the mechanical arm designed for the position and attitude of the current end effector, the position and attitude of the end effector required to capture the target, and the capture time can be about time Polynomial function or sine function; q is the column vector of the joint angle of the manipulator;

is the column vector of the joint angular velocity of the manipulator; J ⁺ (q) = J ^T (JJ ^T ) ^-1 is the generalized inverse of the Jacobian matrix J; the superscript "." represents the derivation operation; the superscript "T" represents the transposition operation ; The superscript "-1" represents the inverse operation.

Then use the collision detection algorithm to judge whether the calculated target configuration collides with the obstacle. If the integrated target configuration collides with the obstacle, the input of the target configuration solution algorithm: the current joint angle of the manipulator can be calculated. Arbitrary adjustments (such as changing the angle of the first joint angle), recalculate according to the target configuration solving algorithm, and obtain the target configuration that meets the position and posture of the end effector required to capture the target, until the target without collision is obtained structure. The above process only adjusts the current joint angle of the manipulator input to the target configuration solving algorithm, and the current joint angle of the manipulator does not change.

(1.2) According to the current configuration and the target configuration of the manipulator, use the collision-free path search algorithm to obtain the path in the configuration space without collision;

As shown in Figure 3, the idea of the collision-free path search algorithm is as follows: the input of the algorithm is the configuration of the current manipulator (starting configuration), the target configuration to be reached by the manipulator, and the parameters of obstacles, and the output is The motion sequence of the joint angles of the colliding arms. Let the positive integer N be the path segmentation constant, and divide the total rotation angle of each joint angle from the current configuration to the target configuration into N parts on average, and define each joint angle as a sub-motion through each rotation angle, which forms pN sub-motions, where p is the number of joints. Then as long as N is large enough and the path is divided fine enough, the pN sub-movements can be spliced into a non-collision polyline path in an appropriate order.

Denote the sub-motion of the i-th joint angle as sub-motion i, i=1, 2, ..., p, and the task of the search algorithm is to determine the order of the pN sub-motions through constant trial and error. That is, the total path is divided into pN steps, and the pN sub-movements are arranged in an appropriate order to form a pN-step motion sequence.

Set the serial number generation function F(i), which generates the sub-motion serial number to be tested in the next step from the current sub-motion serial number i, and F(i) can be selected according to needs, such as increasing, decreasing or randomly generated. Use m to represent the step number, T to represent the total number of trials, and T _max to represent the maximum number of trials that can be accepted. The algorithm is as follows:

1) Initialize, m=1, T=1, select i, set _Tmax ;

2) If m=pN+1, the algorithm ends successfully, and the result is output; otherwise, the collision detection algorithm is used to judge whether the sub-movement i executed by the robot arm in step m collides with an obstacle. If there is no collision, the robotic arm performs sub-motion i (this is a simulation in the computer), and makes a record, perform step 3); if there is a collision, perform step 4);

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

4) judge whether all possible sub-movements have been tested in step m, if so, perform step 5); otherwise i=F (), T=T+1, perform step 6);

5) Jump back to step m-1, let m=m-1. Judge whether all possible sub-movements have been tested in the mth step, if step 5) is re-executed; otherwise let i=F(i(m)), i(m) refers to the original sub-movement sequence number used in the m-th step, T= T+1, execute step 6);

6) If T=T _max , the algorithm fails and ends, otherwise, execute 2.

If the algorithm search fails, you can use a different trial rule F(i) or increase the path division constant N to search again. There can be many kinds of trial rules F(i). If random trial is used, the result of each trial will be different. The diversity of trial rules ensures the effectiveness of the search algorithm. Increasing the path division constant N can increase the success rate of the search, but this is at the cost of increasing the amount of calculation.

(1.3) Using the path optimization algorithm, optimize the path in the configuration space without collision to obtain the motion sequence of each joint angle of the mechanical arm that does not collide with obstacles.

The input of the algorithm is the motion sequence of each joint angle of the manipulator that does not collide with obstacles obtained by the collision-free path search algorithm, and the output is the optimized motion sequence of each joint angle of the manipulator that does not collide with obstacles. Try to combine two adjacent sub-movements in the motion sequence of each joint angle of the collision-free manipulator searched by the collision-free path search algorithm into one sub-motion, and simultaneously perform the two sub-motions that were performed sequentially in chronological order , and then detect whether the merged sub-motion collides with an obstacle. If they do not collide, the two sub-motions are merged; if they collide, the merge is cancelled. Keep trying until the resulting motion sequence cannot further merge adjacent sub-motions.

The collision detection algorithm can detect whether a movement process or a certain configuration of the robotic arm collides with an obstacle.

若存在待检测点在障碍物内部，则认为机械臂与障碍物相碰，否则认为机械臂与障碍物不会发生碰撞。The collision detection algorithm method is as follows: the input of the algorithm is a certain motion process of the robot arm to be detected, that is, the motion law of each joint angle with time. Firstly, the movement process of the manipulator is discretized in time, which is discretized into a series of configurations of the manipulator; in each configuration, the manipulator is simplified into some connected line segments, and then discretized in space, discrete Turn it into a series of points on the robotic arm, simplify the problem to detect whether the point is inside the obstacle; divide the obstacle into several parts, find the envelope of a convex polyhedron for each part, and then use these convex polyhedrons to approximate the obstacle itself . Note that the center point of the i-th face of a certain convex polyhedron is P _i , and the external normal vector of the i-th face is Note that the point to be detected is P _T , then the necessary and sufficient condition for the point P _T to be located in this convex polyhedron is: for all faces of the convex polyhedron, there are

If there is a point to be detected inside the obstacle, it is considered that the robot arm collides with the obstacle, otherwise it is considered that the robot arm will not collide with the obstacle.

2. Trajectory planning of fine capture segment

The trajectory planning method of the fine capture segment includes the following steps:

(2.1) Pre-design the motion track, velocity track and acceleration track of the end effector position/posture from the position/posture of the end effector of the desired capture target and the position/posture of the current end effector;

After the rough capture segment is over, use the visual sensor on the space operation platform to measure the position/attitude of the capture point on the target to obtain the position/attitude of the end effector of the desired capture target, and compare it with the current end effector The position/attitude of the end effector is compared, and then an end effector position/attitude trajectory, velocity trajectory and acceleration trajectory are designed to connect the expected value and the current value.

The velocity and acceleration trajectories of the pre-designed end-effector position/attitude are 0 at the start and end of the operation.

(2.2) According to the relative motion of the target measured by the visual measurement sensor, the motion track of the position/posture of the end effector, the velocity track and the acceleration track are compensated to obtain the motion track of the position/posture of the end effector after compensation, and the speed track and acceleration trajectory; thereby realizing real-time compensation for the relative motion of the target;

(2.3) According to the motion track of the position/attitude of the end effector after compensation, the velocity track and the acceleration track adopt the pseudo-inverse algorithm based on kinematics to obtain the angular velocity track and the angular acceleration track of each joint angle of the mechanical arm; The position and attitude of the robot follow the pre-designed motion trajectory, velocity trajectory and acceleration trajectory of the manipulator.

公式为：The angular velocity trajectory of each joint angle of the manipulator

The formula is:

${\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; [</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span>$

为补偿后的末端作用器位置/姿态的运动轨迹和速度轨迹，X_E(t)为机械臂实际运动过程中末端作用器的位置/姿态；δ·(X_Er(t)-X_E(t))为加入的比例调节项，可以对实际运动与预先设计的轨迹的跟随误差进行补偿，增加系统的抗干扰能力和鲁棒性，其中δ＞0；J⁺＝J^T(JJ^T)^-1为雅克比矩阵J的伪逆；Among them, X _Er (t), and

is the motion trajectory and velocity trajectory of the compensated end effector position/attitude, X _E (t) is the position/attitude of the end effector during the actual movement of the manipulator; δ·(X _Er (t)-X _E (t )) is the added proportional adjustment item, which can compensate the following error between the actual motion and the pre-designed trajectory, and increase the anti-interference ability and robustness of the system, where δ>0; J ⁺ =J ^T (JJ ^T ) ^{- 1} is the pseudo-inverse of the Jacobian matrix J;

The superscript "·" represents the derivative operation; the superscript "T" represents the transpose operation; the superscript "-1" represents the inverse operation.

The trajectory of joint angular acceleration is:

${\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; [</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span><span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; J</span>}}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; [</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; Er</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; E.</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; ]</span>$

为雅克比矩阵J的伪逆的时间导数。in, is the acceleration trajectory of the compensated end effector position/attitude;

is the time derivative of the pseudoinverse of the Jacobian matrix J.

(2.4) Utilizing the redundant degrees of freedom of the manipulator, idling is used to keep the manipulator away from the singular configuration.

Utilizing the redundant degrees of freedom of the manipulator, using idling to keep the manipulator away from the singular configuration, the final joint angular velocity is

${\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; r</span>}}<span\; class="notranslate">\; =</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; rmp</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; J</span>}}\mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; ,</span>$

ξ(q)为奇异度量：ξ(q)＝det(JJ^T)；标量参数α选取为：

上标“·”代表求导运算；上标“T”代表求转置运算；上标“-1”代表求逆运算；

代表标量ξ对矢量q求梯度。in Be the basic solution of the inverse kinematics obtained in step (203); P _J = (IJ ^T (JJ ^T ) ^-1 J) is the orthogonal projection matrix of Jacobian matrix J; Column vector y=αv;

ξ(q) is a singular metric: ξ(q)=det(JJ ^T ); the scalar parameter α is selected as:

The superscript "·" represents the derivative operation; the superscript "T" represents the transpose operation; the superscript "-1" represents the inverse operation;

Represents the scalar ξ to find the gradient of the vector q.

Example 1

Embodiment 1 is mainly used to verify the validity of the algorithm of the rough capture segment.

(1) A 7-DOF manipulator is designed. The geometric parameters of the manipulator are shown in Table 1, where the installation point of the first section of the arm is expressed in the coordinate system of the base. The initial configuration and the positive direction of each joint angle when each joint angle is 0 are shown in Figure 4. The black line segment in the figure represents the mechanical arm, the black circle represents the joints connecting each segment of the mechanical arm, and the arrow next to the joint indicates the positive direction of each joint rotation angle. direction. At this time, the coordinate system of the base coincides with the body coordinate system of each section arm, see the coordinate system oxyz in Fig. 4 . In Fig. 4, 6 is the base installation point of the robotic arm, and 7 is the end effector of the robotic arm, that is, the seventh segment arm. The following takes the robotic arm as an example to verify the effectiveness of the algorithm for the rough capture segment.

Table 1 The installation point position parameters of each section arm

(2) Let the initial configuration of the manipulator be q _S , and each joint angle be (-1.5707, 3.1414, 1, 0, 0, 0, 0) rad. The following parameters are described in the base coordinate system: the end point position of the end effector at the end of the expected rough capture segment is (2.0, 0.3, 0.5) m; the cosine of the end effector axis direction at the end of the expected rough capture segment is (0.4699 , -0.1987, 0.8601); the obstacle is a cuboid whose edges are parallel to the coordinate axis, and the coordinate range is χ: 1.2~1.7m, y: 0~0.5m, z: 0~0.5m. Note that the target pose has no constraints on the rotation of the end effector along the axis, and the axis of the seventh joint arm is parallel to the rotation axis of the seventh joint angle, so the seventh joint angle will not move, which is essentially a 6-DOF mechanical arm that satisfies 5-DOF end-effector pose requirements.

(3) Firstly, a target configuration qE that satisfies the pose requirements of the end effector and does not collide with obstacles is solved by the non-collision target configuration algorithm. The joint angles are (-0.0567, 4.6127, 1.4569, - 2.1691, 0.9561, -0.3024, 0) rad.

(4) Next, a collision-free path search algorithm is used to search for a path that does not collide with obstacles. The path separation constant is N=10, and the angle of the seventh joint does not change. In essence, it is a search problem in 6-dimensional space, which is divided into 6N = 60 sub-movements, the function F(i) uses incremental heuristics. After 64 trials, find a collision-free 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. It can be seen that the front part of the sequence is the same as the trial order without adjustment, and the algorithm adjusts the trial order at the back of the sequence to avoid obstacles. The collision-free path obtained here is not the final result, but in order to illustrate the necessity of the path optimization algorithm, here we give the motion process of the manipulator on the unoptimized collision-free motion sequence and the motion curve of the joint angle. After obtaining the joint angle movement sequence of the manipulator, it is also necessary to give the movement law of the joint angle required by the manipulator to complete each step in the sequence. Set the movement time of each sub-motion to 4s, and the joint angular motion law adopts a polynomial about time to ensure that the joint angular velocity and angular acceleration are 0 at the beginning and end of each sub-motion. The movement process of the mechanical arm is shown in Figure 5. In the figure, the position of the mechanical arm is recorded every once in a while. In the figure, 8 is the initial configuration of the mechanical arm, 9 is the target configuration at the end of the rough capture section, and 6 is the position of the mechanical arm. Base mounting points, 10 for obstacles. As shown in Fig. 5, the robot arm reaches the target configuration from the bottom around the obstacle. Figure 6 shows the angular velocity curves of each joint angle. It can be seen from Figure 6 that the joint angular velocity is not smooth enough in general. Therefore, it is necessary to optimize this path.

(5) Next, the path smoothing algorithm is used to smooth the polyline path obtained above. It was found that the original sequence of 60 sub-movements only needs to be divided into two steps to avoid obstacles. The first step is from the initial configuration to the intermediate configuration, and the second step is from the intermediate configuration to the target configuration. The configuration is shown in Table 2. After obtaining the initial configuration and the final configuration of each step, it is also necessary to give the motion law of the joint angle required by the manipulator to complete each step. The motion time of each step is set to 20s, and the joint angular motion law adopts a polynomial about time to ensure that the joint angular velocity and angular acceleration are 0 at the beginning and end of each step. The movement process of the mechanical arm is shown in Figure 7. In the figure, the position of the mechanical arm is recorded every once in a while. In the figure, 8 is the initial configuration of the mechanical arm, 9 is the target configuration at the end of the rough capture segment, and 6 is the mechanical arm. The base mounting point of the arm, 10 is an obstacle, and 11 is the middle configuration of the mechanical arm. The robot arm moves from the initial configuration 8 to the intermediate configuration 11 in the first step, and moves from the intermediate configuration 11 to the target configuration 9 in the second step. It can be seen from Fig. 7 that the robotic arm bypasses the obstacle from the bottom to reach the target configuration, and the robotic arm does not collide with the obstacle during these two steps of movement. Figure 8 shows the angular velocity curves of each joint angle. It can be seen from Figure 8 that compared with Figure 6, the motion law of each joint angle of the manipulator is very smooth, easy to implement, and the operation time is greatly reduced, which is convenient for fast capture of non-cooperative targets.

Table 2 Three key configurations on the path after smoothing (rad)

Example 2

Embodiment 2 provides the control method of the entire capture process of the redundant space manipulator.

(1) Install the robotic arm in Example 1 on a space operation platform to capture a non-cooperative target. First, according to the requirements of the capture task and the measurement of the sensor, the capture point on the non-cooperative target is determined, and the position and attitude of the end effector required for capturing the target are calculated.

(2) Divide the whole capture process into a rough capture segment and a fine capture segment. The initial state of the rough capture segment is the current configuration of the robotic arm, and the end state of the rough capture segment is set as the end effector of the mechanical arm is located at a certain distance above the capture point (for example, 20cm).

(3) Based on the position and attitude of the end effector at the end of the rough capture segment set in the previous step, the non-collision target configuration solution algorithm is used to obtain the non-collision target configuration. Based on the current configuration and the target configuration of the manipulator, a smooth collision-free joint angular motion sequence is obtained by a collision-free path search algorithm and a path optimization algorithm. According to the time requirements of the capture task, set the motion time of each step in the collision-free motion sequence (for example, each step takes 20s). Then, according to the set motion time, the joint angular motion rule is designed as a polynomial function of time to ensure that the joint angular velocity and angular acceleration are 0 at the beginning and end of each step. Subsequently, the control system of the space operation platform adopts a certain control method (such as a PID controller) to drive the motors at the joints of the manipulator to follow the given joint angle motion law, so as to achieve the goal of the rough capture segment, and move the manipulator The end effector of moves to a certain distance above the grab point without collision.

(4) After the rough capture section is over, the sensor on the space operation platform measures the position of the capture point at this time, and designs the motion trajectory, velocity trajectory and acceleration trajectory of the position and attitude of the end effector. The pseudo-inverse algorithm of kinematics calculates the position, velocity and acceleration information of the joint angle of the manipulator in real time. The control system of the space console adopts a certain control method (such as a PID controller) to drive the motor at the joint of the manipulator. Follow the joint angle motion law calculated by the real-time solution. At the same time, the sensor on the space operation platform measures the movement of the capture point on the target relative to the space operation platform, and compensates the relative movement of the target in the pseudo-inverse algorithm, so as to achieve the final precise capture, that is, the mechanical arm Move the end effector precisely to the snap point on the target.

This specific implementation part realizes the trajectory planning without collision and without singularity for the target capture of the redundant mechanical arm with 7 degrees of freedom, which meets the engineering requirements, is easy to apply in engineering, and has a good promotion prospect.

The content that is not described in detail in the description of the present invention belongs to the well-known technology of those skilled in the art.

Claims (8)

1. A redundant space manipulator on-orbit capture track planning method; the redundant space manipulator is arranged on the space control platform, and the tail end of the manipulator is provided with an end effector; the space control platform is provided with a vision measuring sensor and a star-loaded computer; the on-orbit capture object of the redundant space manipulator is a space non-cooperative target; it is characterized in that the catching operation of the space manipulator is divided into a rough catching section and a fine catching section,

the track planning method of the rough capture segment comprises the following steps:

(1.1) obtaining a collision-free target configuration by using a collision-free target configuration solving algorithm according to the expected position and the attitude of the end effector of the rough catching section;

(1.2) according to the current configuration and the target configuration of the mechanical arm, solving a path in a collision-free configuration space by using a collision-free path search algorithm;

(1.3) optimizing a path in a collision-free configuration space by using a path optimization algorithm to obtain a motion sequence of each joint angle of the mechanical arm, which does not collide with an obstacle;

the track planning method of the fine capture segment comprises the following steps:

(2.1) designing a motion track, a speed track and an acceleration track of the position/posture of the end effector in advance according to the position/posture of the end effector of the expected capture target and the position/posture of the current end effector;

(2.2) compensating the motion track, the speed track and the acceleration track of the position/posture of the end effector according to the relative motion of the target measured by the vision measuring sensor to obtain the compensated motion track, the compensated speed track and the compensated acceleration track of the position/posture of the end effector;

and (2.3) according to the compensated motion track, velocity track and acceleration track of the position/posture of the end effector, obtaining the angular velocity track and angular acceleration track of each joint angle of the mechanical arm by adopting a pseudo-inverse algorithm based on kinematics.

2. The method for planning the on-orbit capture trajectory of the redundant space manipulator according to claim 1, characterized in that: in the step (1.1), the collision-free target configuration solving algorithm is as follows:

firstly, calculating a target configuration according to a target configuration solving algorithm;

the input of the target configuration solving algorithm is the current joint angles of the mechanical arm and the position and the posture of the end effector required by the target capture, and the target configuration solving algorithm utilizes a numerical integration method to solve the inverse kinematics equation of the mechanical arm to obtain the joint angles required by the target capture, namely the target configuration;

and then judging whether the calculated target configuration collides with an obstacle or not by using a collision detection algorithm, if so, adjusting the current joint angle of the mechanical arm input to the target configuration solving algorithm, and recalculating according to the target configuration solving algorithm until a collision-free target configuration is obtained.

3. The method for planning the on-orbit capture trajectory of the redundant space manipulator according to claim 1, characterized in that: in the step (1.2), the collision-free path search algorithm averagely divides the total rotation angle of each joint angle from the current configuration to the target configuration into N parts, and defines that each joint angle rotates through each rotation angle as a sub-motion, so that a pN sub-motion is formed, wherein p is the number of joints;

recording the sub-motion of the ith joint angle as a sub-motion i, i is 1, 2.. and p, arranging the pN sub-motions in a proper sequence by using a collision-free path search algorithm to form a motion sequence of pN steps;

setting a sequence number generation function F (i), wherein the function generates a sub-motion sequence number to be explored next step from a current sub-motion sequence number i; f (i) is an increasing, decreasing or randomly generated function;

m denotes the step number, T denotes the total number of trials, T_maxIndicating an acceptable maximum number of probes;

the collision-free path search algorithm comprises the following steps:

1) initializing, making m equal to 1 and T equal to 1, selecting i, and setting T_max；

2) Judging whether m is equal to pN + 1;

if m is equal to pN +1, outputting a collision-free joint motion sequence, and ending the search algorithm; otherwise, judging whether the m-th execution sub-motion i of the mechanical arm collides with the barrier by using a collision detection algorithm; if not, the mechanical arm executes the sub-motion i, records the sub-motion i and executes the step 3); if collision happens, executing step 4);

3) m +1, i f (i), T +1, performing step 6);

4) judging whether the mth step has tried all possible sub-movements i, if yes, executing the step 5); otherwise, executing step 6), wherein i is f (i), and T is T + 1;

5) jumping back to the step m-1, and enabling m to be m-1; judging whether the mth step detects all possible sub-motions, if so, executing the step 5 again); otherwise, let i ═ F (i) (m)), i (m) refer to the original sub-motion sequence number in the mth step, and T ═ T +1, execute step 6);

6) judging whether T is equal to T_max，

If T ═ T_maxIf so, outputting the algorithm search failure and finishing the search algorithm, otherwise, executing the step 2);

if the algorithm search fails, adopting different heuristic rules F (i) or increasing the path division constant N to execute the collision-free path search algorithm again.

4. The method for planning the on-orbit capture trajectory of the redundant space manipulator according to claim 1, characterized in that: the path optimization algorithm method in the step (1.3) is as follows: combining two adjacent sub-motions in the motion sequence of each joint angle of the collision-free mechanical arm searched by the collision-free path search algorithm into one sub-motion, and then detecting whether the combined sub-motions collide with the obstacle by adopting a collision detection algorithm; if not, the two motions are combined; if the two are in collision, the combination is cancelled; and repeating the process until adjacent sub-motions in the obtained motion sequence cannot be further combined.

5. The method for planning the track of the on-orbit capture of the redundant space manipulator according to any one of claims 2 to 4, wherein the method comprises the following steps: the collision detection algorithm method is as follows: simplifying the mechanical arm into a plurality of connected line segments, then carrying out spatial discretization, discretizing into a series of points on the mechanical arm, dividing the barrier into a plurality of parts, finding an envelope of a convex polyhedron for each part, and then approximately replacing the barrier by the convex polyhedrons; let the center point of the ith surface of a convex polyhedron be P_iThe outer normal vector of the ith plane is

Note that the point to be detected is P_TThen point P_TSufficient requirements to be located in this convex polyhedron are: for all faces of a convex polyhedron there is

If the point to be detected is located inside the obstacle, the mechanical arm is considered to collide with the obstacle, otherwise, the mechanical arm is considered not to collide with the obstacle.

6. The method for planning the on-orbit capture trajectory of the redundant space manipulator according to claim 1, characterized in that: the angular velocity trajectory of each joint angle of the mechanical arm in the step (2.3)

The formula is as follows:

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

wherein, X_Er(t) and

for the compensated end effector position/attitude motion and velocity trajectories, X_E(t) is the position/attitude of the end effector during the actual motion of the robotic arm; delta. (X)_Er(t)-X_E(t)) is an added proportion adjustment term, wherein delta is more than or equal to 0; j. the design is a square⁺＝J^T(JJ^T)^-1Is the pseudo-inverse of Jacobian matrix J;

joint angular acceleration trajectories of respective joint angles of the mechanical arm

Comprises the following steps:

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

wherein,

an acceleration trajectory for the compensated end effector position/attitude;

the time derivative of the pseudo-inverse of the jacobian matrix J.

7. The method for planning the on-orbit capture trajectory of the redundant space manipulator according to claim 1, characterized in that: the track planning method of the fine capture segment further comprises the following steps:

the redundant degree of freedom of the mechanical arm is utilized, and idling is adopted to enable the mechanical arm to be far away from a singular configuration.

8. The method for planning the on-orbit capture trajectory of the redundant space manipulator according to claim 7, wherein the method comprises the following steps:

the angular velocity trajectory of each joint angle of the robot arm with idle rotation is

${{\stackrel{\&CenterDot;}{q}}_r}^{\&prime;}={\stackrel{\&CenterDot;}{q}}_r\left(t\right)+P_{J}y,$

P_J＝(I-J^T(JJ^T)^-1J) An orthogonal projection matrix which is a Jacobian matrix J; column vector y ═ α v;

ξ (q) is the singular metric: ξ (q) ═ det (JJ)^T) (ii) a The scalar parameter α is chosen as:

superscript "-" represents a derivation operation; superscript "T" represents the transpose operation; the superscript "-1" represents the inversion operation;

the representative scalar ξ gradients the vector q.

Images