CN105234930B - Control method of motion of redundant mechanical arm based on configuration plane - Google Patents

Abstract

The invention provides a control method of the motion of a redundant mechanical arm based on a configuration plane. The method proceeds with the configuration plane forming the redundant mechanical arm; the control planning process in which the redundant mechanical arm performs spatial motion is determined through a spatial geometrical method; the space optimized path is fast found through spatial vector guidance, comparison of obstacle-avoiding paths and dynamics check; and a multi-target track planning method is achieved. The method solves the universality problem of motion planning of the redundant mechanical arm, does not depend on robot configuration, is simple in type and reduces solving difficulty and the calculation amount, optimized solutions can be obtained as required, the method has universality and rapidity, and the motion planning requirement of a multi-target task and the environment for the redundant mechanical arm is met.

Description

Redundant mechanical arm motion control method based on configuration plane

Technical field

The present invention relates to a kind of robot control method, specifically a kind of redundant mechanical based on configuration plane Arm motion control track approach, the method ensure that redundant mechanical arm realizes that space avoidance etc. is more in complicated working space Motion control under goal condition.

Background technology

The free degree of space motion object is 6, therefore robotic arm manipulation space is also 6, and redundant mechanical arm has than operation Joint more than spatial degrees of freedom, it is meant that in identical task, redundant mechanical arm has more operation frees degree, Neng Goushi Now more flexible motion control, and then the need for meeting actual task, therefore the research of redundant mechanical arm turns into robot motion The study hotspot of control field.

Many experts propose many methods in terms of the motion control of redundant mechanical arm.Free space based on C spaces Method sets up C spaces with the joint coordinate system of mechanical arm, and barrier is mapped into C spaces, steric configuration obstacle is formed, so as to try to achieve The supplementary set in C spaces, i.e. free space.On this basis, found in the free space of mechanical arm using heuristic search algorithm The motion path of mechanical arm.Although the method can realize mechanical arm collision-free Trajectory Planning of Welding, due to barrier being mapped It is complex to C space-wises, for the requirement that complex environment is difficult to meet real-time.In order to realize space avoidance, to obstacle A repulsion potential field is defined, an attraction potential field is defined at impact point, the motion of mechanical arm is come by two potential field collective effect power Determine, thus smoothly reach final goal point while avoidance ensureing mechanical arm.The method is advised for treatment global path Dynamic obstacle avoidance in drawing is highly effective, but is easily trapped at local minimum points.

When motion control is carried out to robot, the constraint of the angular acceleration in each joint, angular speed constraint and angle restriction It is the major constraints problem being related to.In the case of general low-speed motion, as long as ensureing that joint angles do not transfinite, this is to motion Trajectory planning influences very little.But, when robot movement velocity is very fast, joint angle acceleration and angular speed is easily beyond constraint Scope, causes driving current excessive or occurs beyond spacing accident.Light then robot motion malfunctions, heavy then damage hardware. Now it is necessary to consider various constraintss in robot motion's trajectory planning, most common method is to run duration Optimize.For example, having researcher when the object of rapid flight is intercepted with mechanical arm, the movement locus to mechanical arm applies Joint velocity is constrained and torque constraint.Also scholar is deduced the inverse solution of parsing of the 7R mechanical arms with the configuration of special joint, And make all of joint meet its joint angles to limit.Work is primarily servicing the solution of anthropomorphous machine's arm working space. Numerous scholars expand research to the apery of 7R mechanical arms up to a motion planning.Also working space can be divided into small one by one Lattice, obtain joint angles anthropomorphic in each small lattice, set up look-up table, and terminal position as requested finds in a lookup table Redundancy angle q3, other joint angles are obtained according to the inverse solution of parsing.MINH realizes anthropomorphic motion using the mode of motion unit.But these grind Study carefully being moved it is important that reaching point in apery for work, avoidance is not considered.

The content of the invention

