CN104999463A - Configuration-plane-based motion control method for redundant robot manipulator - Google Patents

Abstract

The invention relates to the technical field of robots and provides a configuration-plane-based motion control method for a redundant robot manipulator. The configuration-plane-based motion control method for the redundant robot manipulator can guarantee that motion control of the redundant robot manipulator in a complicated working space under such multi-objective conditions as space obstacle avoidance can be realized, and comprises the following steps: performing configuration plane partitioning on the redundant robot manipulator by use of dynamics; carrying out preliminary planning on the redundant robot manipulator according to the configuration plane matching mode; conducting space obstacle interference detection on all configuration planes of the redundant robot manipulator and re-adjusting and re-planning the configuration planes subjected to interference; smoothing joint speed and accelerated speed splines on the adjusted plan of the redundant robot manipulator; and performing dynamics check on the whole motion process. Through combination of the structural characteristics and the working mode of the redundant robot manipulator, the configuration planes are introduced into motion control of the redundant robot manipulator, so that the configuration-plane-based motion control method can visually and quickly realize motion control planning of the redundant robot manipulator.

Description

A kind of redundant mechanical arm motion control method based on configuration plane

Technical field

The present invention relates to robotics, be specifically related to a kind of redundant mechanical arm motion control method based on configuration plane that the motion control under the multiple target conditions such as barrier is kept away in redundant mechanical arm implementation space in the working space of complexity that can ensure.

Background technology

For space tasks, consider position and the attitude of end, the dimension of working space is six.Redundant mechanical arm redundancy is greater than 1.The research of redundant mechanical arm has become the study hotspot of robot field.

Many experts propose a lot of method in the motion control of redundant mechanical arm.Free-space Method based on C space sets up C space with the joint coordinate system of mechanical arm, barrier is mapped to C space, forms steric configuration obstacle, thus tries to achieve the supplementary set in C space, i.e. free space.On this basis, utilize heuristic search algorithm in the free space of mechanical arm, find the motion path of mechanical arm.Although the method can realize mechanical arm collision-free Trajectory Planning of Welding, comparatively complicated owing to barrier to be mapped to C space-wise, the requirement meeting real-time is difficult to for complex environment.In order to barrier is kept away in implementation space, repel potential field to obstacle definition one, impact point place defines one and attracts potential field, and the motion of mechanical arm is decided by two potential field acting in conjunction power, ensures that mechanical arm arrives final goal point smoothly while keeping away barrier thus.The method is very effective for the dynamic obstacle avoidance in process global path planning, but is easily absorbed in local minimum points place.

When carrying out motion control to robot, the major constraints problem that the constraint of the angular acceleration in each joint, angular speed constraint and angle restriction relate to.In general low-speed motion situation, as long as ensure that joint angles does not transfinite, this is very little on Motion trajectory impact.But when robot movement velocity is very fast, joint angle acceleration and angular speed very easily exceed restriction range, cause drive current excessive or exceed spacing accident and occur.Light then robot motion makes mistakes, heavy then damage hardware.Now, must consider various constraints when robot motion's trajectory planning, the most frequently used method is optimized run duration.Such as, there is researcher when the object by mechanical arm interception rapid flight, joint velocity constraint and moment constraint are applied to the movement locus of mechanical arm.Also have scholar to be deduced and there is the parsing of the 7R mechanical arm of special joint configuration against separating, and make all joints meet the restriction of its joint angles.Solving of anthropomorphous machine's arm working space is mainly served in work.The apery of numerous scholar to 7R mechanical arm reaches a motion planning and expands research.Also working space can be divided into little lattice one by one, obtain joint angles anthropomorphic in each little lattice, set up look-up table, terminal position as requested, find redundancy angle q3 in a lookup table, obtain other joint angles according to the inverse solution of parsing.MINH utilizes the mode of motion unit to realize anthropomorphic motion.But the emphasis of these research work reaches a motion at apery, and consideration does not keep away barrier.In order to improve its search efficiency,

Summary of the invention

The object of the invention is to provide a kind of redundant mechanical arm inverse kinematics control method.The method does not rely on robot configuration, and form is simple, and reduce amount of calculation, can be optimized solution on demand, has versatility and rapidity.

The object of the present invention is achieved like this:

Use configuration plane to carry out a method for redundant mechanical arm motion control, comprise the steps:

Step 1: given redundant mechanical arm configuration parameter, uses dynamics to carry out configuration plane division to redundant mechanical arm;

Step 2: carry out redundant mechanical arm according to the mode of configuration plane coupling and just plan, configuration plane coupling is carried out with the means of space vector representation of weighting;

Step 3: carry out spatial obstacle thing to each configuration plane of the redundant mechanical arm of planning and interfere detection, readjusts producing the configuration plane of interfering and plans;

Step 4: the speed in joint, the SPL smoothing processing of acceleration are carried out to the redundant mechanical arm planning after adjustment;

Step 5: dynamics check is carried out to whole motion process.

First joint in the configuration plane division of the dynamic (dynamical) redundant mechanical arm of described utilization in configuration plane is revolute joint, last joint is swinging joint or linear joint, and the quantity of swinging joint and linear joint is the number of degrees of freedom in configuration plane.

