CN104999463B - A kind of redundant mechanical arm motion control method based on configuration plane - Google Patents

Abstract

The invention relates to the field of robot technology, and provides a redundant mechanical arm motion control method based on a configuration plane, which can ensure that the redundant mechanical arm can move in a complex workspace under multi-objective conditions such as space obstacle avoidance control. The invention includes the following steps: using dynamics to divide the configuration plane of the redundant manipulator; performing preliminary planning of the redundant manipulator according to the matching mode of the configuration plane; performing spatial planning for each configuration plane of the planned redundant manipulator Obstacle interference detection, re-adjust and plan the configuration plane that generates interference; perform joint speed and acceleration spline smoothing on the adjusted redundant manipulator planning; perform dynamic calibration on the entire motion process. The invention combines the structural characteristics and working mode of the redundant mechanical arm, and introduces the configuration plane into the motion control of the redundant mechanical arm. The method can intuitively and quickly realize the motion control planning of the redundant mechanical arm.

Description

A Motion Control Method of Redundant Manipulator Based on Configuration Plane

technical field

The invention relates to the field of robot technology, in particular to a motion control method of a redundant manipulator based on a configuration plane, which can ensure the motion control of a redundant manipulator in a complex workspace under multi-objective conditions such as space obstacle avoidance.

Background technique

For spatial tasks, the dimensionality of the workspace is six considering the position and attitude of the tip. Redundant manipulator redundancy is greater than 1. The research of redundant manipulators has become a research hotspot in the field of robotics.

Many experts have proposed many methods in terms of motion control of redundant manipulators. The free space method based on C space establishes C space with the joint coordinate system of the manipulator, maps obstacles to C space, forms space configuration obstacles, and obtains the complement of C space, that is, free space. On this basis, the heuristic search algorithm is used to find the motion path of the manipulator in the free space of the manipulator. Although this method can realize the collision-free path planning of the manipulator, it is difficult to meet the real-time requirements for complex environments due to the complexity of mapping obstacles to C space. In order to achieve spatial obstacle avoidance, a repulsive potential field is defined for obstacles, and an attractive potential field is defined at the target point. The movement of the mechanical arm is determined by the combined force of the two potential fields, thereby ensuring that the mechanical arm can avoid obstacles smoothly. reach the final destination point. This method is very effective for dealing with dynamic obstacle avoidance in global path planning, but it is easy to fall into the local minimum point.

In the motion control of the robot, the angular acceleration constraints, angular velocity constraints and angle constraints of each joint are the main constraints involved. In the case of general low-speed motion, it is enough to ensure that the joint angle does not exceed the limit, which has little impact on motion trajectory planning. However, when the robot moves at a fast speed, the joint angular acceleration and angular velocity can easily exceed the constraint range, resulting in accidents where the driving current is too large or exceeds the limit. In the slightest, the movement of the robot is wrong, and in the severest case, the hardware is damaged. At this time, various constraints must be considered comprehensively when planning the trajectory of the robot. The most common method is to optimize the movement time. For example, some researchers imposed joint velocity constraints and moment constraints on the trajectory of the robotic arm when intercepting fast-flying objects with the robotic arm. Some scholars have deduced the analytical inverse solution of the 7R manipulator with a special joint configuration, and made all joints meet their joint angle constraints. The main work is to solve the working space of the anthropomorphic manipulator. Many scholars have carried out research on the motion planning of humanoid reaching point of 7R manipulator. It is also possible to divide the workspace into small grids, obtain the anthropomorphic joint angles in each small grid, establish a lookup table, find the redundant angle q3 in the lookup table according to the required end position, and calculate other angles according to the analytical inverse solution joint angle. MINH uses motion elements to realize anthropomorphic motion. But the focus of these research works is on the point movement in humanoid, and does not consider obstacle avoidance. In order to improve its search efficiency,

Contents of the invention

The purpose of the present invention is to provide a method for controlling inverse motion of a redundant mechanical arm. This method does not depend on the configuration of the robot, is simple in form, reduces the amount of calculation, can obtain optimal solutions as required, and is universal and fast.