Robot configuration is independent of present invention aim at one kind is provided, form is simple, and amount of calculation is few, can obtain on demand Solved to optimization, the redundant mechanical arm motion control method based on configuration plane with versatility and rapidity.

The object of the present invention is achieved like this：

Step 1：Input redundant mechanical arm configuration parameter, configuration plane division is carried out with dynamics to redundant mechanical arm；

Step 2：The configuration plane that will have been divided carries out planning, institute at the beginning of redundant mechanical arm according to the mode that configuration plane is matched State configuration plane matching is carried out with the means of space vector representation for weighting；

Step 3：Each configuration plane of redundant mechanical arm to planning carries out the interference detection of spatial obstacle thing, dry to producing The configuration plane for relating to is readjusted and planned；

Step 4：The smooth place of SPL of the speed in joint, acceleration is carried out to the redundant mechanical arm planning after adjustment Reason, it is ensured that easy motion of the mechanical arm in motion process；

Step 5：Dynamics check is carried out to whole motion process, it is to avoid the overload that redundant mechanical arm occurs in motion process Lotus, the phenomenon of hypervelocity.

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

The present invention compared with prior art, with substantial advantage following prominent and beneficial effect：

(1) design feature and working method of redundant mechanical arm are combined, configuration plane is incorporated into the fortune of redundant mechanical arm In dynamic control, the method intuitively can quickly realize the motion control planning of redundant mechanical arm.

(2) based on configuration plane, locus determination is carried out to configuration plane by way of space vector is guided, And then determine relatively reasonable redundant mechanical arm space bit shape.Robot structure is relied on this method avoid using conventional analytic method Shape form and the free degree, also avoid the solving precision and solving speed problem of numerical method, with practicality and versatility.

(3) dynamic method based on configuration plane.The method is reduced and is calculated and solved step in units of configuration plane Suddenly, can quickly check joint motions performance of the mechanical arm in planning process, for optimize and adjust mechanical arm planning provide according to According to.

Brief description of the drawings

Fig. 1 configuration planes divide schematic diagram accompanying drawing.

Barrier interference detection in Fig. 2 configuration planes.

Fig. 3 space configurations plan-position determines.

The configuration plane planning of Fig. 4 known trajectories point.

Fig. 5 motion control flow charts.

Fig. 6 redundant mechanical arm motion model figures.

The angular speed change curve in each joints of Fig. 7.

The moment variations curve in each joints of Fig. 8.

Sectional drawing in Fig. 9 a- Fig. 9 d redundant mechanical arm motion processes.

Specific embodiment

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

With reference to Fig. 5, the overall step of the method for the present invention is as follows：

Step 1：Given redundant mechanical arm configuration parameter, configuration plane division is carried out with dynamics to redundant mechanical arm.

Step 2：The mode matched according to configuration plane plans that configuration plane matches to weight at the beginning of carrying out redundant mechanical arm Means of space vector representation is carried out.

Step 3：Each configuration plane of redundant mechanical arm to planning carries out the interference detection of spatial obstacle thing, dry to producing The configuration plane for relating to is readjusted and planned.

Step 4：The smooth place of SPL of the speed in joint, acceleration is carried out to the redundant mechanical arm planning after adjustment Reason, it is ensured that easy motion of the mechanical arm in motion process.

Step 5：Dynamics check is carried out to whole motion process, it is to avoid the overload that redundant mechanical arm occurs in motion process Lotus, the phenomenon of hypervelocity.

With reference to Fig. 1,

It is flat that the central axis that the configuration plane of mechanical arm is defined as several joint of robot being sequentially connected is formed Face.According to the principle of space geometry, compared to line, the central axis of revolute joint is used as dividing between configuration plane for two planes Boundary line.For configuration plane can be divided into following several classes by this：

(1) configuration plane terminal position is constant, and attitude is constant：Such configuration plane institute is only link block comprising module, There is the module of amount of exercise change in the absence of swing module, revolution module, mobile module etc.；