The crosscut of three dimensions barrier is become sectional drawing figure by described configuration plane place two dimensional surface, and extend out distance k at barrier region 1, 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};$

Relatively whether the slope of two line segments is equal, do not wait the intersection point then solving two line segment place straight lines,

Interference joint in this configuration plane is readjusted, and interference joint and safety obstacle region are judged between the boundary sections of robot base side.

Under the condition of the constraint of known joint of mechanical arm, mechanical arm tail end track, spatial obstacle object location, described configuration plane is by the space tracking of composition redundant mechanical arm planning mechanical arm:

Space joint constraint is:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i\min }\≤{\mathrm{\θ}}_i\≤{\mathrm{\θ}}_{i\max }(i=1,...n)\\ \left|{\stackrel{\·}{\mathrm{\θ}}}_i\right|\≤{\mathrm{\ν}}_{\max }(i=1,...n)\\ \left|{\stackrel{\·\·}{\mathrm{\θ}}}_i\right|\≤{\mathrm{\α}}_{\max }(i=1,...n)\\ \left|{\stackrel{\·}{\mathrm{\τ}}}_i\right|\≤{\mathrm{\τ}}_{\max }(i=1,...n)\end{array}\right..$

The locus of described configuration plane is determined to adopt weighted space vector method, P _ifor the i-th point in mechanical arm tail end trajectory planning, connect P _io ₀, k _j(j=1 ... n) for the starting point of configuration plane i is to the line of distal point;

With P _io ₀vector is the k of guiding, planning configuration plane _j(j=1 ... n); Under the condition ensureing mechanical arm working space, planning k _j(j=1 ... n-1); Configuration plane and barrier are overlapped the part of interfering, uses the method introduced to carry out checking interference inspection to joint of mechanical arm in this configuration plane and barrier.

The free degree of described configuration plane is n, and joint a is first joint of a composition kth configuration plane, and joint b is revolute joint, and joint b is divided in configuration plane k+1

^aω _afor the angular speed of joint a, the free degree of configuration plane is n, the speed of the starting point of known topography 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}{\mathrm{\ω}_a}_a+\underset{j=a+1}{\overset{a+n}{\Σ}}{\stackrel{\·}{q}}_j{e_j}_j,$

${}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_{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}^j{e}_j;$

At the barycenter k of configuration plane k _cgo out to set up coordinate, this coordinate is parallel to the coordinate at this configuration plane starting point place, represent barycenter k _ccoordinate under coordinate system a+n, then

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

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

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

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

${}^{a+n}{F}_{k_c}=\underset{j=a}{\overset{a+n}{\Σ}}{m_j}^{a+n}+{\stackrel{\·}{\mathrm{\ν}}}_{k_c}$

Configuration plane barycenter k _ccoordinate to the barycenter in each joint is to a+n, then the coordinate of the barycenter in each joint is to configuration plane barycenter k _ccoordinate be

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

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

Force and moment is

Known force ^a+nf _a+n, moment ask ^af _a,

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

Joint a is last joint of configuration plane, then joint a+1 is first joint of next configuration plane adjacent with it, order ^aω _a, for the angular speed in last joint of a upper configuration plane, angular acceleration, i=1, the configuration plane at a place, joint is first configuration plane, then ^aω _a, for pedestal target angular speed, angular acceleration, pedestal mark is motionless, then

${}^a{\mathrm{\ω}}_a{=}^a{\stackrel{\·}{\mathrm{\ω}}}_a=0;$

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

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

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

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

${}^{a+1}{F}_{a+1}={m_{a+1}}^{a+1}{\stackrel{\·}{\mathrm{\ν}}}_{c_{a+1}};$

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

Last joint end effector hand is freely in space, then M _end, F _endequal zero.；

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

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

${}^a{Q}_a{=}^q{M_{J_a}}^a{e}_a;$

Order the i.e. supporting role gravity acceleration g quite upwards that is subject to of robot base.

Compared with prior art, beneficial effect of the present invention is in the present invention:

(1) in conjunction with design feature and the working method of redundant mechanical arm, be incorporated into by configuration plane in the motion control of redundant mechanical arm, the method intuitively can realize the motion control planning of redundant mechanical arm fast.

(2) based on configuration plane, the mode guided by space vector is carried out locus to configuration plane and is determined, and then determines comparatively reasonably redundant mechanical arm space bit shape.This method avoid and adopt conventional analytic method to rely on robot configurations and the free degree, also avoid solving precision and the solving speed problem of numerical method, there is practicality and versatility.

(3) based on the dynamic method of configuration plane.The method, in units of configuration plane, reduces and calculates and solution procedure, can check the joint motions performance of mechanical arm in planning process fast, for optimization and the planning of reconditioner mechanical arm provide foundation.

Accompanying drawing explanation

Fig. 1 configuration plane divides schematic diagram accompanying drawing;

Barrier in Fig. 2 configuration plane interferes detection figure;

Figure is determined in Fig. 3 space configuration plan-position;