The purpose of the present invention is achieved like this:

A method for controlling motion of a redundant manipulator using a configuration plane, comprising the steps of:

Step 1: Given the structural parameters of the redundant manipulator, use dynamics to divide the configuration plane of the redundant manipulator;

Step 2: Carry out the preliminary planning of the redundant manipulator according to the method of configuration plane matching, and the configuration plane matching is carried out by weighted space vector method;

Step 3: Perform spatial obstacle interference detection on each configuration plane of the planned redundant manipulator, and readjust and plan the configuration plane that generates interference;

Step 4: Perform spline smoothing of joint speed and acceleration on the adjusted redundant manipulator planning;

Step 5: Carry out dynamic calibration for the whole movement process.

In the configuration plane division of the redundant mechanical arm using dynamics, the first joint in the configuration plane is a rotary joint, the last joint is a swing joint or a moving joint, and the number of swing joints and moving joints is the configuration plane The number of degrees of freedom in .

The two-dimensional plane where the configuration plane is located cross-cuts the three-dimensional space obstacle into a screenshot graphic, expands the distance k in the obstacle area 1, and the safe obstacle avoidance area is composed of m line segments, then the i-th line segment is:

The configuration of the robot is composed of n segments, then the jth segment is:

Compare whether the slopes of the two line segments are equal, if they are not equal, find the intersection of the straight lines where the two line segments are located,

The interfering joints in the configuration plane are readjusted to make judgments between the interfering joints and the boundary line segment of the safety obstacle area near the robot base.

Under the conditions of the known joint constraints of the manipulator, the trajectory of the end of the manipulator, and the position of the space obstacle, the configuration plane plans the space trajectory of the manipulator by forming a redundant manipulator:

The space joint constraints are:

The spatial position of the configuration plane is determined using the weighted space vector method, P _i is the i-th point in the trajectory planning of the end of the manipulator, connected to P _i O ₀ , k _j (j=1,...n) is the configuration plane i The connection line from the start point to the end point;

Guided by the P _i O ₀ vector, plan k _j (j=1,…n) of the configuration plane; under the condition of ensuring the working space of the manipulator, plan k _j (j=1,…n-1); for For the part where the configuration plane overlaps and interferes with the obstacle, the method introduced is used to check the interference judgment between the manipulator joint and the obstacle in the configuration plane.

The degree of freedom of the configuration plane is n, the joint a is the first joint forming the kth configuration plane, the joint b is a rotary joint, and the joint b is divided into the configuration plane k+1

^a ω _a is the angular velocity of joint a, the degree of freedom of the configuration plane is n, the velocity ^a ω _a of the starting point of the configuration plane is known, Obtain the velocity ^a+n ω _a+n of the last joint of the configuration plane, ^a+n v _a+n ,

Establish coordinates at the centroid k _c of configuration plane k, this coordinate is parallel to the coordinates at the starting point of this configuration plane, Indicates the coordinates of the centroid k _c in the coordinate system a+n, then

The coordinates from the configuration plane centroid k _c to the centroid of each joint are to a+n, Then the coordinates of the centroids of each joint to the centroid k _c of the configuration plane are

The force and moment are

Known force ^a+n f _a+n , moment Find ^a f _a ,

Joint a is the last joint of the configuration plane, then joint a+1 is the first joint of the next adjacent configuration plane, let ^a ω _a , is the angular velocity and angular acceleration of the last joint in the previous configuration plane, i=1, the configuration plane where joint a is located is the first configuration plane, then ^a ω _a , is the angular velocity and angular acceleration of the base mark, and the base mark does not move, then

last joint The end operator is free in space, then M _end and F _end are equal to zero. ;

make That is, the supporting effect on the robot base is equivalent to the upward gravitational acceleration g.

Compared with the prior art, the present invention has the beneficial effects of:

(1) Combining the structural characteristics and working methods of the redundant manipulator, the configuration plane is introduced into the motion control of the redundant manipulator. This method can intuitively and quickly realize the motion control planning of the redundant manipulator.