(2) configuration plane terminal position change, attitude is constant：Comprising link block and mobile mould in such configuration plane Block, in the absence of swing module and revolution module；

(3) configuration plane terminal position is constant, attitudes vibration：Comprising revolution module and basic connection in such configuration plane Module, if there is angle link block, in robot topological structure, angle link block compared to basic link block closer to The center of configuration plane；

(4) change of configuration plane terminal position, attitudes vibration：According to the definition of configuration plane, no matter configuration plane is constituted Robot motion's module how to move, its position and attitude can be changed in two dimensional surface.Angle link block and revolution Module turns into configuration plane separation.Revolute joint is closed as specific configuration plane boundary joint, then swinging joint and movement The quantity of section is the number of degrees of freedom in configuration plane.

With reference to Fig. 2, two dimensional surface where spatial obstacle thing is configured plane is crosscutting into sectional drawing figure, and this sectional view can Energy very irregular, is coated using the relatively simple straightway being sequentially connected, and makes coated figure approximate regulation figure, institute Cladding area marking is barrier region 1, with reference to accompanying drawing 4, it is contemplated that robot links have appearance and size and safe distance, because This is extended out apart from k in barrier region 1, and the actual area of so gained barrier is labeled as region 2.

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

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

Wherein：x_i, z_iFor i-th joint under space coordinates x-axis, the coordinate value on z-axis direction；

When there is obstacle interference situation, it is meant that joint of robot part and the boundary sections for constituting safety obstacle region In the presence of interference.In order to improve overall robot planning computational efficiency, the calculating time is saved, with barrier only in calculating configuration plane There is the joint of mechanical arm of interference in region.For the joint of mechanical arm for interference situation occur, according to the position shape of configuration plane Matching process re-starts allotment, meets planning requirement.

In plane geometry, two straight lines are either parallel or intersecting, therefore, in order to improve the efficiency of deterministic process, first Compare formula (1) and two slopes of line segment shown in formula (2), if equal.If equal, it is meant that two antenna segments are not handed over Point, then will not interfere, and meet planning requirement.The intersection point of straight line where two lines section is solved if not waiting, if intersection point does not exist On one line segment of any of which, then mechanical arm is not interfered with barrier, meets planning requirement, is otherwise interfered.

According to the process of redundant mechanical arm trajectory planning, the center and end of configuration plane can not be in barrier zones , otherwise redundant mechanical arm cannot complete work.Therefore there is obstacle interference situation, be joint of robot part and composition There is interference in the boundary sections in safety obstacle region.In order to improve overall robot planning computational efficiency, only need to be flat in the configuration Interference joint in face is readjusted, in the safety obstacle region in interference joint and near robot base side Judged between boundary sections.

With reference to Fig. 3, the locus of configuration plane determines to use weighted space vector method, P_iFor mechanical arm tail end track is advised I-th point in drawing, O₀It is mechanical arm polar coordinate system origin, connects P_iO₀, k_j(j=1 ... it is n) starting point of configuration plane i to last The line of end points.

With P_iO₀Vector plans the k of configuration plane to be oriented to_j(j=1 ... n).k₁Value it is relevant with initial joint, generally Its space vector direction is, it is known that and k_nRelevant with mechanical arm tail end joint, the direction of its space vector and size are all fixed 's.Under conditions of mechanical arm working space is ensured, k is planned_j(j=1 ... n-1).Due to k_j(j=1 ... n-1) it is not actual Joint of mechanical arm central axis, therefore k_j(j=1 ... n-1) may overlap interference with spatial obstacle thing.To configuration plane Overlap the part of interference with barrier, and joint of mechanical arm in the configuration plane and barrier are carried out with the method introduced Checking interference is checked.

