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

Abstract

The present invention relates to robotics, provide a kind of redundant mechanical arm motion control method based on configuration plane, the method ensure that redundant mechanical arm realize the multiple targets such as space avoidance in complicated work space under the conditions of motor control.The present invention comprises the steps：With kinetics, configuration plane division is carried out to redundant mechanical arm；Mode according to configuration plane coupling carries out planning at the beginning of redundant mechanical arm；The interference detection of spatial obstacle thing is carried out to each configuration plane of the redundant mechanical arm of planning, the configuration plane producing interference is readjusted and plans；Redundant mechanical arm planning after adjustment is carried out with the SPL smoothing processing of the speed in joint, acceleration；Dynamics check is carried out to whole motor process.The present invention combines construction featuress and the working method of redundant mechanical arm, and configuration plane is incorporated in the motor control of redundant mechanical arm, and the method intuitively can quickly realize the motor control planning of redundant mechanical arm.

Description

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

Technical field

The present invention relates to robotics are and in particular to a kind of ensure that redundant mechanical arm is empty in complicated work Between in realize the redundant mechanical arm motor control side based on configuration plane of motor control under the conditions of the multiple targets such as space avoidance Method.

Background technology

For space tasks it is considered to the position of end and attitude, the dimension of work space is six.Redundant mechanical arm redundancy More than 1.The research of redundant mechanical arm has become as the study hotspot of robot field.

Many experts propose a lot of methods in terms of the motor control of redundant mechanical arm.Free space based on C space Method sets up C space with the joint coordinate system of mechanical arm, and barrier is mapped to C space, forms steric configuration obstacle, thus trying to achieve The supplementary set in C space, 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 is capable of mechanical arm collision-free Trajectory Planning of Welding, due to barrier being mapped Complex to C space-wise, complex environment is difficult to meet the requirement of real-time.In order to realize space avoidance, to obstacle Define a repulsion potential field, at impact point, define an attraction potential field, the motion of mechanical arm is come by two potential field collective effect power Determine, thus to ensure that mechanical arm smoothly reaches final goal point while avoidance.The method is advised for processing global path Dynamic obstacle avoidance in drawing is highly effective, but is easily trapped at local minimum points.

When motor control is carried out to robot, the angular acceleration constraint 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 affects very little.But, when robot movement velocity is very fast, joint angular acceleration and angular velocity easily exceed constraint Scope, leads to driving current excessive or occurs beyond spacing accident.Light then robot motion error, heavy then damage hardware. Now it is necessary to consider various constraintss in robot motion's trajectory planning, most common method is to movement time It is optimized.For example, there is research worker when intercepting the object of rapid flight with mechanical arm, the movement locus of mechanical arm are applied Joint velocity constraint and moment constraint.Scholar is also had to be deduced the inverse solution of parsing of the 7R mechanical arm 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 work space. Numerous scholars reach a motion planning to the apery of 7R mechanical arm and expand research.Also work space can be divided into little one by one Lattice, obtain in each little lattice anthropomorphic joint angles, set up look-up table, terminal position as requested, find in a lookup table Redundancy angle q3, obtains other joint angles according to the inverse solution of parsing.MINH realizes anthropomorphic motion using the mode of motion unit.But these grind Study carefully work, it is important that reaching a motion in apery, considers avoidance.In order to improve its search efficiency,

Content of the invention

Present invention aim at providing a kind of redundant mechanical arm inverse kinematics control method.The method is independent of robot structure Shape, form is simple, reduces amount of calculation, can obtain on demand optimizing solution, have versatility and rapidity.

The object of the present invention is achieved like this：

A kind of method carrying out redundant mechanical arm motor control with configuration plane, comprises the steps：

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

Step 2：Mode according to configuration plane coupling carries out at the beginning of redundant mechanical arm planning, and configuration plane coupling is to weight Means of space vector representation is carried out；

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

Step 4：The speed in joint is carried out to the redundant mechanical arm planning after adjustment, the SPL of acceleration smooths place Reason；

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

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

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

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

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

Interference joint in this configuration plane is readjusted, and makes interference joint and safety obstacle region near machine Judged between the boundary sections of people's pedestal side.

Under conditions of the constraint of known joint of mechanical arm, mechanical arm tail end track, spatial obstacle object location, described configuration Plane is passed through to form the space tracking that redundant mechanical arm plans mechanical arm：