(2) Based on the configuration plane, the spatial position of the configuration plane is determined by means of space vector guidance, and then a more reasonable spatial configuration of the redundant manipulator is determined. This method avoids the dependence on robot configuration form and degrees of freedom by traditional analytical methods, and also avoids the problems of solution accuracy and solution speed of numerical methods, and has practicality and versatility.

(3) Dynamics method based on configuration plane. This method takes the configuration plane as the unit, reduces the calculation and solution steps, can quickly check the joint motion performance of the manipulator during the planning process, and provides a basis for optimizing and adjusting the manipulator planning.

Description of drawings

Fig. 1 is a schematic drawing of configuration plane division;

Fig. 2 Obstacle interference detection diagram in configuration plane;

Figure 3 is a diagram for determining the position of the space configuration plane;

Figure 4 motion control flow chart.

detailed description

The present invention will be described in more detail below in conjunction with the accompanying drawings.

The invention discloses a method for controlling the movement of a redundant mechanical arm by using a configuration plane. The motion control of redundant manipulators is a complex process limited by the joint constraints of manipulators and space obstacles. The present invention starts from the configuration plane of the redundant manipulator, uses the method of space geometry to control the spatial motion of the redundant manipulator, and quickly finds the space-optimized path through space vector guidance, comparison of obstacle avoidance paths, and dynamic calibration , to implement a multi-objective trajectory planning method. The method does not depend on the configuration of the robot, has a simple form, reduces the difficulty of solving, reduces the amount of calculation, and can obtain optimal solutions as required, and is universal and fast.

The technical scheme adopted in the present invention is:

Step 1: Given the structural parameters of the redundant manipulator, use dynamics to divide the configuration plane of the redundant manipulator.

Step 2: Carry out the preliminary planning of the redundant manipulator according to the configuration plane matching method, and the configuration plane matching is carried out by the weighted space vector method.

Step 3: Perform space obstacle interference detection on each configuration plane of the planned redundant manipulator, and readjust and plan the configuration plane that generates interference.

Step 4: Smooth the spline curves of joint speed and acceleration for the adjusted redundant manipulator plan to ensure the smooth movement of the manipulator during the movement process.

Step 5: Carry out dynamic calibration for the entire movement process to avoid overload and overspeed of the redundant robotic arm during the movement process.

A method for motion control of redundant manipulators using configuration planes, space motion control of redundant manipulators, through space vector guidance, comparison of obstacle avoidance paths, and dynamic calibration, to quickly find the space-optimized path and realize multiple Target trajectory planning method.

The overall steps are as follows:

Step 1: Given the structural parameters of the redundant manipulator, use dynamics to divide the configuration plane of the redundant manipulator.

Step 2: Carry out the preliminary planning of the redundant manipulator according to the configuration plane matching method, and the configuration plane matching is carried out by the weighted space vector method.

Step 3: Perform space obstacle interference detection on each configuration plane of the planned redundant manipulator, and readjust and plan the configuration plane that generates interference.

Step 4: Smooth the spline curves of joint speed and acceleration for the adjusted redundant manipulator plan to ensure the smooth movement of the manipulator during the movement process.

Step 5: Carry out dynamic calibration for the entire movement process to avoid overload and overspeed of the redundant robotic arm during the movement process.

Configuration plane partitioning of redundant manipulators using dynamics. The first joint in the configuration plane is a swivel joint, and the last joint is a swing or translation joint. The first joint of the first configuration plane may not be a swivel joint. If the last joint of the tandem robot is a revolving joint, this joint is regarded as a configuration plane alone. In the planar configuration, the revolving joint has no effect on the distance relationship between the configuration end and the configuration center. It is the same as the connecting rod. Only the swing joint and the moving joint can change the position relationship between the configuration end and the configuration center. Therefore, the swing joint and the number of moving joints is the number of degrees of freedom in the configuration plane.