With P_iO₀Vector plans the k of configuration plane to be oriented to_j(j=1 ... n).k₁Value it is relevant with initial joint, generally Its space vector direction is, it is known that and k_nRelevant with mechanical arm tail end joint, the direction of its space vector and size are all fixed 's.Under conditions of mechanical arm working space is ensured, k is planned_j(j=1 ... n-1).Due to k_j(j=1 ... n-1) it is not actual Joint of mechanical arm central axis, therefore k_j(j=1 ... n-1) may overlap interference with spatial obstacle thing.To configuration plane Overlap the part of interference with barrier, and joint of mechanical arm in the configuration plane and barrier are carried out with the method introduced Checking interference is checked.

And it is fairly simple in actual operation, because configuration plane has had the pass of redundancy feature in simplified mechanical arm Section, constitutes mechanical arm configuration plane negligible amounts, generally in 3-5 or so, after a small amount of configuration plane is planned, and remaining 2 structures Shape plane can determine its locus by way of parsing.

In the case where spatial obstacle thing takes seldom to mechanical arm working space, the configuration plane locus of mechanical arm There are more selections.With reference to accompanying drawing 4, left hand view and right part of flg are the two states under identical tracking condition, due to fortune The requirement of dynamic rail mark, what the configuration plane CPn where the last joint of mechanical arm was to determine, therefore configuration plane CPn under two states It is identical.

In this case, overall consideration is carried out to the planning of the space tracking of mechanical arm.One work of tracing point of the above Reference is configured as, evaluation function is set up.

U in formula_h(h=1 ... m) is weighted value, and the power and bit in the weighted value and h joints is equipped with passs, power it is bigger with Near the joint that pedestal is nearer, the weighted value is bigger, under the conditions of multiple choices, high-power pass is may be such that by the weighted value Section motion amplitude, improves the dynamics function of mechanical arm；F is used for evaluating current trace points with the operating configuration of a upper tracing point Difference, make every effort to minimum under the conditions of multiple choices.

To ensure mechanical end stable movement, trajectory planning is carried out using B-spline curves.Use the side of B-spline curves Method so that the position of the starting point and ending point of the speed of robot, acceleration, acceleration and robot can stablize and Meet the requirements.

Dynamics check method,

Dynamics check method based on configuration plane is divided into two parts：Configuration plane internal dynamics analysis method and structure The dynamic analysis method of shape interplanar.The external force for being applied torque by driver or being applied on motion arm transports motion arm Dynamic, configuration plane internal dynamics analysis method mainly studies first joint and last joint power, power in this configuration plane Relation between square；Configuration interplanar dynamic analysis method mainly studies in this configuration plane last joint and next Relation between configuration plane first joint power, torque.

(1) configuration plane internal dynamics analysis method

With reference to accompanying drawing 1, the free degree of k-th configuration plane is n, and joint a is k-th the first of configuration plane pass of composition Section, because joint b is revolute joint, is considering angular speed, during angular acceleration Superposition Formula, calculates for convenience, and joint b is drawn In assigning to configuration plane k+1.

1) speed formula

^aω_aIt is the angular speed of joint a, if the free degree of configuration plane is n, it is known that the speed of the starting point of configuration plane^a ω_a, The speed in last joint of configuration plane is tried to achieve by speed formula^a+nω_a+n, ^{a+ n}v_a+n,

With reference to Fig. 1, in the barycenter k of configuration plane k_cGo out to set up coordinate, if this coordinate is parallel to this configuration plane starting point Coordinate,Represent barycenter k_cCoordinate under coordinate system a+n, then

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

2) power and torque formula

Known force^a+nF_a+n, torqueAsk^aF_a,

(2) dynamic analysis method between configuration plane

If joint a is last joint of configuration plane, then joint a+1 is next configuration plane adjacent thereto First joint

1) speed formula