Fig. 4 motion control flow chart.

Detailed description of the invention

Below in conjunction with accompanying drawing, the present invention is described in more detail.

The invention discloses a kind of method using configuration plane to carry out the motion control of redundant mechanical arm.By joint of mechanical arm constraint and the restriction of spatial obstacle, the motion control of redundant mechanical arm is a complex process.The present invention starts with from composition redundant mechanical arm configuration plane, utilize the method for space geometry, spatial movement control carried out to redundant mechanical arm, guided by space vector, keep away comparison, the dynamics check in barrier path, find the path of space optimization fast, realize multi-target track planing method.The method does not rely on robot configuration, and form is simple, reduces the difficulty solved, and reduce amount of calculation, can be optimized solution on demand, has versatility and rapidity.

The technical solution used in the present invention is:

Step 1: given redundant mechanical arm configuration parameter, uses dynamics to carry out configuration plane division to redundant mechanical arm.

Step 2: carry out redundant mechanical arm according to the mode of configuration plane coupling and just plan, configuration plane coupling is carried out with the means of space vector representation of weighting.

Step 3: carry out spatial obstacle thing to each configuration plane of the redundant mechanical arm of planning and interfere detection, readjusts producing the configuration plane of interfering and plans.

Step 4: the speed in joint, the SPL smoothing processing of acceleration are carried out to the redundant mechanical arm planning after adjustment, ensures the easy motion of mechanical arm in motion process.

Step 5: dynamics check is carried out to whole motion process, the mistake load avoiding redundant mechanical arm to occur at motion process, the phenomenon of hypervelocity.

A kind of method using configuration plane to carry out the motion control of redundant mechanical arm, spatial movement control is carried out to redundant mechanical arm, guided, keep away comparison, the dynamics check in barrier path by space vector, find the path of space optimization fast, realize multi-target track planing method.

Its overall step is as follows:

Step 1: given redundant mechanical arm configuration parameter, uses dynamics to carry out configuration plane division to redundant mechanical arm.

Step 2: carry out redundant mechanical arm according to the mode of configuration plane coupling and just plan, configuration plane coupling is carried out with the means of space vector representation of weighting.

Step 3: carry out spatial obstacle thing to each configuration plane of the redundant mechanical arm of planning and interfere detection, readjusts producing the configuration plane of interfering and plans.

Step 4: the speed in joint, the SPL smoothing processing of acceleration are carried out to the redundant mechanical arm planning after adjustment, ensures the easy motion of mechanical arm in motion process.

Step 5: dynamics check is carried out to whole motion process, the mistake load avoiding redundant mechanical arm to occur at motion process, the phenomenon of hypervelocity.

The configuration plane of dynamic (dynamical) redundant mechanical arm is used to divide.First joint in configuration plane is revolute joint, and last joint is swinging joint or linear joint.First joint of first configuration plane can not be revolute joint.If last joint of serial manipulator is revolute joint, then a configuration plane is regarded separately as in this joint.In plane configuration revolute joint on the distance relation between configuration end and configuration center without impact, identical with connecting rod effect, only have swinging joint and linear joint can change configuration end and configuration center relation, therefore, the quantity of swinging joint and linear joint is the number of degrees of freedom in configuration plane.

Three dimensions barrier is configured the two dimensional surface crosscut of plane place and becomes sectional drawing figure, this sectional view possibility very irregular, the comparatively simple straightway be connected successively is adopted to carry out coated, as shown in Figure 4, consider that robot links has appearance and size and safe distance, therefore extend out distance k at barrier region 1, the actual area of such barrier just becomes the state in region 2.

If safe obstacle-avoidance area is made up of m line segment, then i-th section of line segment can be expressed from the next:

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

Robot configuration is made up of n section line segment, then jth section line segment is expressed from the next:

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

In plane geometry, article two, straight line is parallel or crossing, therefore, in order to improve the efficiency of deterministic process, first the slope of two line segments shown in formula (1) and formula (2) is compared, whether equal, do not wait the intersection point then solving two line segment place straight lines, whether this intersection point has just easily judged between these two line segments.

According to the process of redundant mechanical arm trajectory planning, the center of configuration plane and end can not in barrier zones, otherwise redundant mechanical arm just can not have been finished the work.Therefore there is obstacle and interfere situation, to be joint of robot part with the boundary sections in composition safety obstacle region exist interferes.In order to improve overall robot planning computational efficiency, also all need not judge between all joint of robot and all line segments in composition safety obstacle region, only need readjust in the interference joint in this configuration plane, interference joint and safety obstacle region are judged between the boundary sections of robot base side.

Trajectory planning process based on configuration plane makes every effort to succinct fast, avoids complicated calculations process.Under the condition of the constraint of known joint of mechanical arm, mechanical arm tail end track, spatial obstacle object location, by the space tracking of the configuration plane planning mechanical arm of composition redundant mechanical arm.