The three-dimensional space obstacle is cross-cut by the two-dimensional plane where the configuration plane is located. This cross-sectional figure may be very irregular, and it is covered by relatively simple straight line segments connected in sequence, as shown in Figure 4. Considering the robot connecting rod It has external dimensions and safety distance, so it expands the distance k in the obstacle area 1, so that the actual area of the obstacle becomes the state of area 2.

Assuming that the safe obstacle avoidance area is composed of m line segments, then the i-th line segment can be expressed by the following formula:

The configuration of the robot is composed of n segments, and the jth segment is expressed by the following formula:

In plane geometry, two straight lines are either parallel or intersecting. Therefore, in order to improve the efficiency of the judgment process, first compare the slopes of the two line segments shown in formula (1) and formula (2). It is easy to judge whether the intersection point is between the two line segments.

According to the trajectory planning process of the redundant manipulator, the center and end of the configuration plane cannot be in the obstacle area, otherwise the redundant manipulator cannot complete the work. Therefore, there is an obstacle interference situation, which is the interference between the joint part of the robot and the boundary line segment that constitutes the safety obstacle area. In order to improve the calculation efficiency of the overall robot planning, it is not necessary to make judgments between all the robot joints and all the line segments that make up the safety obstacle area. It is only necessary to readjust the interfering joints in the configuration plane so that the interfering joints and the safety obstacle Judgment is made between the boundary line segments of the area close to the robot base side.

The trajectory planning process based on configuration plane strives to be fast and concise, avoiding complicated calculation process. Under the conditions of the known joint constraints of the manipulator, the trajectory of the end of the manipulator, and the position of space obstacles, the spatial trajectory of the manipulator is planned through the configuration planes that make up the redundant manipulator.

The space joint constraints are as follows:

The spatial position of the configuration plane is determined using the weighted space vector method, as shown in Figure 3, in which P _i is the i-th point in the trajectory planning of the end of the manipulator, connected to P _i O ₀ , k _j (j=1,… n) is the connecting line from the start point to the end point of configuration plane i.

Guided by the P _i O ₀ vector, plan k _j (j=1,...n) of the configuration plane. The value of k ₁ is related to the initial joint, usually the direction of its space vector is known, while k _n is related to the end joint of the manipulator, and the direction and size of its space vector are fixed. Under the condition of ensuring the working space of the manipulator, plan k _j (j=1,...n-1). Since k _j (j=1,...n-1) is not the central axis of the actual manipulator joint, k _j (j=1,...n-1) may overlap and interfere with space obstacles. For the overlapping interference between the configuration plane and the obstacle, the method introduced is used to check the interference between the manipulator joint and the obstacle in the configuration plane. The actual calculation will not be so complicated, because the configuration plane has simplified the joints with redundant functions in the manipulator, and the number of configuration planes that make up the manipulator is small, usually around 3-5. After planning a small number of configuration planes, The spatial positions of the remaining two configuration planes can be determined analytically.

In-plane dynamics analysis

As shown in the schematic diagram of the kth configuration plane in Fig. 2, the degree of freedom of the configuration plane is n, and the joint a is the first joint forming the kth configuration plane. Since the joint b is a rotary joint, considering the angular velocity , when the angular acceleration superposition formula is used, for the convenience of calculation, the joint b is divided into the configuration plane k+1.

1) Speed formula

^a ω _a is the angular velocity of the joint a, assuming that the degree of freedom of the configuration plane is n, the velocity ^a ω _a of the starting point of the configuration plane is known, Calculate the velocity ^a+n ω _a+n of the last joint of the configuration plane by the velocity formula, ^{a+ n} v _a+n ,

As shown in Figure 2, the coordinates are established at the centroid k _c of the configuration plane k, and this coordinate is set to be parallel to the coordinates at the starting point of the configuration plane, Indicates the coordinates of the centroid k _c in the coordinate system a+n, then

Let the coordinates from the center of mass k _c of the configuration plane to the center of mass of each joint be (j=a to a+n), Then the coordinates of the centroids of each joint to the centroid k _c of the configuration plane are

2) Force and moment formulas

Known force ^a+n f _a+n , moment Find ^a f _a ,