In formula (7), formula (8), order^aω_a、It is the angular speed in last joint of a upper configuration plane, angle adds Speed, i=1, then first angular speed in joint of next configuration plane adjacent thereto can be obtaining.Where a of joint Configuration plane be first configuration plane, then^aω_a、It is pedestal target angular speed, angular acceleration, if pedestal mark is motionless,

2) power and torque formula

Last joint (all joint numbers of mechanical arm are r)If end effector hand exists Space is free, then M_end、F_endEqual to zero.

Consider connecting rod in itself the influence of weight when, orderThe supporting role that i.e. robot base is subject to is quite upward Gravity acceleration g.So process just the same with the influence of each module gravity.

With reference to Fig. 6, citing verify the kinematics model of redundant mechanical arm, and the joint parameter of redundant mechanical arm of illustrating is such as Shown in table 1.

The joint parameter of table 1

Joint title	Range of movement	Joint parameter	Size
				Joint 1	-180°≤θ1≤180°	d1	350mm
Joint 2	-190°≤θ2≤10°	d2	450mm
				Joint 3	-100°≤θ3≤100°	d3	350mm
Joint 4	-100°≤θ4≤100°	d4	150mm
				Joint 5	-150°≤θ5≤150°	d5	145mm
Joint 6	-100°≤θ6≤100°	d6	150mm
				Joint 7	-180°≤θ7≤180°	d7	160mm

The mechanical arm trajectory planning mission requirements mechanical arm tail end locus of points is straight line section, during manipulator motion, The vertical straight path of paw clamping face, the Existential Space barrier in mechanical arm working space.Motion process for accelerate-it is even Speed-slow down.

Line segment starting point position auto―control is：

Line segment terminal position auto―control is：

The locus of the beginning and end according to task track, carries out space tracking interpolation calculating, is integrally divided into 10 sections, Totally 11 space tracking points, the space coordinates of tracing point is as shown in table 2.

The mechanical arm interpolation tracing point of table 2

Interpolated point	px	py	pz
				1	0	140.5	35
2	-6.8608	135.6706	40.8254
				3	-13.7216	130.8412	46.6509
4	-20.5824	126.0118	52.4763
				5	-27.4431	121.1824	58.3017
6	-34.3039	116.353	64.1272
				7	-41.1647	111.5236	69.9526
8	-48.0255	106.6942	75.7781
				9	-54.8863	101.8648	81.6035
10	-61.7471	97.0353	87.429
				11	-68.6079	92.2059	93.2544

The coordinate of the space tracking point according to table 2, according to configuration plane method for planning track, by configuration plane Matching, the interference detection of simultaneous computer tool arm and spatial obstacle thing, from starting point starting, determines the number of each joint angle of redundant mechanical Value, as shown in table 3.

Angle of each joint of the mechanical arm of table 3 in interpolated point

	θ1	θ2	θ3	θ4	θ5	θ6	θ7
								1	0	0	0	0	13.58	0	94.41
2	7.72	-18.55	30.27	-9.1	11.76	-20.3	54.15
								3	10.22	-23.5	39.6	-8.1	6.41	-37.82	47.35
4	14.22	-31.8	48.84	-9.5	6.46	-40.73	47.96
								5	17.22	-34.94	52.05	-6.71	9.5	-49.44	52.9
6	25.05	-37.82	50.37	-4.15	36.61	-52.82	76.45
								7	27.15	-40.02	51.48	-0.78	31.2	-58.19	77.16
8	31.2	-43.03	51.7	-0.83	31	-57.9	80.82
								9	35.15	-45.77	51.16	-1.24	29.94	-55.47	83.94
10	38.91	-48.13	49.69	-1.98	27.87	-50.74	86.01
								11	42.4	-49.75	46.8	-2.91	24.53	-43.69	86.41

Dynamics check is carried out to the space planning of overall redundant mechanical arm with configuration plane dynamic method, it is resulting Each joint angle speed change curves are as shown in Figure 7.As shown in Figure 8, mass motion process is shown in figure to M curve in motion process Shown in 9a- Fig. 9 d.