Space joint constraint is shown below:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i\min }\≤{\mathrm{\θ}}_i\≤{\mathrm{\θ}}_{i\max }(i=1,...n)\\ \left|{\stackrel{\·}{\mathrm{\θ}}}_i\right|\≤{\mathrm{\ν}}_{\max }(i=1,...n)\\ \left|{\stackrel{\·\·}{\mathrm{\θ}}}_i\right|\≤{\mathrm{\α}}_{\max }(i=1,...n)\\ \left|{\stackrel{\·}{\mathrm{\τ}}}_i\right|\≤{\mathrm{\τ}}_{\max }(i=1,...n)\end{array}\right.---\left(3\right)$

The locus of configuration plane is determined to adopt weighted space vector method, as shown in Figure 3, and P in figure _ifor the i-th point in mechanical arm tail end trajectory planning, connect P _io ₀, k _j(j=1 ... n) for the starting point of configuration plane i is to the line of distal point.

With P _io ₀vector is the k of guiding, planning configuration plane _j(j=1 ... n).K ₁value relevant with initial joint, its space vector direction is known usually, and k _nrelevant with mechanical arm tail end joint, the direction of its space vector and size are all fixing.Under the condition ensureing mechanical arm working space, planning k _j(j=1 ... n-1).Due to k _j(j=1 ... n-1) not actual machine shoulder joint central axis, therefore k _j(j=1 ... n-1) may overlap with spatial obstacle thing and interfere.Configuration plane and barrier are overlapped the part of interfering, uses the method introduced to carry out checking interference inspection to joint of mechanical arm in this configuration plane and barrier.Can not be complicated like this in actual operation, because configuration plane has simplified in mechanical arm the joint with redundancy feature, composition mechanical arm configuration plane negligible amounts, usually at about 3-5, after a small amount of configuration plane of planning, the mode of remaining 2 configuration planes just by resolving determines its locus.

Configuration plane internal dynamics is analyzed

As shown in the schematic diagram of Fig. 2 kth configuration plane, the free degree of this configuration plane is n, joint a is first joint of a composition kth configuration plane, because joint b is revolute joint, at consideration angular speed, during angular acceleration Superposition Formula, conveniently calculate, joint b is divided in configuration plane k+1.

1) speed formula

^aω _afor the angular speed of joint a, if the free degree of configuration plane is n, the speed of the starting point of known topography plane ^aω _a, negotiation speed formula tries to achieve the speed in last joint of configuration plane ^a+nω _a+n, ^a+nv _a+n,

${}^{a+n}{\mathrm{\ω}}_{a+n}={R_a^{a+n}}^a{\mathrm{\ω}}_a+\underset{j=a+1}{\overset{a+n}{\Σ}}{{\stackrel{\·}{q}}_j}^j{e}_j---\left(5\right)$

${}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_{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}^j{e}_j---\left(6\right)$

As shown in Figure 2, at the barycenter k of configuration plane k _cgo out to set up coordinate, if this coordinate is parallel to the coordinate at this configuration plane starting point place, represent barycenter k _ccoordinate under coordinate system a+n, then

${}^{a+n}{\mathrm{\ρ}}_{k_c}\underset{i=a}{\overset{a+n}{\Σ}}{m}_i=\underset{i=a}{\overset{a+n}{\Σ}}R_i^{a+n}{m_i}^i{\mathrm{\ρ}}_i---\left(7\right)$

${}^{a+n}{\mathrm{\ω}}_{k_c}{=}^{a+n}{\mathrm{\ω}}_{a+n}---\left(8\right)$

${}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_{k_c}{=}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_{a+n}---\left(9\right)$

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

${}^{a+n}{F}_{k_c}=\underset{j=a}{\overset{a+n}{\Σ}}{m_j}^{a+n}{\stackrel{\·}{\mathrm{\ν}}}_{k_c}---\left(11\right)$

If configuration plane barycenter k _ccoordinate to the barycenter in each joint is (j=a to a+n), then the coordinate of the barycenter in each joint is to configuration plane barycenter k _ccoordinate be

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

${}^{a+n}{M}_k{=}^{k_c}{I_{a+n}}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_k{+}^{a+n}{\mathrm{\ω}}_k{\×}^{k_c}{I_{a+n}}^{a+n}{\mathrm{\ω}}_k---\left(13\right)$

2) force and moment formula

Known force ^a+nf _a+n, moment ask ^af _a,

${}^a{M}_{J_a}=R_{a+n}^a{(}^{a+n}{M}_{J_{a+n}}{+}^{a+n}{M}_{k_{a+n}}{\×}^{a+n}{F}_{k_{a+n}}{\×}^{a+n}{\mathrm{\ρ}}_{k_c})---\left(14\right)$

(2) dynamic analysis between configuration plane

If joint a is last joint of configuration plane, then joint a+1 is first joint of next configuration plane adjacent with it

1) speed formula

In formula (7), formula (8), order ^aω _a, for the angular speed in last joint of a upper configuration plane, angular acceleration, i=1, then the angular speed in first joint of adjacent with it next configuration plane just can be obtained.If the configuration plane at a place, joint is first configuration plane, then ^aω _a, for pedestal target angular speed, angular acceleration, if pedestal mark is motionless, then