(2) Dynamic analysis between configuration planes

Let joint a be the last joint of the configuration plane, then joint a+1 is the first joint of the next adjacent configuration plane

1) Speed formula

In formula (7) and formula (8), let ^a ω _a , is the angular velocity of the last joint in the previous configuration plane, angular acceleration, i=1, then the angular velocity of the first joint in the next adjacent configuration plane can be obtained. If the configuration plane where the joint a is located is the first configuration plane, then ^a ω _a , is the angular velocity and angular acceleration of the base mark, if the base mark does not move, then

2) Force and moment formulas

The last joint (the number of all joints of the robotic arm is r) If the end manipulator is free in space, M _end and F _end are equal to zero.

When considering the influence of the weight of the connecting rod itself, let That is, the supporting effect on the robot base is equivalent to the upward gravitational acceleration g. This is done exactly the same as the effect of each module's gravity.

The present invention summarizes the structural characteristics of redundant manipulators in series, based on the configuration plane, and aims at the specific working configuration of redundant manipulators in series. The method is simple in form, has high precision and solution speed, and has strong practicability and generality.

Implementation 1, combined with Figure 1, uses dynamic redundant manipulator configuration plane division. The first joint in the configuration plane is a swivel joint, and the last joint is a swing or translation joint. The first joint of the first configuration plane may not be a swivel joint. If the last joint of the tandem robot is a revolving joint, this joint is regarded as a configuration plane alone. In the planar configuration, the revolving joint has no effect on the distance relationship between the configuration end and the configuration center. It is the same as the connecting rod. Only the swing joint and the moving joint can change the position relationship between the configuration end and the configuration center. Therefore, the swing joint and the number of moving joints is the number of degrees of freedom in the configuration plane.

Implementation 2, combined with Figure 2, the three-dimensional space obstacle is cross-cut by the two-dimensional plane where the configuration plane is located. The connecting rod has external dimensions and a safety distance, so it expands a distance k in the obstacle area 1, so that the actual area of the obstacle becomes the state of the area 2.

Assuming that the safe obstacle avoidance area is composed of m line segments, then the i-th line segment can be expressed by the following formula:

The configuration of the robot is composed of n segments, and the jth segment is expressed by the following formula:

In plane geometry, two straight lines are either parallel or intersecting. Therefore, in order to improve the efficiency of the judgment process, first compare the slopes of the two line segments shown in formula (1) and formula (2). It is easy to judge whether the intersection point is between the two line segments.

According to the trajectory planning process of the redundant manipulator, the center and end of the configuration plane cannot be in the obstacle area, otherwise the redundant manipulator cannot complete the work. Therefore, there is an obstacle interference situation, which is the interference between the joint part of the robot and the boundary line segment that constitutes the safety obstacle area. In order to improve the calculation efficiency of the overall robot planning, it is not necessary to make judgments between all the robot joints and all the line segments that make up the safety obstacle area. It is only necessary to readjust the interfering joints in the configuration plane so that the interfering joints and the safety obstacle Judgment is made between the boundary line segments of the area close to the robot base side.

Implementation 3, combined with Figure 3, the trajectory planning process based on the configuration plane strives to be fast and concise, avoiding complex calculation processes. Under the conditions of the known joint constraints of the manipulator, the trajectory of the end of the manipulator, and the position of space obstacles, the spatial trajectory of the manipulator is planned through the configuration planes that make up the redundant manipulator.

The space joint constraints are as follows:

Implementation 3, the spatial position of the configuration plane is determined using the weighted space vector method, as shown in accompanying drawing 3, in the figure P _i is the i-th point in the trajectory planning of the end of the manipulator, connected to P _i O ₀ , k _j (j= 1,...n) is the connection line from the start point to the end point of configuration plane i.