Claims (4)

1. a kind of redundant mechanical arm motion control method based on configuration plane, it is characterized in that comprising the following steps：

Step 1：Input redundant mechanical arm configuration parameter, configuration plane division is carried out with dynamics to redundant mechanical arm；It is described Configuration plane is divided includes following several classes：

(1) configuration plane terminal position is constant, and attitude is constant：Such configuration plane institute is only link block comprising module, is not deposited There is the module of amount of exercise change in swing module, revolution module, mobile module；

(2) configuration plane terminal position change, attitude is constant：Link block and mobile module are included in such configuration plane, no There is swing module and revolution module；

(3) configuration plane terminal position is constant, attitudes vibration：Comprising revolution module and basic connection mode in such configuration plane Block, if there is angle link block, in robot topological structure, angle link block is compared to basic link block closer to structure The center of shape plane；

(4) change of configuration plane terminal position, attitudes vibration：According to the definition of configuration plane, the machine of configuration plane no matter is constituted How device people motion module is moved, and its position and attitude can be changed in two dimensional surface, angle link block and revolution module As configuration plane separation；

Revolute joint is demarcated joint as specific configuration plane, during the quantity of swinging joint and linear joint is configuration plane Number of degrees of freedom；

Step 2：The configuration plane that will have been divided carries out planning, the structure at the beginning of redundant mechanical arm according to the mode that configuration plane is matched The matching of shape plane is carried out with the means of space vector representation for weighting；

Step 3：Each configuration plane of redundant mechanical arm to planning carries out the interference detection of spatial obstacle thing, to producing interference Configuration plane is readjusted and planned；

Step 4：Speed, the SPL smoothing processing of acceleration in joint are carried out to the redundant mechanical arm planning after adjustment；

Step 5：Dynamics check is carried out to whole motion process, it is to avoid mistake load that redundant mechanical arm occurs in motion process, The phenomenon of hypervelocity.

2. the redundant mechanical arm motion control method based on configuration plane according to claim 1, it is characterized in that described Configuration plane matching is specifically included with the means of space vector representation for weighting：

P_iIt is i-th point in mechanical arm tail end trajectory planning, O₀It is mechanical arm polar coordinate system origin, connects P_iO₀, k_jFor configuration is flat Line, j=1 ... n of the starting point of face i to distal point, with P_iO₀Vector plans the k of configuration plane to be oriented to_j；k₁Value with it is first Beginning, joint was relevant, k₁Space vector direction is, it is known that k_nIt is relevant with mechanical arm tail end joint, k_nThe direction of space vector and size All it is fixed, under conditions of mechanical arm working space is ensured, plans k_j, j=1 ... n-1, due to k_j, j=1 ... n-1 is not It is actual machine shoulder joint central axis, therefore k_j, j=1 ... n-1 may overlap interference with spatial obstacle thing, to configuration Plane and barrier are overlapped the part of interference, and checking interference inspection is carried out to joint of mechanical arm in the configuration plane and barrier Look into.

3. the redundant mechanical arm motion control method based on configuration plane according to claim 1, it is characterized in that described Spatial obstacle thing interference detection method is specifically included：

Spatial obstacle thing is configured that two dimensional surface where plane is crosscutting into sectional drawing figure, and this sectional view is irregular, using according to Secondary connected straightway is coated, and makes coated figure approximate regulation figure, coats area marking for barrier region 1, Extended out apart from k in barrier region 1, the actual area of gained barrier is labeled as region 2；

If safe obstacle-avoidance area is made up of m line segments, then i-th section of line segment is 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 sections of 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)$

Wherein：x_i, z_iFor i-th joint under space coordinates x-axis, the coordinate value on z-axis direction；