${}^a{\mathrm{\ω}}_a{=}^a{\stackrel{\·}{\mathrm{\ω}}}_a=0.$

${}^{a+1}{\mathrm{\ω}}_{a+1}={R_a^{a+1}}^a{\mathrm{\ω}}_a\×{{\stackrel{\·}{q}}_{a+1}}^{a+1}{e}_{a+1}---\left(15\right)$

${}^{a+1}{\stackrel{\·}{\mathrm{\ω}}}_{a+1}={R_a^{a+1}}^a{\stackrel{\·}{\mathrm{\ω}}}_a{+}^{a+1}{P}_{a+1}{R_a^{a+1}}^a{\mathrm{\ω}}_a\×{{\stackrel{\·}{q}}_{a+1}}^{a+1}{e}_{a+1}+{{\stackrel{\·\·}{q}}_{a+1}}^{a+1}{e}_{a+1}---\left(16\right)$

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

${}^{a+1}{\stackrel{\·}{v}}_{c_{a+1}}{=}^{a+1}{\stackrel{\·}{\mathrm{\ω}}}_{a+1}{\×}^{a+1}{\mathrm{\ρ}}_{a+1}{+}^{a+1}{\mathrm{\ω}}_{a+1}\×\left({}^{a+1}{\mathrm{\ω}}_{a+1}{\×}^{a+1}{\mathrm{\ρ}}_{a+1}\right){+}^{a+1}{\stackrel{\·}{v}}_{a+1}---\left(18\right)$

${}^{a+1}{F}_{a+1}={m_{a+1}}^{a+1}{\stackrel{\·}{\mathrm{\ν}}}_{c_{a+1}}---\left(19\right)$

${}^{a+1}{M}_{a+1}{=}^{c_{a+1}}{I_{a+1}}^{a+1}{\stackrel{\·}{\mathrm{\ω}}}_{a+1}{+}^{a+1}{\mathrm{\ω}}_{a+1}{\×}^{c_{a+1}}{I_{a+1}}^{a+1}{\mathrm{\ω}}_{a+1}---\left(20\right)$

2) force and moment formula

(mechanical arm all passes joint number is r) in last joint if end effector hand is freely in space, then M _end, F _endequal zero.

${}^a{F}_{J_a}={R_{a+1}^a}^{a+1}{F}_{J_{a+1}}{+}^a{F}_a---\left(21\right)$

${}^a{M}_{J_a}{=}^a{M}_a+{R_{a+1}^a}^{a+1}{M}_{J_{a+1}}{+}^a{\mathrm{\ρ}}_a{\×}^a{F}_a{+}^a{l}_{a+1}\×{R_{a+1}^a}^{a+1}{F}_{J_{a+1}}---\left(22\right)$

${}^a{Q}_a{=}^a{M_{J_a}}^a{e}_a---\left(23\right)$

When considering the affecting of connecting rod own weight, order the i.e. supporting role gravity acceleration g quite upwards that is subject to of robot base.The impact of such process and each module gravity is just the same.

The present invention sums up the redundant mechanical arm configuration feature of cascade based on configuration plane, for the series redundancy mechanical arm operating configuration of concrete form.The method form is succinct, has very high precision and solving speed, has very strong practicality and versatility.

Implement 1, by reference to the accompanying drawings 1, use the configuration plane of dynamic (dynamical) redundant mechanical arm to divide.First joint in configuration plane is revolute joint, and last joint is swinging joint or linear joint.First joint of first configuration plane can not be revolute joint.If last joint of serial manipulator is revolute joint, then a configuration plane is regarded separately as in this joint.In plane configuration revolute joint on the distance relation between configuration end and configuration center without impact, identical with connecting rod effect, only have swinging joint and linear joint can change configuration end and configuration center relation, therefore, the quantity of swinging joint and linear joint is the number of degrees of freedom in configuration plane.

Implement 2, by reference to the accompanying drawings 2, three dimensions barrier is configured the two dimensional surface crosscut of plane place and becomes sectional drawing figure, this sectional view possibility very irregular, the comparatively simple straightway be connected successively is adopted to carry out coated, consider that robot links has appearance and size and safe distance, therefore extend out distance k at barrier region 1, the actual area of such barrier just becomes the state in region 2.

If safe obstacle-avoidance area is made up of m line segment, then i-th section of line segment can be expressed from the next:

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

Robot configuration is made up of n section line segment, then jth section line segment is expressed from the next:

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

In plane geometry, article two, straight line is parallel or crossing, therefore, in order to improve the efficiency of deterministic process, first the slope of two line segments shown in formula (1) and formula (2) is compared, whether equal, do not wait the intersection point then solving two line segment place straight lines, whether this intersection point has just easily judged between these two line segments.

According to the process of redundant mechanical arm trajectory planning, the center of configuration plane and end can not in barrier zones, otherwise redundant mechanical arm just can not have been finished the work.Therefore there is obstacle and interfere situation, to be joint of robot part with the boundary sections in composition safety obstacle region exist interferes.In order to improve overall robot planning computational efficiency, also all need not judge between all joint of robot and all line segments in composition safety obstacle region, only need readjust in the interference joint in this configuration plane, interference joint and safety obstacle region are judged between the boundary sections of robot base side.