Guided by the P _i O ₀ vector, plan k _j (j=1,...n) of the configuration plane. The value of k ₁ is related to the initial joint, usually the direction of its space vector is known, while k _n is related to the end joint of the manipulator, and the direction and size of its space vector are fixed. Under the condition of ensuring the working space of the manipulator, plan k _j (j=1,...n-1). Since k _j (j=1,...n-1) is not the central axis of the actual manipulator joint, k _j (j=1,...n-1) may overlap and interfere with space obstacles. For the overlapping interference between the configuration plane and the obstacle, the method introduced is used to check the interference between the manipulator joint and the obstacle in the configuration plane. The actual calculation will not be so complicated, because the configuration plane has simplified the joints with redundant functions in the manipulator, and the number of configuration planes that make up the manipulator is small, usually around 3-5. After planning a small number of configuration planes, The spatial positions of the remaining two configuration planes can be determined analytically.

Implement 4, the dynamic school kernel method,

As shown in the schematic diagram of the kth configuration plane, the degree of freedom of the configuration plane is n, and joint a is the first joint that forms the kth configuration plane. Since joint b is a rotary joint, considering angular velocity and angular acceleration When superimposing the formula, in order to facilitate the calculation, the joint b is divided into the configuration plane k+1.

1) Speed formula

^a ω _a is the angular velocity of the joint a, assuming that the degree of freedom of the configuration plane is n, the velocity ^a ω _a of the starting point of the configuration plane is known, Calculate the velocity ^a+n ω _a+n of the last joint of the configuration plane by the velocity formula, ^{a+ n} v _a+n ,

Establish coordinates at the centroid k _c of the configuration plane k, let this coordinate be parallel to the coordinates at the starting point of the configuration plane, Indicates the coordinates of the centroid k _c in the coordinate system a+n, then

Let the coordinates from the center of mass k _c of the configuration plane to the center of mass of each joint be to a+n), Then the coordinates of the centroids of each joint to the centroid k _c of the configuration plane are

2) Force and moment formulas

Known force ^a+n f _a+n , moment Find ^a f _a ,

Let joint a be the last joint of the configuration plane, then joint a+1 is the first joint of the next adjacent configuration plane

1) Speed formula

In formula (7) and formula (8), let ^a ω _a , is the angular velocity of the last joint in the previous configuration plane, angular acceleration, i=1, then the angular velocity of the first joint in the next adjacent configuration plane can be obtained. If the configuration plane where the joint a is located is the first configuration plane, then ^a ω _a , is the angular velocity and angular acceleration of the base mark, if the base mark does not move, then

2) Force and moment formulas

The last joint (the number of all joints of the robotic arm is r) If the end operator is free in space, M _end and F _end are equal to zero.

When considering the influence of the weight of the connecting rod itself, let That is, the supporting effect on the robot base is equivalent to the upward gravitational acceleration g. This is done exactly the same as the effect of each module's gravity.

Implementation 5, in conjunction with accompanying drawing 4, its overall steps are as follows:

Step 1: Given the structural parameters of the redundant manipulator, use dynamics to divide the configuration plane of the redundant manipulator.

Step 2: Carry out the preliminary planning of the redundant manipulator according to the configuration plane matching method, and the configuration plane matching is carried out by the weighted space vector method.

Step 3: Perform space obstacle interference detection on each configuration plane of the planned redundant manipulator, and readjust and plan the configuration plane that generates interference.

Step 4: Smooth the spline curves of joint speed and acceleration for the adjusted redundant manipulator plan to ensure the smooth movement of the manipulator during the movement process.

Step 5: Carry out dynamic calibration for the entire movement process to avoid overload and overspeed of the redundant robotic arm during the movement process.

Claims (2)

1. a kind of carry out the method for redundant mechanical arm motor control it is characterised in that comprising the steps with configuration plane：

Step 1：Given redundant mechanical arm configuration parameter, carries out configuration plane division with kinetics to redundant mechanical arm；

Step 2：Mode according to configuration plane coupling carries out planning at the beginning of redundant mechanical arm, the space to weight for the configuration plane coupling Vector method is carried out；

Step 3：The interference detection of spatial obstacle thing is carried out to each configuration plane of the redundant mechanical arm of planning, generation is interfered Configuration plane is readjusted and is planned；