Space joint constraint is：

The locus of described configuration plane determine using weighted space vector method, P_iFor in mechanical arm tail end trajectory planning I-th point, connect P_iO₀, k_j(j=1 ... n) starting point for configuration plane i is to the line of distal point；

With P_iO₀Vector is guiding, the k of planning configuration plane_j(j=1 ... n)；In the condition ensureing mechanical arm work space Under, plan k_j(j=1 ... n-1)；Overlap the part of interference to configuration plane and barrier, with the method introduced to this In configuration plane, joint of mechanical arm and barrier carry out checking interference inspection.

The degree of freedom of described configuration plane is n, and joint a is first joint of k-th configuration plane of composition, and joint b is Revolute joint, is divided into joint b in configuration plane k+1

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

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

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

Power and moment are

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

Joint a is last joint of configuration plane, then joint a+1 is the of next configuration plane adjacent thereto One joint, order^aω_a、For the angular velocity in last joint of a upper configuration plane, angular acceleration, i=1, joint a The configuration plane being located is first configuration plane, then^aω_a、For pedestal target angular velocity, angular acceleration, pedestal mark is not Dynamic, then

Last jointEnd effector handss are free in space, then M_end、F_endIt is equal to Zero.；

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

The present invention compared with prior art, the beneficial effects of the present invention is：

(1) combine construction featuress and the working method of redundant mechanical arm, configuration plane is incorporated into the fortune of redundant mechanical arm In dynamic control, the method intuitively can quickly realize the motor control planning of redundant mechanical arm.

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

(3) dynamic method based on configuration plane.The method, in units of configuration plane, reduces and calculates and solve step Suddenly, can quickly check joint motions performance in planning process for the mechanical arm, for optimize and adjustment mechanical arm planning provide according to According to.

Brief description

Fig. 1 configuration plane divides schematic diagram accompanying drawing；

Barrier in Fig. 2 configuration plane interferes detection figure；

Fig. 3 space configuration plan-position determines figure；

Fig. 4 motor control flow chart.

Specific embodiment

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

The invention discloses a kind of method carrying out redundant mechanical arm motor control with configuration plane.By joint of mechanical arm Constraint and the restriction of spatial obstacle, the motor control of redundant mechanical arm is a complex process.The present invention is from composition redundant mechanical Arm configuration plane is started with, the method for utilization space geometry, carries out space motion control to redundant mechanical arm, is drawn by space vector Lead, the comparison in avoidance path, dynamics check, be quickly found out the path of space optimization, realize multi-target track planing method.Should Method is independent of robot configuration, and form is simple, reduces the difficulty of solution, reduces amount of calculation, can be optimized on demand Solution, has versatility and rapidity.

The technical solution used in the present invention is：

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

Step 2：Mode according to configuration plane coupling carries out at the beginning of redundant mechanical arm planning, and configuration plane coupling is to weight Means of space vector representation is carried out.

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

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

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

A kind of method carrying out redundant mechanical arm motor control with configuration plane, carries out space motion to redundant mechanical arm Control, by space vector guiding, the comparison in avoidance path, dynamics check, be quickly found out the path of space optimization, realize many Target trajectory planing method.

Its overall step is as follows：

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

Step 2：Mode according to configuration plane coupling carries out at the beginning of redundant mechanical arm planning, and configuration plane coupling is to weight Means of space vector representation is carried out.

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

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

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

Configuration plane with dynamic (dynamical) redundant mechanical arm divides.First joint in configuration plane is that revolution is closed Section, last joint is swinging joint or linear joint.First joint of first configuration plane can not be revolution Joint.If last joint of serial manipulator is revolute joint, then a configuration plane is individually regarded as in this joint.Flat In the configuration of face, revolute joint no affects on the distance between configuration end and configuration center relation, identical with connecting rod effect, only Swinging joint and linear joint can change configuration end and configuration center relation, therefore, swinging joint and linear joint Quantity is the number of degrees of freedom in configuration plane.

Three dimensions barrier is configured plane place two dimensional surface crosscutting one-tenth sectional drawing figure, and this sectional view may be very Irregularly, coated using the relatively simple straightway being sequentially connected, as shown in Figure 4 it is contemplated that robot links have Overall dimensions and safe distance, therefore extend out apart from k in barrier region 1, and the actual area of such barrier is just changed into region 2 State.

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：

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

