CN105234930A - 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

Based on the redundant mechanical arm motion control method of configuration plane

Technical field

What the present invention relates to is a kind of robot control method, specifically based on a redundant mechanical arm motion control track approach for configuration plane, the method can ensure redundant mechanical arm implementation space in the working space of complexity keep away the multiple target conditions such as barrier under motion control.

Background technology

The free degree of space motion object is 6, therefore robotic arm manipulation space is also 6, redundant mechanical arm has the joint more than the operating space free degree, mean in identical task, redundant mechanical arm has and more operates the free degree, can realize flexible motion more to control, and then meet the needs of actual task, therefore the research of redundant mechanical arm becomes the study hotspot in motion planning and robot control 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.

Summary of the invention

The object of the invention is to provide one not rely on robot configuration, and form is simple, and amount of calculation is few, and can be optimized solution on demand, has the redundant mechanical arm motion control method based on configuration plane of versatility and rapidity.

The object of the present invention is achieved like this:

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

Step 2: the configuration plane divided is carried out redundant mechanical arm according to the mode that configuration plane mates and just plans, described 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 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.

The present invention compared with prior art, has following outstanding substantial advantage and beneficial effect:

(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.

Fig. 3 space configuration plan-position is determined.

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

Fig. 5 motion control flow chart.

Fig. 6 redundant mechanical arm motion model figure.

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

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

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

Detailed description of the invention

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

Composition graphs 5, the overall step of method of the present invention 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.

Composition graphs 1,

The plane that the central axis that the configuration plane of mechanical arm is defined as the joint of robot that several connect successively is formed.According to the principle of space geometry, two planes are compared to line, and the central axis of revolute joint is as the line of demarcation between configuration plane.Configuration plane can be divided into following a few class for this reason:

(1) configuration plane terminal position is constant, and attitude is constant: module that this type of configuration plane comprises is only link block, there is not swing module, revolution module, mobile module etc. exist the module that amount of exercise changes;

(2) configuration plane terminal position change, attitude is constant: comprise link block and mobile module in this type of configuration plane, there is not swing module and revolution module;

(3) configuration plane terminal position is constant, attitudes vibration: comprise revolution module and basic link block in this type of configuration plane, if there is angle link block, then in robot topological structure, angle link block compares the center of basic link block closer to configuration plane;

(4) configuration plane terminal position change, attitudes vibration: according to the definition of configuration plane, how the robot motion's module no matter forming configuration plane moves, and its position and attitude are all can change at two dimensional surface.Angle link block and revolution module become configuration plane separation.Revolute joint is as specific configuration plane boundary joint, then the quantity of swinging joint and linear joint is the number of degrees of freedom in configuration plane.

Composition graphs 2, spatial obstacle thing is configured the two dimensional surface crosscut of plane place and becomes sectional drawing figure, this sectional view possibility very irregular, adopt the comparatively simple straightway be connected successively to carry out coated, make coated figure approximate regulation figure, coated region is not labeled as barrier region 1, by reference to the accompanying drawings 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 gained barrier is labeled as 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)$

Wherein: x _i, z _ibe i-th joint x-axis under space coordinates, the coordinate value on z-axis direction;

When there is obstacle and interfering situation, mean that joint of robot part exists with the boundary sections in composition safety obstacle region and interfere.In order to improve overall robot planning computational efficiency, saving computing time, only calculating in configuration plane and there is the joint of mechanical arm of interfering with barrier region.For the joint of mechanical arm occurring interference situation, re-start allotment according to the position shape matching process of configuration plane, meet planning requirement.

Whether in plane geometry, two straight lines are parallel or crossing, therefore, in order to improve the efficiency of deterministic process, first compare the slope of two line segments shown in formula (1) and formula (2), equal.If equal, mean that two antenna segment do not have intersection point, then can not interfere, meet planning requirement.If not etc., solve the intersection point of two line segment place straight lines, if intersection point is not wherein on any one line segment, then mechanical arm and barrier do not interfere, and meet planning requirement, otherwise interfere.

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, only need readjust in the interference joint in this configuration plane, judging in the safety obstacle region of interfering joint and between the boundary sections of robot base side.

Composition graphs 3, the locus of configuration plane is determined to adopt weighted space vector method, P _ifor the i-th point in mechanical arm tail end trajectory planning, O ₀for mechanical arm polar coordinate system initial point, 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.

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.