Step 4：Redundant mechanical arm planning after adjustment is carried out with the SPL smoothing processing of the speed in joint, acceleration；

Step 5：Dynamics check is carried out to whole motor process；

First joint in configuration plane in the described configuration plane division with dynamic (dynamical) redundant mechanical arm is that revolution is closed Section, last joint is swinging joint or linear joint, and the quantity of swinging joint and linear joint is in configuration plane Number of degrees of freedom；

Described configuration plane place two dimensional surface, by crosscutting for three dimensions barrier one-tenth sectional drawing figure, extends out in barrier region 1 Apart from k, safe obstacle-avoidance area is made up of m line segment, then i-th section of line segment is：

$z=\frac{(z_i-z_{i-1})(x-x_{i-1})}{x_i-x_{i-1}}+z_{i-1}$

Robot configuration is made up of n section line segment, then jth section line segment is：

$z=\frac{(z_j-z_{j-1})(x-x_{j-1})}{x_j-x_{j-1}}+z_{j-1};$

Whether the slope of relatively two line segments is equal, does not wait the intersection point then solving two lines section place straight line,

Interference joint in this configuration plane is readjusted, and makes interference joint and safety obstacle region near robot base Judged between the boundary sections of seat side.

2. a kind of method carrying out redundant mechanical arm motor control with configuration plane according to claim 1, its feature It is：The degree of freedom of described configuration plane is n, and joint a is first joint of k-th configuration plane of composition, and joint b is revolution Joint, is divided into joint b in configuration plane k+1,

^aω_aFor the angular velocity of joint a, the degree of freedom of configuration plane is n it is known that the speed of the starting point of configuration plane^aω_a, Try to achieve the speed in last joint of configuration plane^a+nω_a+n, ^a+nv_a+n,

${\mathrm{\ω}_{a+n}}_{a+n}={R_a^{a+n}}^a{\mathrm{\ω}}_a+\underset{j=a+1}{\overset{a+n}{\Σ}}{\stackrel{\·}{q}}_j{e_j}_j,$