Implement 3, by reference to the accompanying drawings 3, the trajectory planning process based on configuration plane makes every effort to succinct fast, avoids complicated calculations process.Under the condition of the constraint of known joint of mechanical arm, mechanical arm tail end track, spatial obstacle object location, by the space tracking of the configuration plane planning mechanical arm of composition redundant mechanical arm.

Space joint constraint is shown below:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i\min }\≤{\mathrm{\θ}}_i\≤{\mathrm{\θ}}_{i\max }(i=1,...n)\\ \left|{\stackrel{\·}{\mathrm{\θ}}}_i\right|\≤{\mathrm{\ν}}_{\max }(i=1,...n)\\ \left|{\stackrel{\·\·}{\mathrm{\θ}}}_i\right|\≤{\mathrm{\α}}_{\max }(i=1,...n)\\ \left|{\stackrel{\·}{\mathrm{\τ}}}_i\right|\≤{\mathrm{\τ}}_{\max }(i=1,...n)\end{array}\right.---\left(3\right)$

Implement 3, the locus of configuration plane is determined to adopt weighted space vector method, as shown in Figure 3, and P in figure _ifor the i-th point in mechanical arm tail end trajectory planning, connect P _io ₀, k _j(j=1 ... n) for the starting point of configuration plane i is to the line of distal point.

With P _io ₀vector is the k of guiding, planning configuration plane _j(j=1 ... n).K ₁value relevant with initial joint, its space vector direction is known usually, and k _nrelevant with mechanical arm tail end joint, the direction of its space vector and size are all fixing.Under the condition ensureing mechanical arm working space, planning k _j(j=1 ... n-1).Due to k _j(j=1 ... n-1) not actual machine shoulder joint central axis, therefore k _j(j=1 ... n-1) may overlap with spatial obstacle thing and interfere.Configuration plane and barrier are overlapped the part of interfering, uses the method introduced to carry out checking interference inspection to joint of mechanical arm in this configuration plane and barrier.Can not be complicated like this in actual operation, because configuration plane has simplified in mechanical arm the joint with redundancy feature, composition mechanical arm configuration plane negligible amounts, usually at about 3-5, after a small amount of configuration plane of planning, the mode of remaining 2 configuration planes just by resolving determines its locus.

Implement 4, dynamics check method,

Shown in the schematic diagram of a kth configuration plane, the free degree of this configuration plane is n, and joint a is first joint of a composition kth configuration plane, because joint b is revolute joint, at consideration angular speed, during angular acceleration Superposition Formula, conveniently calculate, joint b is divided in configuration plane k+1.

1) speed formula

^aω _afor the angular speed of joint a, if the free degree of configuration plane is n, the speed of the starting point of known topography plane ^aω _a, negotiation speed formula tries to achieve the speed in last joint of configuration plane ^a+nω _a+n, ^a+nv _a+n,

${}^{a+n}{\mathrm{\ω}}_{a+n}={R_a^{a+n}}^a{\mathrm{\ω}}_a+\underset{j=a+1}{\overset{a+n}{\Σ}}{{\stackrel{\·}{q}}_j}^j{e}_j---\left(5\right)$

${}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_{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}^j{e}_j---\left(6\right)$

At the barycenter k of configuration plane k _cgo out to set up coordinate, if this coordinate is parallel to the coordinate at this configuration plane starting point place, represent barycenter k _ccoordinate under coordinate system a+n, then

${}^{a+n}{\mathrm{\ρ}}_{k_c}\underset{i=a}{\overset{a+n}{\Σ}}{m}_i=\underset{i=a}{\overset{a+n}{\Σ}}R_i^{a+n}{m_i}^i{\mathrm{\ρ}}_i---\left(7\right)$

${}^{a+n}{\mathrm{\ω}}_{k_c}{=}^{a+n}{\mathrm{\ω}}_{a+n}---\left(8\right)$

${}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_{k_c}{=}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_{a+n}---\left(9\right)$

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

${}^{a+n}{F}_{k_c}=\underset{j=a}{\overset{a+n}{\Σ}}{m_j}^{a+n}+{\stackrel{\·}{\mathrm{\ν}}}_{k_c}---\left(11\right)$

If configuration plane barycenter k _ccoordinate to the barycenter in each joint is to a+n), then the coordinate of the barycenter in each joint is to configuration plane barycenter k _ccoordinate be

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

${}^{a+n}{M}_k{=}^{k_c}{I_{a+n}}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_k{+}^{a+n}{\mathrm{\ω}}_k{\×}^{k_c}{I_{a+n}}^{a+n}{\mathrm{\ω}}_k---\left(13\right)$

2) force and moment formula

Known force ^a+nf _a+n, moment ask ^af _a,

${}^a{M}_{J_a}=R_{a+n}^a{(}^{a+n}{M}_{J_{a+n}}{+}^{a+n}{M}_{k_{a+n}}{\×}^{a+n}{F}_{k_{a+n}}{\×}^{a+n}{\mathrm{\ρ}}_{k_c})---\left(14\right)$

