CN110076783A - A kind of plane activation lacking mechanical arm position control method based on fuzzy control and liapunov function - Google Patents
A kind of plane activation lacking mechanical arm position control method based on fuzzy control and liapunov function Download PDFInfo
- Publication number
- CN110076783A CN110076783A CN201910517174.8A CN201910517174A CN110076783A CN 110076783 A CN110076783 A CN 110076783A CN 201910517174 A CN201910517174 A CN 201910517174A CN 110076783 A CN110076783 A CN 110076783A
- Authority
- CN
- China
- Prior art keywords
- joint
- angle
- mechanical arm
- control
- stage
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J17/00—Joints
- B25J17/02—Wrist joints
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
- B25J9/023—Cartesian coordinate type
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The embodiment of the invention discloses a kind of plane activation lacking mechanical arm position control method based on fuzzy control and liapunov function.It include: to establish active (PAA type) the activation lacking mechanical arm kinetic model of the passive-active-of planar three freedom, and analyze the integral constraint relationship between its each joint angles, angular speed;Its reachable tree is analyzed, calculates each diarthrodial target angle of drive using particle swarm algorithm;The segmentation position control law based on fuzzy control and liapunov function is designed, first stage design fuzzy controller makes second joint reach target angle, and design liapunov function makes third joint keep original state;Second stage design fuzzy controller makes third joint reach target angle, and design liapunov function makes second joint keep angle constant, realizes position control.The technical solution that example is proposed according to the present invention can guarantee the stability of position control, and overshoot and steady state time are decreased obviously.
Description
[technical field]
The invention belongs to technical field of automatic control, and it is direct to be related to model reduction, fuzzy control and Liapunov
Control methods, it is specifically a kind of that position controlling party is segmented based on the plane activation lacking mechanical arm of fuzzy control and liapunov function
Method.
[background technique]
With the development of aeronautical and space technology and Robot Control Technology, be difficult to repair especially for space manipulator and
Underwater robot structure actual conditions compact to design, the research of Underactuated Mechanical Systems receive global extensive concern.
Since the external control input number of Underactuated Mechanical Systems is less than freedom degree number, the complete mature control driven in mechanical arm
Method processed, such as overall-finished housing linearization technique, PID control methods cannot be pushed away directly based on passive adaptive control law etc.
Extensively arrive in activation lacking mechanical arm.Therefore, the control problem of activation lacking mechanical arm is the recognized problem in the field.
Activation lacking mechanical arm can be divided into horizontal plane movement according to structure and perpendicular moves two kinds.In perpendicular
The activation lacking mechanical arm of movement only has highest and lowest two equilbrium positions.But horizontal activation lacking mechanical arm is not by gravity shadow
It rings, there is numerous equilbrium position.And first joint be the horizontal activation lacking mechanical arm of passive joint be nonholonomic system, this makes
Its position control is more difficult.It is two two free by three-degree of freedom under-actuated tool arm depression of order using the thought of model reduction
Spend mechanical arm.In existing control method, according to the state equation of system, only by construction liapunov function, control is derived
System rule.This method is capable of the control law of design stability, but is difficult to find that optimal liapunov function, so control is fast
Speed is bad.The control of plane PAA type mechanical arm position is realized based on single sliding formwork control method, and mechanical arm is divided into several subsystems
System, respectively each subsystem design sliding-mode surface and switching function, reconstruct total sliding-mode surface, stablize whole system.But the party
There is buffeting in method, steady-state performance is bad.The dynamic characteristic of control process also urgently optimizes.
[summary of the invention]
In view of this, being difficult to the problem of taking into account rapidity and accuracy in view of existing control method, the present invention proposes one kind
Position control method is segmented based on the plane activation lacking mechanical arm of fuzzy control and liapunov function, comprising:
Active (PAA type) the activation lacking mechanical arm kinetic model of the passive-active-of planar three freedom is established, and is analyzed
Integral constraint relationship between its each joint angles, angular speed;
Its reachable tree is analyzed, calculates each diarthrodial target angle of drive using particle swarm algorithm;
The segmentation position control law based on fuzzy control and liapunov function is designed, the first stage designs fuzzy control
Device makes second joint reach target angle, and design liapunov function makes third joint keep original state;Second stage is set
Meter fuzzy controller makes third joint reach target angle, and design liapunov function makes second joint keep angle constant,
Realize position control.
In the above method, active (PAA type) the activation lacking mechanical arm dynamics of the passive-active-of planar three freedom is established
Model, and analyze the integral constraint relationship between its each joint angles, angular speed, comprising:
Planar three freedom is passive-actively-active (PAA type) activation lacking mechanical arm is not influenced by gravitation, therefore is not had to
Consider potential energy, and be freely rotatable at passive joint do not have braking moment and frictional force, research object has been reduced to one just
It is as follows to establish its kinetic model according to Lagrange's equation for body structure:
Wherein, Mij∈R3×3(i, j=1,2,3) specifically,
ak(k=1,2 ..., 6) be system structural parameters, qi(i=1,2,3) is the i-th joint angle of mechanical arm,For the i-th joint angular acceleration of mechanical arm;
miFor the quality (i=1,2,3) of the i-th bar, LiFor the length (i=1,2,3) of the i-th bar, liBefore being arrived for the i-th bar mass center
The length (i=1,2,3) in one joint, JiFor the rotary inertia (i=1,2,3) of the i-th bar;Hi∈R3×1It (i=1,2,3) is Ge Shi
Power and centrifugal force item,
τ=(0, τ2,τ3)TFor joint moment vector, wherein the output torque of passive joint is 0,For machinery
I-th joint angular speed of arm;
Plane PAA type activation lacking mechanical arm is single order nonholonomic system, and plane PA type mechanical arm is holonomic system;Based on mould
Control process is divided into two stages by the thought of type depression of order, maintains third joint initial angle constant in the first stage, control the
Two joints reach target angle;Maintain second joint target angle constant in second stage, control third joint reaches target angle
Degree;Row locating for drive lacking is taken out in formula (1), and formula (4) are substituted into and can be obtained:
Using integration by parts, formula (5) seeks definite integral to time t, can obtain:
It maintains angle constant in each control stage, joint angular speed can be approximately consideredIt is zero, therefore, to formula (6) two
Definite integral is sought at end again, and abbreviation can obtain:
Wherein, q1 1(0) by lever initial angle, q when the expression first stage2(0) and q3(0) the second bar and are respectively indicated
The initial angle of three bars, D1,E1,G1,g1For the middle entry of formula (7) abbreviation process,
D1=(8a3a5-4a2a6-4a1a6-4a4a6)cosq2(0)-a1 2-a2 2- 4a6 2cos2q2(0)-a4 2+4a5 2+4a3 2-
2a1a2-2a1a4-2a2a4
G1=(a1+a2+a4+2a6cosq2(0)-2a5cosq2(0)-2a3)(cosq2-1)+2a5sinq2(0)sinq2
Similarly, it maintains second joint angle constant in the control of second stage, joint angular speed can be approximately considered
It is zero, asks definite integral that the constraint of second stage joint angles can be obtained at formula (6) both ends again.
In the above method, its reachable tree is analyzed, calculates each diarthrodial target angle of drive, packet using particle swarm algorithm
It includes:
The working space of mechanical arm refers to the set for the spatial position that robot arm end effector can reach, by mechanical arm
Working field is denoted as A (p), then each joint angles of mechanical arm and the mapping of work reachable tree may be expressed as:
A (p)=p (θ) | θ ∈ Q } (8)
Wherein Q is the constraint space of joint angle, is constrained using joint limit angle as arm angle, it may be assumed that
In formula,WithIndicate the lower and upper limit of joint motions, θnIndicate n-th of joint angles.In addition, PA type machine
There are angle restrictions, and joint angle constraint space should also be added between the active and passive joint of tool arm;It is calculated with monte carlo method more
The essence of the working space of articulated mechanical arm be to joint variable by be uniformly distributed be assigned to it is a certain number of meet joints variation
It is required that random quantity, then each joint variable is combined again, and mechanical using the direct kinematics recurrence calculation of mechanical arm
The coordinate value of arm end effector endpoint, the set that these coordinate values are constituted are the reachable tree of mechanical arm tail end;
Plane PAA type activation lacking mechanical arm terminal position cartesian coordinate can indicate are as follows:
Wherein (X, Y) is the desired cartesian coordinate in end;Choose mechanical arm tail end coordinate and the conduct of desired location difference
Evaluation function, which can make optimal region of the population constantly into solution space mobile, and then obtain the mesh of drive rod
Mark angle;
The step of particle swarm optimization algorithm, is as follows:
1) initial population for generating m particle at random, initializes speed and the position of each particle, gives inertia weight
ω, Studying factors c1 and c2, pbest, gbest;
2) fitness of each particle is calculated;
3) flight position and speed of each particle are updated according to formula (11), (12);
vi=vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbesti-xi) (11)
xi=xi+vi (12)
4) if the fitness of particle is set as pbest better than original individual extreme value pbest, current fitness, selection is most
Excellent individual extreme value is as group extreme value gbest;
5) judge whether algorithm reaches termination condition.If so, output result;Otherwise the cyclic part from 1 to M is carried out,
Wherein M is maximum number of iterations;
Two are calculated using particle swarm algorithm and drives diarthrodial target angle, are laid the foundation for follow-up location control.
In the above method, the segmentation position control law based on fuzzy control and liapunov function, first stage are designed
Design fuzzy controller makes second joint reach target angle, and design liapunov function makes third joint keep initial shape
State;Second stage design fuzzy controller makes third joint reach target angle, and design liapunov function makes second joint
It keeps angle constant, realizes position control, comprising:
It is as follows that kinetic model is rewritten as affine nonlinear system form first:
Wherein, F, G are the nonlinear function about state variable x,
[G1 G2 G3]T=-M-1(q);According to the control mesh that the definition of liapunov function and mechanical arm tail end are stable in target point
Mark will also make the angle of drive link while the function constructed guarantees that the angle of drive link is finally intended to target angle
Speed finally converges to zero, therefore the third connecting rod liapunov function constructed is as follows:
Wherein, V3(x) it is a positive semidefinite function, derivation is carried out to above formula, and system state equation formula (12) are substituted into
It can obtain:
Wherein a22For the item of the second row secondary series in G matrix, control law can be acquired according to above formula:
τ3=(- x5+x5(0)-F3-μx5-aa22τ2)/aa22 (16)
Wherein μ is the constant greater than 0, for adjusting convergence rate;x5It (0) is the initial set value of third joint angle.It can
To see, the output torque in third joint changes with the output torque of second joint, and no matter how second joint angle becomes
Change, third joint is stable in the initial angle of setting.Next the Fuzzy Control Law of design second connecting rod, takes joint angles to miss
Difference and error rate are inputted as fuzzy system, and the torque of second joint carries out anti fuzzy method using gravity model appoach as output,
When second joint angle reaches target position, and angular speed is 0, control strategy enters second stage;It is similar with the first stage,
The liapunov function for constructing second joint respectively, keeps the initial angle of second joint constant, designs fuzzy controller
Realize the control target in third joint;When drive rod angle is stable in respective target angle respectively, according to the pact between angle
Beam relationship, the related angle control for realizing drive lacking connecting rod, the final position control for realizing PAA type mechanical arm;
[Detailed description of the invention]
In order to illustrate the technical solution of the embodiments of the present invention more clearly, below will be to needed in the embodiment attached
Figure is briefly described, it should be apparent that, drawings in the following description are only some embodiments of the invention, for this field
For those of ordinary skill, without any creative labor, it can also be obtained according to these attached drawings other attached
Figure.
Fig. 1 shows present example control method block diagrams;
Fig. 2 indicates mechanical arm reachable tree;
The relationship of Fig. 3 expression mechanical arm tail end position and each joint angle;
Fig. 4 indicates joint angles subordinating degree function;
Fig. 5 indicates joint angular speed subordinating degree function;
Fig. 6 indicates position control change procedure;
Fig. 7 indicates first stage controlling curve;
Fig. 8 shows second stage controlling curves.
[specific embodiment]
Technical solution for a better understanding of the present invention with reference to the accompanying drawing retouches the embodiment of the present invention in detail
It states.
It will be appreciated that described embodiment is only a part of the embodiments of the present invention, instead of all the embodiments.
Based on the embodiments of the present invention, obtained by those of ordinary skill in the art without making creative efforts all
Other embodiments shall fall within the protection scope of the present invention.
It is a kind of that position control method, tool are segmented based on the plane activation lacking mechanical arm of fuzzy control and liapunov function
Body comprises the following steps that
1. active (PAA type) the activation lacking mechanical arm kinetic model of the passive-active-of planar three freedom is established, and point
Analyse the integral constraint relationship between its each joint angles, angular speed;2. analyzing its reachable tree, calculated using particle swarm algorithm each
Drive diarthrodial target angle;3. designing the segmentation position control law based on fuzzy control and liapunov function, the first rank
Section design fuzzy controller makes second joint reach target angle, and design liapunov function makes third joint keep initial shape
State;Second stage design fuzzy controller makes third joint reach target angle, and design liapunov function makes second joint
It keeps angle constant, realizes position control.
The embodiment of the present invention is provided based on fuzzy and liapunov function plane activation lacking mechanical arm segmentation position control
Method processed, referring to FIG. 1, it provides the plane activation lacking mechanical based on fuzzy and liapunov function for the embodiment of the present invention
Arm is segmented the control process block diagram of position control method, method includes the following steps:
Step 101, active (PAA type) the activation lacking mechanical arm kinetic simulation of the passive-active-of planar three freedom is established
Control process is decomposed into two control stages according to model reduction, and analyzes the integral between each joint angles, angular speed by type
The constraint relationship;
Specifically, establishing active (PAA type) the activation lacking mechanical arm kinetic model of the passive-active-of planar three freedom
It is as follows:
Wherein, Mij∈R3×3(i, j=1,2,3) specifically,
ak(k=1,2 ..., 6) be system structural parameters, qi(i=1,2,3) is the i-th joint angle of mechanical arm,For the i-th joint angular acceleration of mechanical arm;
miFor the quality (i=1,2,3) of the i-th bar, LiFor the length (i=1,2,3) of the i-th bar, liBefore being arrived for the i-th bar mass center
The length (i=1,2,3) in one joint, JiFor the rotary inertia (i=1,2,3) of the i-th bar;Hi∈R3×1It (i=1,2,3) is Ge Shi
Power and centrifugal force item,
τ=(0, τ2,τ3)TFor joint moment vector, wherein the output torque of passive joint is 0,For machinery
I-th joint angular speed of arm;
Holonomic system in mechanics refers to that institute's Prescribed Properties an of system can directly be become not by integral way
The constraint of the difference quotient containing coordinate, the movement of holonomic system is limited on a smooth hypersurface, in fact, most of mechanical systems
It is considered as nonholonomic system, the second order that the single order nonholonomic system and acceleration that can not be accumulated including speed can not accumulate
Nonholonomic system, integrability condition are defined as follows:
Define 1: the first integrability condition: torque caused by potential energy item is constant;Unactuated joint variable is not present in used
In property matrix;
Define 2: the second integrability conditions: the first integrability condition is set up;Drive lacking item kernel in inertial matrix is distributed
Pairing;
According to above-mentioned definition it is found that plane PAA type activation lacking mechanical arm is single order nonholonomic system, plane PA type is mechanical
Arm is holonomic system;Control process is divided into two stages by the thought based on model reduction, maintains third joint in the first stage
Initial angle is constant, and control second joint reaches target angle;Maintain second joint target angle constant in second stage, control
Third joint reaches target angle;Row locating for drive lacking is taken out in formula (1), and formula (4) are substituted into and can be obtained:
Using integration by parts, formula (5) seeks definite integral to time t, can obtain:
It maintains angle constant in each control stage, joint angular speed can be approximately consideredIt is zero, therefore, to formula (6) two
Definite integral is sought at end again, and abbreviation can obtain:
Wherein, q1 1(0) by lever initial angle, q when the expression first stage2(0) and q3(0) the second bar and are respectively indicated
The initial angle of three bars, D1, E1, G1, g1For the middle entry of formula (7) abbreviation process,
D1=(8a3a5-4a2a6-4a1a6-4a4a6)cosq2(0)-a1 2-a2 2- 4a6 2cos2q2(0)-a4 2+4a5 2+4a3 2-
2a1a2-2a1a4-2a2a4
G1=(a1+a2+a4+2a6cosq2(0)-2a5cosq2(0)-2a3)(cosq2-1)+2a5sinq2(0)sinq2
Similarly, it maintains second joint angle constant in the control of second stage, joint angular speed can be approximately considered
It is zero, asks definite integral that the constraint of second stage joint angles can be obtained at formula (6) both ends again.
Step 102, the angle integral constraint relationship based on activation lacking mechanical arm, analyzes its reachable tree, with mechanical arm end
Fitness function of the difference of end and target position as particle swarm algorithm, calculates and drives diarthrodial target angle;
Specifically, the working space of mechanical arm refers to the set for the spatial position that robot arm end effector can reach,
As shown in Fig. 2, mechanical arm working field is denoted as A (p), then the mapping of each joint angles of mechanical arm and work reachable tree can table
It is shown as:
A (p)=p (θ) | θ ∈ Q } (8)
Wherein Q is the constraint space of joint angle, using joint limit angle as the angle restriction of mechanical arm, it may be assumed that
In formula,WithIndicate the lower and upper limit of joint motions, θnIndicate n-th of joint angles.In addition, PA type machine
There are angle restrictions, and joint angle constraint space should also be added between the active and passive joint of tool arm;It is calculated with monte carlo method more
The essence of the working space of articulated mechanical arm be to joint variable by be uniformly distributed be assigned to it is a certain number of meet joints variation
It is required that random quantity, then each joint variable is combined again, and mechanical using the direct kinematics recurrence calculation of mechanical arm
The coordinate value of arm end effector endpoint, the set that these coordinate values are constituted are the reachable tree of mechanical arm tail end;
According to kinematics, as shown in figure 3, plane PAA type activation lacking mechanical arm terminal position cartesian coordinate can indicate
Are as follows:
Wherein (X, Y) is the desired cartesian coordinate in end;The angle of drive rod is sought according to system end point target coordinate
Degree, is substantially exactly the inverse kinematics of system;Numerical method is conjecture and iteration intrinsically, until wrong enough
It is small, or until thinking to abandon;Mechanical arm tail end coordinate and desired location difference are chosen as evaluation function, which can
Keep optimal region of the population constantly into solution space mobile, and then obtains the target angle of drive rod;
The step of particle swarm optimization algorithm, is as follows:
1) initial population for generating m particle at random, initializes speed and the position of each particle, gives inertia weight
ω, Studying factors c1 and c2, pbest, gbest;
2) fitness of each particle is calculated;
3) flight position and speed of each particle are updated according to formula (11), (12);
vi=vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbesti-xi) (11)
xi=xi+vi (12)
4) if the fitness of particle is set as pbest better than original individual extreme value pbest, current fitness, selection is most
Excellent individual extreme value is as group extreme value gbest;
5) judge whether algorithm reaches termination condition.If so, output result;Otherwise the cyclic part from 1 to M is carried out,
Wherein M is maximum number of iterations;
Two are calculated using particle swarm algorithm and drives diarthrodial target angle, are laid the foundation for follow-up location control.
Step 103, it proposes a kind of based on fuzzy and liapunov function planar three freedom activation lacking mechanical arm point
Fragment position control method, in the first stage, design fuzzy controller make second joint reach target angle, and pass through Li Ya
Pu Nuofu function, which acquires third joint control rule, makes it keep original state;In second stage, designs fuzzy controller and make the
Three joints reach target angle, and acquiring second joint control law by liapunov function makes it keep target angle, packet
It includes:
It is as follows that kinetic model is rewritten as affine nonlinear system form first:
Wherein, F, G are the nonlinear function about state variable x,
[G1 G2 G3]T=-M-1(q);According to the control mesh that the definition of liapunov function and mechanical arm tail end are stable in target point
Mark will also make the angle of drive link while the function constructed guarantees that the angle of drive link is finally intended to target angle
Speed finally converges to zero, therefore the third connecting rod liapunov function constructed is as follows:
Wherein, V3(x) it is a positive semidefinite function, derivation is carried out to above formula, and system state equation formula (12) are substituted into
It can obtain:
Wherein aa22For the item of the second row secondary series in G matrix, control law can be acquired according to above formula:
τ3=(- x5+x5(0)-F3-μx5-aa22τ2)/aa22 (16)
Wherein μ is the constant greater than 0, for adjusting convergence rate;x5It (0) is the initial set value of third joint angle.It can
To see, the output torque in third joint changes with the output torque of second joint, and no matter how second joint angle becomes
Change, third joint is stable in the initial angle of setting.Next the Fuzzy Control Law of design second connecting rod, takes joint angles to miss
Difference and error rate are inputted as fuzzy system, and the torque of second joint is subordinate to as output, joint angles and joint angular speed
Category degree function is as shown in Figure 4, Figure 5, carries out anti fuzzy method using gravity model appoach, when second joint angle reaches target position, and angle
When speed is 0, control strategy enters second stage;It is similar with the first stage, the Liapunov of second joint is constructed respectively
Function keeps the initial angle of second joint constant, and design fuzzy controller realizes the control target in third joint;Work as drive rod
Angle is stable in respective target angle respectively, according to the constraint relationship between angle, the related angle for realizing drive lacking connecting rod
Degree control, the final position control for realizing PAA type mechanical arm;
In the case where steady result is not much different, which has bright the method in overshoot and steady state time
Aobvious decline can preferably optimize controlling element, and mechanical arm is made to obtain better dynamic property.
In specific implementation, Manipulator Dynamics parameter and particle swarm algorithm parameter are as shown in the table:
a1 | a2 | a3 | a4 | a5 | a6 | L1 | L2 | L3 |
1.57867 | 2.016 | 1.536 | 0.33333 | 0.4 | 0.6 | 0.8 | 1.2 | 1.0 |
q1(0) | q2(0) | q3(0) | NP | Gen_max | c1 | c2 | ||
0 | 0 | 0 | 15 | 300 | 2 | 1.8 |
Fuzzy reasoning table in fuzzy controller are as follows:
e/de | NB | NM | Z | PM | PB |
NB | PB | PM | PM | PS | Z |
NM | PM | PM | PS | Z | NS |
Z | PM | PS | Z | NS | NM |
PM | PS | Z | NS | NM | NB |
PB | Z | NS | NM | NM | NB |
As shown in fig. 6, mechanical arm initial configuration qinit=[0 0 0];Using base position as coordinate origin, target is chosen
Position is (1, -0.5), and solving corresponding driving joint angles by particle swarm algorithm is [3.0117, -1.7322], and unit is
Radian.Two stage controlling curve and active joint moment curve of output are as shown in Figure 7, Figure 8, in the range of reachable tree,
For arbitrary initial configuration, mentioned control method can efficiently accomplish position control, and terminal position coincidence loss range.The party
Method is realized in finite time to the position control to set the goal, and improves mechanical arm tail end position, overshoot, steady state time
The performance of three aspects;Specifically, the control method is in overshoot and steady state time in the case where steady result is not much different
It significantly decreases, preferably controlling element can be optimized, mechanical arm is made to obtain better dynamic property.
The above content is merely illustrative of the invention's technical idea, and this does not limit the scope of protection of the present invention, all to press
According to technical idea proposed by the present invention, any changes made on the basis of the technical scheme each falls within claims of the present invention
Protection scope within.
Claims (4)
1. a kind of plane activation lacking mechanical arm position control method based on fuzzy control and liapunov function, feature exist
In the method step includes:
(1) active (PAA type) the activation lacking mechanical arm kinetic model of the passive-active-of planar three freedom is established, and analyzes it
Integral constraint relationship between each joint angles, angular speed;
(2) its reachable tree is analyzed, calculates each diarthrodial target angle of drive using particle swarm algorithm;
(3) the segmentation position control law based on fuzzy control and liapunov function is designed, the first stage designs fuzzy control
Device makes second joint reach target angle, and design liapunov function makes third joint keep original state;Second stage is set
Meter fuzzy controller makes third joint reach target angle, and design liapunov function makes second joint keep angle constant,
Realize position control.
2. the method according to claim 1, wherein establishing the active (PAA of the passive-active-of planar three freedom
Type) activation lacking mechanical arm kinetic model, and analyze the integral constraint relationship between its each joint angles, angular speed;
Active (PAA type) the activation lacking mechanical arm of the passive-active-of planar three freedom is not influenced by gravitation, and at passive joint
It is freely rotatable, does not have braking moment and frictional force, research object is reduced to a rigid structure, according to Lagrange
It is as follows to establish its kinetic model for journey:
Wherein, Mij∈R3×3(i, j=1,2,3) specifically,
ak(k=1,2 ..., 6) be system structural parameters, qi(i=1,2,3) is the i-th joint angle of mechanical arm,For the i-th joint angular acceleration of mechanical arm;
miFor the quality (i=1,2,3) of the i-th bar, LiFor the length (i=1,2,3) of the i-th bar, liFor the i-th bar mass center to previous pass
The length (i=1,2,3) of section, JiFor the rotary inertia (i=1,2,3) of the i-th bar;Hi∈R3×1(i=1,2,3) for coriolis force and
Centrifugal force item,
τ=(0, τ2,τ3)TFor joint moment vector, wherein the output torque of passive joint is 0,For mechanical arm
I-th joint angular speed;
Plane PAA type activation lacking mechanical arm is single order nonholonomic system, and plane PA type mechanical arm is holonomic system;It is dropped based on model
Control process is divided into two stages by the thought of rank, maintains third joint initial angle constant in the first stage, and control second is closed
Section reaches target angle;Maintain second joint target angle constant in second stage, control third joint reaches target angle;?
Row locating for drive lacking is taken out in formula (1), and formula (4) are substituted into and can be obtained:
Using integration by parts, formula (5) seeks definite integral to time t, can obtain:
It maintains angle constant in each control stage, joint angular speed can be approximately consideredIt is zero, therefore, again to formula (6) both ends
Secondary to seek definite integral, abbreviation can obtain:
Wherein, q1 1(0) by lever initial angle, q when the expression first stage2(0) and q3(0) the second bar and third bar are respectively indicated
Initial angle, D1,E1,G1,g1For the middle entry of formula (7) abbreviation process,
D1=(8a3a5-4a2a6-4a1a6-4a4a6)cosq2(0)-a1 2-a2 2-4a6 2cos2q2(0)-a4 2+4a5 2+4a3 2-2a1a2-
2a1a4-2a2a4
G1=(a1+a2+a4+2a6cosq2(0)-2a5cosq2(0)-2a3)(cosq2-1)+2a5sinq2(0)sinq2
Similarly, it maintains second joint angle constant in the control of second stage, joint angular speed can be approximately consideredIt is zero,
Ask definite integral that the constraint of second stage joint angles can be obtained at formula (6) both ends again.
3. the method according to claim 1, wherein the angle integral constraint relationship based on activation lacking mechanical arm,
Its reachable tree is analyzed, using the difference of mechanical arm tail end and target position as the fitness function of particle swarm algorithm, is calculated
Drive diarthrodial target angle;
The working space of mechanical arm refers to the set for the spatial position that robot arm end effector can reach, and mechanical arm is worked
Domain is denoted as A (p), then each joint angles of mechanical arm and the mapping of work reachable tree may be expressed as:
A (p)=p (θ) | θ ∈ Q } (8)
Wherein Q is the constraint space of joint angle, using joint limit angle as the angle restriction of mechanical arm, it may be assumed that
In formula,WithIndicate the lower and upper limit of each joint motions, θnIndicate n-th of joint angles;In addition, PA type machine
There are angle restrictions, and joint angle constraint space should also be added between the active and passive joint of tool arm;It is calculated with monte carlo method more
The working space of articulated mechanical arm is assigned to a certain number of random quantitys for meeting joint variation and requiring by being uniformly distributed, then
Each joint variable is combined again, and utilizes the direct kinematics recurrence calculation robot arm end effector endpoint of mechanical arm
Coordinate value, the set that these coordinate values are constituted are the reachable tree of mechanical arm tail end;
Plane PAA type activation lacking mechanical arm terminal position cartesian coordinate can indicate are as follows:
Wherein (X, Y) is the desired cartesian coordinate in end;Mechanical arm tail end coordinate and desired location difference are chosen as evaluation
Function, which can make optimal region of the population constantly into solution space mobile, and then obtain the target angle of drive rod
Degree;
The step of particle swarm optimization algorithm, is as follows:
1) initial population for generating m particle at random, initializes speed and the position of each particle, gives inertia weight ω, learns
Practise factor c1 and c2, pbest, gbest;
2) fitness of each particle is calculated;
3) flight position and speed of each particle are updated according to formula (11), (12);
vi=vi+c1*rand()*(pbesti-xi)+c2*rand()*(gbesti-xi) (11)
xi=xi+vi (12)
4) if the fitness of particle is set as pbest, selects optimal better than original individual extreme value pbest, current fitness
Body extreme value is as group extreme value gbest;
5) judge whether algorithm reaches termination condition, if so, output result;Otherwise the cyclic part from 1 to M is carried out, wherein M
For maximum number of iterations;
Two are calculated using particle swarm algorithm and drives diarthrodial target angle, are laid the foundation for follow-up location control.
4. the method according to claim 1, wherein proposing a kind of based on fuzzy and liapunov function flat
Face three-degree of freedom under-actuated tool arm is segmented position control method, and in the first stage, design fuzzy controller makes second joint
Target angle is reached, and acquiring third joint control rule by liapunov function makes it keep original state;Second
In the stage, design fuzzy controller makes third joint reach target angle, and acquires second joint by liapunov function
Control law makes it keep target angle;
It is as follows that kinetic model is rewritten as affine nonlinear system form first:
Wherein, F, G are the nonlinear function about state variable x, [G1
G2 G3]T=-M-1(q);According to the control target that the definition of liapunov function and mechanical arm tail end are stable in target point,
While the function constructed guarantees that the angle of drive link is finally intended to target angle, also to make the angular speed of drive link
Zero is finally converged to, therefore the third connecting rod liapunov function constructed is as follows:
Wherein, V3(x) it is a positive semidefinite function, derivation is carried out to above formula, and system state equation formula (12) are substituted into and can be obtained:
Wherein a22For the item of the second row secondary series in G matrix, control law can be acquired according to above formula:
τ3=(- x5+x5(0)-F3-μx5-a22τ2)/a22 (16)
Wherein μ is the constant greater than 0, for adjusting convergence rate;x5It (0) is the initial set value of third joint angle;It can see
It arriving, the output torque in third joint changes with the output torque of second joint, and no matter how second joint angle changes, the
Three joints are stable in the initial angle of setting;Next design second connecting rod Fuzzy Control Law, take joint angles error and
Error rate is inputted as fuzzy system, and the torque of second joint carries out anti fuzzy method as output, using gravity model appoach, when the
Two joint angles reach target position, and angular speed be 0 when, control strategy enters second stage;It is similar with the first stage, respectively
The liapunov function for constructing second joint, keeps the initial angle of second joint constant, and design fuzzy controller is realized
The control target in third joint;When drive rod angle is stable in respective target angle respectively, according to the constraint pass between angle
System, the related angle control for realizing drive lacking connecting rod, the final position control for realizing PAA type mechanical arm.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910517174.8A CN110076783B (en) | 2019-06-14 | 2019-06-14 | Planar under-actuated mechanical arm position control method based on fuzzy control and Lyapunov function |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910517174.8A CN110076783B (en) | 2019-06-14 | 2019-06-14 | Planar under-actuated mechanical arm position control method based on fuzzy control and Lyapunov function |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110076783A true CN110076783A (en) | 2019-08-02 |
CN110076783B CN110076783B (en) | 2021-01-19 |
Family
ID=67424262
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910517174.8A Expired - Fee Related CN110076783B (en) | 2019-06-14 | 2019-06-14 | Planar under-actuated mechanical arm position control method based on fuzzy control and Lyapunov function |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110076783B (en) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110434858A (en) * | 2019-09-11 | 2019-11-12 | 青岛大学 | A kind of power of the Multi-arm robots based on command filtering/position mixing control method |
CN110795856A (en) * | 2019-11-04 | 2020-02-14 | 首都师范大学 | Mechanical arm stability formalized analysis method, device, equipment and storage medium |
CN112272806A (en) * | 2019-08-13 | 2021-01-26 | 深圳市大疆创新科技有限公司 | Holder control method and device, holder and storage medium |
CN113296401A (en) * | 2021-05-17 | 2021-08-24 | 哈尔滨工业大学 | Direct obstacle avoidance tracking control method based on switching strategy and storage medium |
CN113378363A (en) * | 2021-05-28 | 2021-09-10 | 浙江工业大学 | Real-time industrial mechanical arm fault identification method |
CN115616922A (en) * | 2022-12-19 | 2023-01-17 | 安徽大学 | Time coverage control method for heterogeneous mobile robot cluster |
CN116512286A (en) * | 2023-04-23 | 2023-08-01 | 九众九机器人有限公司 | Six-degree-of-freedom stamping robot and stamping method thereof |
CN117047782A (en) * | 2023-10-11 | 2023-11-14 | 中建四局安装工程有限公司 | Control method and device suitable for three-joint manipulator, terminal and medium |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2008197848A (en) * | 2007-02-09 | 2008-08-28 | Kakei Gakuen | Fuzzy controller, lane travel support device and steering auxiliary device |
CN106584465A (en) * | 2017-01-22 | 2017-04-26 | 北京工业大学 | Position and posture control method for planar 4R under-actuation mechanical arm |
CN108972560A (en) * | 2018-08-23 | 2018-12-11 | 北京邮电大学 | A kind of activation lacking mechanical arm Hierarchical sliding mode control method based on fuzzy optimization |
CN109262612A (en) * | 2018-10-09 | 2019-01-25 | 北京邮电大学 | A kind of activation lacking mechanical shoulder joint angle optimization method based on improvement particle swarm algorithm |
CN109298710A (en) * | 2018-09-06 | 2019-02-01 | 大连理工大学 | Double-wheel self-balancing car owner based on human-computer interaction is dynamic to follow composite control method |
-
2019
- 2019-06-14 CN CN201910517174.8A patent/CN110076783B/en not_active Expired - Fee Related
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2008197848A (en) * | 2007-02-09 | 2008-08-28 | Kakei Gakuen | Fuzzy controller, lane travel support device and steering auxiliary device |
CN106584465A (en) * | 2017-01-22 | 2017-04-26 | 北京工业大学 | Position and posture control method for planar 4R under-actuation mechanical arm |
CN108972560A (en) * | 2018-08-23 | 2018-12-11 | 北京邮电大学 | A kind of activation lacking mechanical arm Hierarchical sliding mode control method based on fuzzy optimization |
CN109298710A (en) * | 2018-09-06 | 2019-02-01 | 大连理工大学 | Double-wheel self-balancing car owner based on human-computer interaction is dynamic to follow composite control method |
CN109262612A (en) * | 2018-10-09 | 2019-01-25 | 北京邮电大学 | A kind of activation lacking mechanical shoulder joint angle optimization method based on improvement particle swarm algorithm |
Non-Patent Citations (1)
Title |
---|
X. GAO,ETC: "Two-Stage Switching Hybrid Control Method Based on Improved PSO for Planar Three-Link Under-Actuated Manipulator", 《IEEE ACCESS》 * |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112272806A (en) * | 2019-08-13 | 2021-01-26 | 深圳市大疆创新科技有限公司 | Holder control method and device, holder and storage medium |
CN110434858A (en) * | 2019-09-11 | 2019-11-12 | 青岛大学 | A kind of power of the Multi-arm robots based on command filtering/position mixing control method |
CN110434858B (en) * | 2019-09-11 | 2020-11-17 | 青岛大学 | Force/position hybrid control method of multi-mechanical-arm system based on command filtering |
CN110795856B (en) * | 2019-11-04 | 2023-04-14 | 首都师范大学 | Mechanical arm stability formalized analysis method, device, equipment and storage medium |
CN110795856A (en) * | 2019-11-04 | 2020-02-14 | 首都师范大学 | Mechanical arm stability formalized analysis method, device, equipment and storage medium |
CN113296401A (en) * | 2021-05-17 | 2021-08-24 | 哈尔滨工业大学 | Direct obstacle avoidance tracking control method based on switching strategy and storage medium |
CN113296401B (en) * | 2021-05-17 | 2022-04-19 | 哈尔滨工业大学 | Direct obstacle avoidance tracking control method based on switching strategy and storage medium |
CN113378363A (en) * | 2021-05-28 | 2021-09-10 | 浙江工业大学 | Real-time industrial mechanical arm fault identification method |
CN115616922A (en) * | 2022-12-19 | 2023-01-17 | 安徽大学 | Time coverage control method for heterogeneous mobile robot cluster |
CN115616922B (en) * | 2022-12-19 | 2023-03-21 | 安徽大学 | Time coverage control method for heterogeneous mobile robot cluster |
CN116512286A (en) * | 2023-04-23 | 2023-08-01 | 九众九机器人有限公司 | Six-degree-of-freedom stamping robot and stamping method thereof |
CN116512286B (en) * | 2023-04-23 | 2023-11-14 | 九众九机器人有限公司 | Six-degree-of-freedom stamping robot and stamping method thereof |
CN117047782A (en) * | 2023-10-11 | 2023-11-14 | 中建四局安装工程有限公司 | Control method and device suitable for three-joint manipulator, terminal and medium |
CN117047782B (en) * | 2023-10-11 | 2023-12-08 | 中建四局安装工程有限公司 | Control method and device suitable for three-joint manipulator, terminal and medium |
Also Published As
Publication number | Publication date |
---|---|
CN110076783B (en) | 2021-01-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110076783A (en) | A kind of plane activation lacking mechanical arm position control method based on fuzzy control and liapunov function | |
CN110275436B (en) | RBF neural network self-adaptive control method of multi-single-arm manipulator | |
Guez et al. | Solution to the inverse kinematics problem in robotics by neural networks | |
US8825207B2 (en) | Trajectory planning method, trajectory planning system and trajectory planning and control system | |
CN110658821B (en) | Multi-robot anti-interference grouping time-varying formation control method and system | |
Veysi et al. | A novel self-adaptive modified bat fuzzy sliding mode control of robot manipulator in presence of uncertainties in task space | |
CN106406097B (en) | The distributed self-adaption control method for coordinating of Multi-arm robots | |
CN115157238B (en) | Multi-degree-of-freedom robot dynamics modeling and track tracking method | |
CN108393886B (en) | Distributed multi-mobile manipulator cooperative carrying method capable of optimizing energy and operation degree | |
CN111522341A (en) | Multi-time-varying formation tracking control method and system for network heterogeneous robot system | |
Mao et al. | A hybrid differential evolution and particle swarm optimization algorithm for numerical kinematics solution of remote maintenance manipulators | |
Li et al. | An improved artificial potential field method for solving local minimum problem | |
CN108845493A (en) | The set time tracking and controlling method of mechanical arm system with output constraint | |
CN112936286B (en) | Self-adaptive consistency tracking control method and system for multi-flexible mechanical arm system | |
Pambudi et al. | Simulation design of trajectory planning robot manipulator | |
Zebin et al. | Modeling and Control of a Two-link Flexible Manipulator using Fuzzy Logic and Genetic Optimization Techniques. | |
Huang et al. | PSO-based time-optimal trajectory planning for space robot with dynamic constraints | |
Su et al. | Robot path planning based on random coding particle swarm optimization | |
Du et al. | Application of IPSO algorithm to inverse kinematics solution of reconfigurable modular robots | |
Zebin et al. | Dynamic modeling and fuzzy logic control of a two-link flexible manipulator using genetic optimization techniques | |
CN113219825A (en) | Single-leg track tracking control method and system for quadruped robot | |
Yunqiang et al. | Research on multi-objective path planning of a robot based on artificial potential field method | |
Duong | Dynamic modeling and control of a flexible link manipulators with translational and rotational joints | |
Mehdi et al. | Position/force control optimized by particle swarm intelligence for constrained robotic manipulators | |
Nagata et al. | Neural network-based inverse kinematics for an industrial robot and its learning method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210119 Termination date: 20210614 |