In plane geometry, or two straight lines are parallel intersecting, therefore, in order to improve the efficiency of judge process, first Compare the slope of two line segments shown in formula (1) and formula (2), if equal, do not wait and then solve two lines section place straight line Intersection point, this intersection point whether be easy between this two lines section judge.

According to the process of redundant mechanical arm trajectory planning, the center of configuration plane and end can not be in barrier zones , otherwise redundant mechanical arm cannot complete work.Therefore there is obstacle and interfere situation, be joint of robot part and composition The boundary sections in safety obstacle region exist interferes.In order to improve overall robot planning computational efficiency, also need not will be all of All judged between all line segments in joint of robot and composition safety obstacle region, only need to interference in this configuration plane Joint is readjusted, and so that interference joint and safety obstacle region is carried out between the boundary sections of robot base side Judge.

Make every effort to quickly succinct based on the trajectory planning process of configuration plane, it is to avoid complicated calculations process.In known machinery Shoulder joint constraint, mechanical arm tail end track, under conditions of spatial obstacle object location, by forming the configuration plane of redundant mechanical arm The space tracking of planning mechanical arm.

Space joint constraint is shown below：

The locus of configuration plane determine using weighted space vector method, as shown in Figure 3, in figure P_iFor mechanical arm end In the trajectory planning of end i-th point, connects P_iO₀, k_j(j=1 ... n) starting point for configuration plane i is to the line of distal point.

With P_iO₀Vector is guiding, the k of planning configuration plane_j(j=1 ... n).k₁Value 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 fixing 's.Under conditions of ensureing mechanical arm work space, plan k_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 be overlapped interference with spatial obstacle thing.To configuration plane Overlapped the part of interference with barrier, with the method introduced, joint of mechanical arm in this configuration plane and barrier is carried out Checking interference checks.Will not be so complicated in actual operation, because configuration plane has simplified has redundancy work(in mechanical arm The joint of energy, forms mechanical arm configuration plane negligible amounts, generally in 3-5 about, after planning a small amount of configuration plane, remaining 2 configuration planes can determine its locus by way of parsing.

Configuration plane internal dynamicss are analyzed

As shown in the schematic diagram of k-th configuration plane of Fig. 2, the degree of freedom of this configuration plane is n, and joint a is k-th of composition First joint of configuration plane, because joint b is revolute joint, is considering angular velocity, during angular acceleration Superposition Formula, in order to Convenient calculating, is divided into joint b in configuration plane k+1.

1) speed formula

^aω_aFor the angular velocity of joint a, if the degree of freedom of configuration plane is n it is known that the speed of the starting point of configuration plane^a ω_a, Negotiation speed formula tries to achieve the speed in last joint of configuration plane^a+nω_a+n, ^{a+ n}v_a+n,

As shown in Fig. 2 the barycenter k in configuration plane k_cGo out to set up coordinate, if this coordinate is parallel to this configuration plane starting point The coordinate at place,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 moment formula

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

(2) dynamic analyses 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、For the angular velocity in last joint of a upper configuration plane, angle adds Speed, i=1, then the angular velocity in first joint of next configuration plane adjacent thereto can be to obtain.If joint a institute Configuration plane be first configuration plane, then^aω_a、For pedestal target angular velocity, angular acceleration, if pedestal mark is not Dynamic, then

2) power and moment formula

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

During the impact of consideration connecting rod weight itself, orderThe supporting role that i.e. robot base is subject to is quite upwards Gravity acceleration g.So process just the same with the impact of each module gravity.

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, has very high precision and solving speed, has very strong practicality Property and versatility.

Implement 1, in conjunction with accompanying drawing 1, the configuration plane with dynamic (dynamical) redundant mechanical arm divides.In configuration plane first Individual joint is revolute joint, and last joint is swinging joint or linear joint.First pass of first configuration plane Section can not be revolute joint.If last joint of serial manipulator is revolute joint, then this joint is individually regarded as One configuration plane.In plane configuration, revolute joint no affects on the distance between configuration end and configuration center relation, with company Bar effect is identical, and only swinging joint and linear joint can change configuration end and configuration center relation, therefore, wave pass The quantity of section and linear joint is the number of degrees of freedom in configuration plane.