${\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{a+n}={R_a^{a+n}}^a{\stackrel{\·}{\mathrm{\ω}}}_a+{R_a^{a+n}}^a{\mathrm{\ω}}_a\×\underset{j=a+1}{\overset{a+n}{\Σ}}{{\stackrel{\·}{q}}_j}^j{e}_j+\underset{j=a+1}{\overset{a+n}{\Σ}}{\stackrel{\·\·}{q}}_j{e_j}_j;$

Barycenter k in configuration plane k_cGo out to set up coordinate, this coordinate parallel to the coordinate of this configuration plane starting point,Represent Barycenter k_cCoordinate under coordinate system a+n, then

${\mathrm{\ρ}_{a+n}}_{k_c}\underset{i=a}{\overset{a+n}{\Σ}}{m}_i=\underset{i=a}{\overset{a+n}{\Σ}}R_i^{a+n}{m}_i{\mathrm{\ρ}_i}_i$

${\mathrm{\ω}_{a+n}}_{k_c}={\mathrm{\ω}_{a+n}}_{a+n}$

${\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{k_c}={\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{a+n}$

${\stackrel{\·}{v}_{a+n}}_{k_c}={\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{k_c}\×{\mathrm{\ρ}_{a+n}}_{k_c}+{\mathrm{\ω}_{a+n}}_{k_c}\×({\mathrm{\ω}_{a+n}}_{k_c}\×{\mathrm{\ρ}_{a+n}}_{k_c})+{R_a^{a+n}}^a{\stackrel{\·}{v}}_a$

${F_{a+n}}_{k_c}=\underset{j=a}{\overset{a+n}{\Σ}}{m}_j{\stackrel{\·}{v}_{a+n}}_{k_c}$

Configuration plane barycenter k_cCoordinate to the barycenter in each joint isTo a+n,Then each pass The coordinate of the barycenter of section is to configuration plane barycenter k_cCoordinate be

${I_{k_c}}_{a+n}=\underset{j=a}{\overset{a+n}{\Σ}}R_j^{a+n}({I_{c_j}}_j+m_j({\mathrm{\ρ}_{k_c}}_j\mathrm{\ρ}_{k_c}{{}_j}^T{I}_3-\mathrm{\ρ}_{k_c}{{}_j}^T{\mathrm{\ρ}_{k_c}}_j\left)\right){R_j^{a+n}}^T$

${M_{a+n}}_k={I_{k_c}}_{a+n}{\stackrel{\·}{\mathrm{\ω}}_{a+n}}_k+{\mathrm{\ω}_{a+n}}_k\×{I_{k_c}}_{a+n}{\mathrm{\ω}_{a+n}}_k$

Power and moment are

Known force^a+nf_a+n, momentAsk^af_a,

${M_a}_{J_a}=R_{a+n}^a({M_{a+n}}_{J_{a+n}}+{M_{a+n}}_{k_{a+n}}+{F_{a+n}}_{k_{a+n}}\×{\mathrm{\ρ}_{a+n}}_{k_c});$

Joint a is last joint of configuration plane, then joint a+1 is first of next configuration plane adjacent thereto Joint, order^aω_a、For the angular velocity in last joint of a upper configuration plane, angular acceleration, i=1, joint a are located Configuration plane be first configuration plane, then^aω_a、For pedestal target angular velocity, angular acceleration, pedestal mark is motionless, then

${\mathrm{\ω}_{a+1}}_{a+1}={R_a^{a+1}}^a{\mathrm{\ω}}_a+{\stackrel{\·}{q}}_{a+1}{e_{a+1}}_{a+1};$

${\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}={R_a^{a+1}}^a{\stackrel{\·}{\mathrm{\ω}}}_a+{P_{a+1}}_{a+1}{R_a^{a+1}}^a{\mathrm{\ω}}_a\×{\stackrel{\·}{q}}_{a+1}{e_{a+1}}_{a+1}+{\stackrel{\·\·}{q}}_{a+1}{e_{a+1}}_{a+1};$

${\stackrel{\·}{v}_{a+1}}_{a+1}=R_a^{a+1}{(}^a{\stackrel{\·}{\mathrm{\ω}}}_a+{l_a}_{a+1}{+}^a{\mathrm{\ω}}_a\×\left({}^a{\mathrm{\ω}}_a\×{l_a}_{a+1}\right)+{\stackrel{\·}{v}_a}_a);$

${\stackrel{\·}{v}_{a+1}}_{c_{a+1}}={\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}\×{\mathrm{\ρ}_{a+1}}_{a+1}+{\mathrm{\ω}_{a+1}}_{a+1}\×({\mathrm{\ω}_{a+1}}_{a+1}\×{\mathrm{\ρ}_{a+1}}_{a+1})+{\stackrel{\·}{v}_{a+1}}_{a+1};$

${F_{a+1}}_{a+1}=m_{a+1}{\stackrel{\·}{v}_{a+1}}_{c_{a+1}};$

${M_{a+1}}_{a+1}={I_{c_{a+1}}}_{a+1}{\stackrel{\·}{\mathrm{\ω}}_{a+1}}_{a+1}+{\mathrm{\ω}_{a+1}}_{a+1}\×{I_{c_{a+1}}}_{a+1}{\mathrm{\ω}_{a+1}}_{a+1};$

Last jointEnd effector handss are free in space, then M_end、F_endEqual to zero；

${F_a}_{J_a}={R_{a+1}^a}^{a+1}{F}_{J_{a+1}}+{F_a}_a;$

${M_a}_{J_a}={M_a}_a+{R_{a+1}^a}^{a+1}{M}_{J_{a+1}}+{\mathrm{\ρ}_a}_a\×{F_a}_a\×{l_a}_{a+1}\×{R_{a+1}^a}^{a+1}{F}_{J_{a+1}};$

${Q_a}_a={M_a}_{J_a}{e_a}_a;$

OrderThe supporting role that i.e. robot base is subject to gravity acceleration g upwards.