And it is fairly simple 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.

When spatial obstacle thing takies few to mechanical arm working space, the configuration plane space bit of mechanical arm has been equipped with more selection.By reference to the accompanying drawings 4, left hand view and right part of flg are the two states under identical tracking condition, and due to the requirement of movement locus, the configuration plane CPn at place, mechanical arm last joint determines, therefore under two states, configuration plane CPn is identical.

In this case, entirety to be carried out to the space tracking planning of mechanical arm to consider.The operating configuration of an above tracing point is reference, sets up evaluation function.

$f=\underset{h=1}{\overset{m}{\Σ}}{u}_h({\mathrm{\θ}}_i^h-{\mathrm{\θ}}_i^{,h})---\left(3\right)$

U in formula _h(h=1 ... m) be weighted value, the power and bit in this weighted value and h joint is equipped with pass, the joint that power is larger and close pedestal is nearer, this weighted value is larger, under multiple choices condition, high-power joints' movement range can be made by this weighted value, improve the dynamics function of mechanical arm; F is used for the difference of the operating configuration evaluating current trace points and a upper tracing point, under multiple choices condition, make every effort to minimum.

For ensureing mechanical end stable movement, what adopt B-spline curves carries out trajectory planning.Use the method for B-spline curves, make 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: the dynamic analysis method between configuration plane internal dynamics analytical method and configuration plane.Applying moment or the external force be applied on motion arm by driver makes motion arm move, and configuration plane internal dynamics analytical method mainly studies the relation between first joint and last joint power in this configuration plane, moment; Configuration interplanar dynamic analysis method mainly studies the relation between last joint and next configuration plane first joint power in this configuration plane, moment.

(1) configuration plane internal dynamics analytical method

By reference to the accompanying drawings 1, the free degree of a kth 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,