Implement 2, in conjunction with accompanying drawing 2, three dimensions barrier is configured plane place two dimensional surface crosscutting one-tenth sectional drawing figure, this The possible very irregular of individual sectional view, is coated using the relatively simple straightway being sequentially connected it is contemplated that robot connects Bar has overall dimensions and safe distance, therefore extends out apart from k in barrier region 1, and the actual area of such barrier just becomes State for 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：

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

In plane geometry, or two straight lines are parallel intersecting, therefore, in order to improve the efficiency of judge process, first Compare the slope of two line segments shown in formula (1) and formula (2), if equal, do not wait and then solve two lines section place straight line Intersection point, this intersection point whether be easy between this two lines section judge.

According to the process of redundant mechanical arm trajectory planning, the center of configuration plane and end can not be in barrier zones , otherwise redundant mechanical arm cannot complete work.Therefore there is obstacle and interfere situation, be joint of robot part and composition The boundary sections in safety obstacle region exist interferes.In order to improve overall robot planning computational efficiency, also need not will be all of All judged between all line segments in joint of robot and composition safety obstacle region, only need to interference in this configuration plane Joint is readjusted, and so that interference joint and safety obstacle region is carried out between the boundary sections of robot base side Judge.

Implement 3, in conjunction with accompanying drawing 3, make every effort to quickly succinct based on the trajectory planning process of configuration plane, it is to avoid complicated calculations mistake Journey.Under conditions of the constraint of known joint of mechanical arm, mechanical arm tail end track, spatial obstacle object location, by forming redundancy The configuration plane of mechanical arm plans the space tracking of mechanical arm.

Space joint constraint is shown below：

Implement 3, the locus of configuration plane determine using weighted space vector method, as shown in Figure 3, in figure P_iFor machine In the planning of tool arm end orbit i-th point, connects P_iO₀, k_j(j=1 ... n) starting point for configuration plane i is to the company of distal point Line.

With P_iO₀Vector is guiding, the k of planning configuration plane_j(j=1 ... n).k₁Value 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 fixing 's.Under conditions of ensureing mechanical arm work space, plan k_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 be overlapped interference with spatial obstacle thing.To configuration plane Overlapped the part of interference with barrier, with the method introduced, joint of mechanical arm in this configuration plane and barrier is carried out Checking interference checks.Will not be so complicated in actual operation, because configuration plane has simplified has redundancy work(in mechanical arm The joint of energy, forms mechanical arm configuration plane negligible amounts, generally in 3-5 about, after planning a small amount of configuration plane, remaining 2 configuration planes can determine its locus by way of parsing.

Enforcement 4, dynamics check method,

Shown in the schematic diagram of k-th configuration plane, the degree of freedom of this configuration plane is n, and joint a is k-th configuration of composition First joint of plane, because joint b is revolute joint, is considering angular velocity, during angular acceleration Superposition Formula, for convenience Calculate, joint b is divided in configuration plane k+1.

1) speed formula

^aω_aFor the angular velocity of joint a, if the degree of freedom of configuration plane is n it is known that the speed of the starting point of configuration plane^a ω_a, Negotiation speed formula tries to achieve the speed in last joint of configuration plane^a+nω_a+n, ^{a+ n}v_a+n,

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

If configuration plane barycenter k_cCoordinate to the barycenter in each joint isTo a+n), Then the coordinate of the barycenter in each joint is to configuration plane barycenter k_cCoordinate be

2) power and moment formula

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

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、For the angular velocity in last joint of a upper configuration plane, angle adds Speed, i=1, then the angular velocity in first joint of next configuration plane adjacent thereto can be to obtain.If joint a institute Configuration plane be first configuration plane, then^aω_a、For pedestal target angular velocity, angular acceleration, if pedestal mark is not Dynamic, then

2) power and moment formula

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

During the impact of consideration connecting rod weight itself, orderThe supporting role that i.e. robot base is subject to is quite upwards Gravity acceleration g.So process just the same with the impact of each module gravity.

Implement 5, in conjunction with accompanying drawing 4, its overall step is as follows：

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

Step 2：Mode according to configuration plane coupling carries out at the beginning of redundant mechanical arm planning, and configuration plane coupling is to weight Means of space vector representation is carried out.

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

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

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

Claims (2)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Power and moment are

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

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

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

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

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

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

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

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

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

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

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

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

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

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