Compare formula (1) first and whether two slopes of line segment shown in formula (2) are equal, if equal, it is meant that two antennas Section will not then be interfered without intersection point, meet planning requirement；The intersection point of straight line where two lines section is solved if not waiting, if Not wherein on any one line segment, then mechanical arm is not interfered intersection point with barrier, meets planning requirement, is otherwise occurred dry Relate to.

4. the redundant mechanical arm motion control method based on configuration plane according to claim 1, it is characterized in that described Dynamics check method is specifically included：

Dynamics check method based on configuration plane is divided into two parts：Configuration plane internal dynamics analysis method and configuration are flat Dynamic analysis method between face；

(1) configuration plane internal dynamics analysis method

The free degree of k-th configuration plane is n, and joint a is k-th the first of configuration plane joint of composition, and joint b is revolution Joint, joint b is divided into configuration plane k+1；

1) speed formula

^aω_aIt is the angular speed of joint a, if the free degree of configuration plane is n, it is known that the speed of the starting point of configuration plane^aω_a, The speed in last joint of configuration plane is tried to achieve by speed formula^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}^j{e}_j---\left(3\right)$

${\stackrel{\·}{\mathrm{\ω}}_{a+n}}_{a+n}=R_a^{a+n}{\stackrel{\·}{\mathrm{\ω}}_a}_a+R_a^{a+n}{\mathrm{\ω}_a}_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(4\right)$

In the barycenter k of configuration plane k_cGo out to set up coordinate, if this coordinate is parallel to the coordinate of this configuration plane starting point,Table Show barycenter k_cCoordinate under coordinate system a+n, then

If configuration plane barycenter k_cCoordinate to the barycenter in each joint isJ=a to a+n,It is then each The coordinate of the barycenter in individual joint is to configuration plane barycenter k_cCoordinate be

2) power and torque formula

Known force^a+nF_a+n, torqueAsk^aF_a,

(2) dynamic analysis method between configuration plane

If joint a is last joint of configuration plane, then joint a+1 is the first of next configuration plane adjacent thereto Individual joint

1) speed formula

In formula (6), formula (7), order^aω_a、It is the angular speed in last joint of a upper configuration plane, angular acceleration, I=1, then obtain first angular speed in joint of next configuration plane adjacent thereto；If the configuration plane where a of joint It is first configuration plane, then^aω_a、It is pedestal target angular speed, angular acceleration, if pedestal mark is motionless,

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

${\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}---\left(15\right)$

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

2) power and torque formula

Last jointIf end effector hand is free, M in space_end、F_endIt is equal to Zero,

Consider connecting rod in itself the influence of weight when, orderThe quite upward gravity of supporting role that i.e. robot base is subject to Acceleration g；

Wherein：ⁱw_i：The expression of i-th angular speed in joint under i-th joint coordinate system；

ⁱv_i：The expression of i-th linear velocity in joint under i-th joint coordinate system；

The expression of i-th angular acceleration in joint under i-th joint coordinate system；

The expression of i-th linear acceleration in joint under i-th joint coordinate system；

q_j：J-th angular displacement in joint；

J-th itself angular acceleration in joint；

ⁱe_i：Coordinate system { O_iUnder Z axis identity component；

ⁱl_i+1：Expression of the length in i+1 joint under i-th joint coordinate system；

Joint i-1 is to the active force of connecting rod i in { O_iUnder expression；

Joint i-1 is to the opplied moment of connecting rod i in { O_iUnder expression；

ⁱF_i：Connecting rod i-1 is to the active force of connecting rod i in { O_iUnder expression；

ⁱM_i：Connecting rod i-1 is to the opplied moment of connecting rod i in { O_iUnder expression；

It is suffered at configuration plane barycenter to make a concerted effort；

Resultant moment suffered by configuration plane barycenter；

ⁱQ_i：Z-component of the i joint moments under its coordinate system；

m_i：I-th quality in joint；

F_end：External force suffered by mechanical arm tail end；

M_end：Moment of face suffered by mechanical arm tail end.