${\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---\left(4\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{e_j}_j+\underset{j=a+1}{\overset{a+n}{\Σ}}{\stackrel{\·\·}{q}}_j{e_j}_j---\left(5\right)$

Composition graphs 1, 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

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

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

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

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

${F_{a+n}}_{k_c}=\underset{j=a}{\overset{a+v}{\Σ}}{m}_j{\stackrel{\·}{v}_{a+n}}_{k_c}---\left(10\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

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

${M_{a+n}}_{k_C}={I_{k_c}}_{a+n}{\stackrel{\·}{\mathrm{\ω}}_{a+n}}_k+{\mathrm{\ω}_{a+n}}_k\×{I_{k_c}}_{a+n}{\mathrm{\ω}_{a+n}}_k---\left(12\right)$

2) force and moment formula

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

${M_a}_{J_a}=R_{a+n}^a\left({M_{a+n}}_{J_{a+n}}+{M_{a+n}}_{k_c}+{\mathrm{\ρ}_{a+n}}_{k_c}\×{F_{a+n}}_{K_C}\right)---\left(13\right)$

${Q_a}_a={M_a}_{J_a}*{e_a}_a---\left(14\right)$

(2) dynamic analysis method 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.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 ${\mathrm{\ω}_a}_a={\stackrel{\·}{\mathrm{\ω}}_a}_a=0.$

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

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

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

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

${F_{a+1}}_{a+1}=m_{a+1}{\stackrel{\·}{v}_{a+1}}_{c_{a+1}}---\left(19\right)$

${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}---\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.

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

${M_a}_{J_a}={M_a}_a+R_{a+1}^a{M_{a+1}}_{J_{a+1}}+{\mathrm{\ρ}_a}_a\×{F_a}_a+{l_a}_{a+1}\×R_{a+1}^a{F_{a+1}}_{J_{a+1}}---\left(22\right)$

${Q_a}_a={M_a}_{J_a}*{e_a}_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.

Composition graphs 6, the kinematics model verifying redundant mechanical arm is carried out in citing, and the joint parameter of citing redundant mechanical arm is as shown in table 1.

Table 1 joint parameter

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, in manipulator motion process, and paw clamping face this straight path vertical, Existential Space barrier in mechanical arm working space.Motion process is for accelerating-at the uniform velocity-deceleration.

Line segment starting point position auto―control is:

$\left[\begin{array}{cccc}0.9511& -0.3089& 0& 0\\ 0& 0& 1& 140.5\\ -0.3089& -0.9511& 0& 35\\ 0& 0& 0& 1\end{array}\right]---\left(24\right)$

Line segment terminal position auto―control is:

$\left[\begin{array}{cccc}0.7279& -0.6487& -0.2161& -68.6079\\ 0.5703& 0.4030& 0.7158& 92.2059\\ -0.3773& -0.6456& 0.6640& 93.2544\\ 0& 0& 0& 1\end{array}\right]---\left(25\right)$

According to the starting point of task track and the locus of terminal, carry out space tracking interpolation calculating, entirety is divided into 10 sections, totally 11 space tracking points, and the space coordinates of tracing point is as shown in table 2.

Table 2 mechanical arm interpolation tracing point

Interpolated point	p x	p y	p z
				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

According to the coordinate of the space tracking point shown in table 2, according to configuration plane method for planning track, by the coupling of configuration plane, the interference of simultaneous computer mechanical arm and spatial obstacle thing detects, initial from starting point, determine the numerical value of each joint angle of redundant mechanical, as shown in table 3.

The each joint of table 3 mechanical arm is in the angle of 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

Use configuration plane dynamic method to carry out dynamics check to the space planning of overall redundant mechanical arm, each joint angle speed change curves obtained as shown in Figure 7.M curve in motion process as shown in Figure 8, be shown in shown in Fig. 9 a-Fig. 9 d by mass motion process.

Claims (5)

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

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

Step 2: the configuration plane divided is carried out redundant mechanical arm according to the mode that configuration plane mates and just plans, described 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, the mistake load avoiding redundant mechanical arm to occur at motion process, the phenomenon of hypervelocity.

2. the redundant mechanical arm motion control method based on configuration plane according to claim 1, is characterized in that described configuration plane divides and comprises following a few class:

(1) configuration plane terminal position is constant, and attitude is constant: module that this type of configuration plane comprises is only link block, there is not swing module, module that revolution module, mobile module exist amount of exercise change;

(2) configuration plane terminal position change, attitude is constant: comprise link block and mobile module in this type of configuration plane, there is not swing module and revolution module;

(3) configuration plane terminal position is constant, attitudes vibration: comprise revolution module and basic link block in this type of configuration plane, if there is angle link block, then in robot topological structure, angle link block compares the center of basic link block closer to configuration plane;

(4) configuration plane terminal position change, attitudes vibration: according to the definition of configuration plane, how the robot motion's module no matter forming configuration plane moves, and its position and attitude are all can change at two dimensional surface, and angle link block and revolution module become configuration plane separation;

Revolute joint is as specific configuration plane boundary joint, and the quantity of swinging joint and linear joint is the number of degrees of freedom in configuration plane.

3. the redundant mechanical arm motion control method based on configuration plane according to claim 1, is characterized in that described configuration plane coupling specifically comprises with the means of space vector representation of weighting:

P _ifor the i-th point in mechanical arm tail end trajectory planning, O ₀for mechanical arm polar coordinate system initial point, connect P _io ₀, k _jfor the starting point of configuration plane i is to line, the j=1 of distal point ... n, with P _io ₀vector is the k of guiding, planning configuration plane _j; k ₁value relevant with initial joint, k ₁space vector direction is known, k _nrelevant with mechanical arm tail end joint, k _nthe direction of space vector and size are all fixing, under the condition ensureing mechanical arm working space, and planning k _j, j=1 ... n-1, due to k _j, j=1 ... n-1 is not actual machine shoulder joint central axis, therefore k _j, j=1 ... n-1 may overlap with spatial obstacle thing and interfere, and to overlap the part of interfering, carry out checking interference inspection to joint of mechanical arm in this configuration plane and barrier to configuration plane and barrier.

4. the redundant mechanical arm motion control method based on configuration plane according to claim 1, is characterized in that described spatial obstacle thing interference detection method specifically comprises:

Spatial obstacle thing is configured the two dimensional surface crosscut of plane place and becomes sectional drawing figure, this sectional view is irregular, the straightway be connected successively is adopted to carry out coated, make coated figure approximate regulation figure, coated region is not labeled as barrier region 1, extend out distance k at 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 segment, 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 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)$

Wherein: x _i, z _ibe i-th joint x-axis under space coordinates, the coordinate value on z-axis direction;

Whether the slope first comparing two line segments shown in formula (1) and formula (2) is equal, if equal, mean that two antenna segment do not have intersection point, then can not interfere, meet planning requirement; If not etc., solve the intersection point of two line segment place straight lines, if intersection point is not wherein on any one line segment, then mechanical arm and barrier do not interfere, and meet planning requirement, otherwise interfere.

5. the redundant mechanical arm motion control method based on configuration plane according to claim 1, is characterized in that described dynamics check method specifically comprises:

Dynamics check method based on configuration plane is divided into two parts: the dynamic analysis method between configuration plane internal dynamics analytical method and configuration plane;

(1) configuration plane internal dynamics analytical method

The free degree of a kth 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;

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,

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

${\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}^i{\mathrm{\ρ}}_i---\left(5\right)$

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

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

${\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})+{\stackrel{\·}{v}_a^{a+n}}_a---\left(8\right)$