If joint a is last joint of configuration plane, then joint a+1 is first joint of next configuration plane adjacent with it

1) speed formula

In formula (7), formula (8), order ^aω _a, for the angular speed in last joint of a upper configuration plane, angular acceleration, i=1, then the angular speed in first joint of adjacent with it next configuration plane just can be obtained.If the configuration plane at a place, joint is first configuration plane, then ^aω _a, for pedestal target angular speed, angular acceleration, if pedestal mark is motionless, then

${}^a{\mathrm{\ω}}_a{=}^a{\stackrel{\·}{\mathrm{\ω}}}_a=0.$

${}^{a+1}{\mathrm{\ω}}_{a+1}={R_a^{a+1}}^a{\mathrm{\ω}}_a\×{{\stackrel{\·}{q}}_{a+1}}^{a+1}{e}_{a+1}---\left(15\right)$

${}^{a+1}{\stackrel{\·}{\mathrm{\ω}}}_{a+1}={R_a^{a+1}}^a{\stackrel{\·}{\mathrm{\ω}}}_a{+}^{a+1}{P}_{a+1}{R_a^{a+1}}^a{\mathrm{\ω}}_a\×{{\stackrel{\·}{q}}_{a+1}}^{a+1}{e}_{a+1}+{{\stackrel{\·\·}{q}}_{a+1}}^{a+1}{e}_{a+1}---\left(16\right)$

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

${}^{a+1}{\stackrel{\·}{v}}_{c_{a+1}}{=}^{a+1}{\stackrel{\·}{\mathrm{\ω}}}_{a+1}{\×}^{a+1}{\mathrm{\ρ}}_{a+1}{+}^{a+1}{\mathrm{\ω}}_{a+1}\×\left({}^{a+1}{\mathrm{\ω}}_{a+1}{\×}^{a+1}{\mathrm{\ρ}}_{a+1}\right){+}^{a+1}{\stackrel{\·}{v}}_{a+1}---\left(18\right)$

${}^{a+1}{F}_{a+1}={m_{a+1}}^{a+1}{\stackrel{\·}{\mathrm{\ν}}}_{c_{a+1}}---\left(19\right)$

${}^{a+1}{M}_{a+1}{=}^{c_{a+1}}{I_{a+1}}^{a+1}{\stackrel{\·}{\mathrm{\ω}}}_{a+1}{+}^{a+1}{\mathrm{\ω}}_{a+1}{\×}^{c_{a+1}}{I_{a+1}}^{a+1}{\mathrm{\ω}}_{a+1}---\left(20\right)$

2) force and moment formula

(mechanical arm all passes joint number is r) in last joint if end effector hand is freely in space, then M _end, F _endequal zero.

${}^a{F}_{J_a}={R_{a+1}^a}^{a+1}{F}_{J_{a+1}}{+}^a{F}_a---\left(21\right)$

${}^a{M}_{J_a}{=}^a{M}_a+{R_{a+1}^a}^{a+1}{M}_{J_{a+1}}{+}^a{\mathrm{\ρ}}_a{\×}^a{l}_{a+1}\×{R_{a+1}^a}^{a+1}{F}_{J_{a+1}}---\left(22\right)$

${}^a{Q}_a{=}^a{M_{J_a}}^a{e}_a---\left(23\right)$

When considering the affecting of connecting rod own weight, order the i.e. supporting role gravity acceleration g quite upwards that is subject to of robot base.The impact of such process and each module gravity is just the same.

Implement 5, by reference to the accompanying drawings 4, its overall step is as follows:

Step 1: given redundant mechanical arm configuration parameter, uses dynamics to carry out configuration plane division to redundant mechanical arm.

Step 2: carry out redundant mechanical arm according to the mode of configuration plane coupling and just plan, configuration plane coupling is carried out with the means of space vector representation of weighting.

Step 3: carry out spatial obstacle thing to each configuration plane of the redundant mechanical arm of planning and interfere detection, readjusts producing the configuration plane of interfering and plans.

Step 4: the speed in joint, the SPL smoothing processing of acceleration are carried out to the redundant mechanical arm planning after adjustment, ensures the easy motion of mechanical arm in motion process.

Step 5: dynamics check is carried out to whole motion process, the mistake load avoiding redundant mechanical arm to occur at motion process, the phenomenon of hypervelocity.

Claims (6)

1. use configuration plane to carry out a method for redundant mechanical arm motion control, it is characterized in that, comprise the steps:

Step 1: given redundant mechanical arm configuration parameter, uses dynamics to carry out configuration plane division to redundant mechanical arm;

Step 2: carry out redundant mechanical arm according to the mode of configuration plane coupling and just plan, configuration plane coupling is carried out with the means of space vector representation of weighting;

Step 3: carry out spatial obstacle thing to each configuration plane of the redundant mechanical arm of planning and interfere detection, readjusts producing the configuration plane of interfering and plans;

Step 4: the speed in joint, the SPL smoothing processing of acceleration are carried out to the redundant mechanical arm planning after adjustment;