${F_{a+n}}_{k_c}=\underset{j=a}{\overset{a+n}{\Σ}}{m_j}^{a+n}{\stackrel{\·}{v}}_{k_c}---\left(9\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

${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}^{k_c}{{\mathrm{\ρ}}_j}^T{I}_3-\mathrm{\ρ}_{k_c}{{{}_j}^T}^{k_c}{\mathrm{\ρ}}_j\left)\right){R_j^{a+n}}^T---\left(10\right)$

${M_{a+n}}_{k_C}={I_{k_c}}_{a+n}{\stackrel{\·}{\mathrm{\ω}}_{a+n}}_k+{\mathrm{\ω}_{a+n}}_k\×{I_{k_c}}_{a+n}{\mathrm{\ω}_{a+n}}_k---\left(11\right)$

2) force and moment formula

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

${M_a}_{J_a}=R_{a+n}^a({M_{a+n}}_{J_{a+n}}+{M_{a+n}}_{k_C}+{\mathrm{\ρ}_{a+n}}_{k_c}\×{F_{a+n}}_{k_C})---\left(12\right)$

${Q_a}_a={M_a}_{J_a}*{e_a}_a---\left(13\right)$

(2) dynamic analysis method 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 (6), formula (7), order ^aω _a, for the angular speed in last joint of a upper configuration plane, angular acceleration, i=1, then obtain the angular speed in first joint of next configuration plane adjacent with it; 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

${\mathrm{\ω}_{a+1}}_{a+1}={R_a^{a+1}}^a{\mathrm{\ω}}_a+{{\stackrel{\·}{q}}_{a+1}}^{a+1}{e}_{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)$

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

${F_{a+1}}_{a+1}=m_{a+1}{\stackrel{\·}{v}_{a+1}}_{c_{a+1}}---\left(18\right)$

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

2) force and moment formula

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

${F_a}_{J_a}={R_{a+2}^a}^{a+1}{F}_{J_{a+1}}+{F_a}_a---20)$

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

${Q_a}_a={M_a}_{J_a}*{e_a}_a---\left(22\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;

Wherein: ⁱw _i: the expression of angular speed under i-th joint coordinate system in i-th joint;

ⁱv _i: the expression of linear velocity under i-th joint coordinate system in i-th joint;

the expression of angular acceleration under i-th joint coordinate system in i-th joint;

the expression of linear acceleration under i-th joint coordinate system in i-th joint;

Q _j: the angular displacement in a jth joint;

self angular acceleration in a jth joint;

ⁱe _i: coordinate system { O _iunder Z axis identity component;

ⁱl _i+1: the expression of length under i-th joint coordinate system in the i-th+1 joint;

joint i-1 to the active force of connecting rod i at { O _iunder expression;

joint i-1 to the opplied moment of connecting rod i at { O _iunder expression;

ⁱf _i: connecting rod i-1 to the active force of connecting rod i at { O _iunder expression;

ⁱm _i: connecting rod i-1 to the opplied moment of connecting rod i at { O _iunder expression;

configuration plane barycenter place be subject to make a concerted effort;

configuration plane barycenter place be subject to resultant moment;

ⁱq _i: the Z axis component of i joint moment under its coordinate system;

M _i: the quality in i-th joint;

F _end: external force suffered by mechanical arm tail end;

M _end: moment of face suffered by mechanical arm tail end.