Step 5: dynamics check is carried out to whole motion process.

2. a kind of method using configuration plane to carry out the motion control of redundant mechanical arm according to claim 1, it is characterized in that: first joint in the configuration plane division of the dynamic (dynamical) redundant mechanical arm of described utilization in configuration plane is revolute joint, last joint is swinging joint or linear joint, and the quantity of swinging joint and linear joint is the number of degrees of freedom in configuration plane.

3. a kind of method using configuration plane to carry out the motion control of redundant mechanical arm according to claim 1, it is characterized in that: the crosscut of three dimensions barrier is become sectional drawing figure by described configuration plane place two dimensional surface, distance k is extended out at barrier region 1, 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};$

Relatively whether the slope of two line segments is equal, do not wait the intersection point then solving two line segment place straight lines,

Interference joint in this configuration plane is readjusted, and interference joint and safety obstacle region are judged between the boundary sections of robot base side.

4. a kind of method using configuration plane to carry out the motion control of redundant mechanical arm according to claim 1, it is characterized in that: under the condition of the constraint of known joint of mechanical arm, mechanical arm tail end track, spatial obstacle object location, described configuration plane is by the space tracking of composition redundant mechanical arm planning mechanical arm:

Space joint constraint is:

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i\min }\≤{\mathrm{\θ}}_i\≤{\mathrm{\θ}}_{i\max }(i=1,...n)\\ \left|{\stackrel{\·}{\mathrm{\θ}}}_i\right|\≤{\mathrm{\ν}}_{\max }(i=1,...n)\\ \left|{\stackrel{\·\·}{\mathrm{\θ}}}_i\right|\≤{\mathrm{\α}}_{\max }(i=1,...n)\\ \left|{\mathrm{\τ}}_i\right|\≤{\mathrm{\τ}}_{\max }(i=1,...n)\end{array}\right..$

5. a kind of method using configuration plane to carry out the motion control of redundant mechanical arm according to claim 1, is characterized in that: the locus of described configuration plane is determined to adopt weighted space vector method, P _ifor the i-th point in mechanical arm tail end trajectory planning, connect P _io ₀, k _j(j=1 ... n) for the starting point of configuration plane i is to the line of distal point;

With P _io ₀vector is the k of guiding, planning configuration plane _j(j=1 ... n); Under the condition ensureing mechanical arm working space, planning k _j(j=1 ... n-1); Configuration plane and barrier are overlapped the part of interfering, uses the method introduced to carry out checking interference inspection to joint of mechanical arm in this configuration plane and barrier.

6. a kind of method using configuration plane to carry out the motion control of redundant mechanical arm according to claim 1, it is characterized in that: the free degree of described configuration plane is n, joint a is first joint of a composition kth configuration plane, joint b is revolute joint, joint b is divided in configuration plane k+1

^aω _afor the angular speed of joint a, the free degree of configuration plane is n, the speed of the starting point of known topography plane ^aω _a, try to achieve the speed in last joint of configuration plane ^a+nω _a+n, ^a+nv _a+n,

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

${}^{a+n}{\stackrel{\·}{\mathrm{\ω}}}_{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}^j{e}_j;$

At the barycenter k of configuration plane k _cgo out to set up coordinate, this coordinate is parallel to the coordinate at this configuration plane starting point place, represent barycenter k _ccoordinate under coordinate system a+n, then

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

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

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

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

${}^{a+n}{F}_{k_c}=\underset{j=a}{\overset{a+n}{\Σ}}{m_j}^{a+n}+{\stackrel{\·}{\mathrm{\ν}}}_{k_c}$

Configuration plane barycenter k _ccoordinate to the barycenter in each joint is to a+n, then the coordinate of the barycenter in each joint is to configuration plane barycenter k _ccoordinate be

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

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

Force and moment is

Known force ^a+nf _a+n, moment ask ^af _a,

${}^a{M}_{J_a}=R_{a+n}^a\left({}^{a+n}{M}_{J_{a+n}}{+}^{a+n}{M}_{k_{a+n}}{+}^{a+n}{F}_{k_{a+n}}{\×}^{a+n}{\mathrm{\ρ}}_{k_c}\right);$

Joint a is last joint of configuration plane, then joint a+1 is first joint of next configuration plane adjacent with it, order ^aω _a, for the angular speed in last joint of a upper configuration plane, angular acceleration, i=1, the configuration plane at a place, joint is first configuration plane, then ^aω _a, for pedestal target angular speed, angular acceleration, pedestal mark is motionless, then ${}^a{\mathrm{\ω}}_a{=}^a{\stackrel{\·}{\mathrm{\ω}}}_a=0;$

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

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

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

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

${}^{a+1}{F}_{a+1}={m_{a+1}}^{a+1}{\stackrel{\·}{\mathrm{\ν}}}_{c_{a+1}};$

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

Last joint end effector hand is freely in space, then M _end, F _endequal zero;

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

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

${}^a{Q}_a{=}^a{M_{J_a}}^a{e}_a;$

Order the i.e. supporting role gravity acceleration g quite upwards that is subject to of robot base.